From 07c48a5eca7f4f303ee927879b5f12484101aedd Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 29 May 2023 16:16:20 +0900 Subject: [PATCH 001/118] Update FaultInjection to prepare Node and Publisher in the parse phase Signed-off-by: yamacir-kit --- .../src/syntax/custom_command_action.cpp | 25 +++++++++++++------ 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp b/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp index 67ae64287ba..f65e6fe8a93 100644 --- a/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp @@ -30,21 +30,32 @@ inline namespace syntax template struct ApplyFaultInjectionAction : public CustomCommand { - using CustomCommand::CustomCommand; - - static auto node() -> rclcpp::Node & + static auto node() -> auto & { - static rclcpp::Node node{"custom_command_action", "simulation"}; + static auto node = []() { + auto options = rclcpp::NodeOptions(); + options.use_global_arguments(false); + return rclcpp::Node( + "custom_command_action_" + boost::lexical_cast(std::this_thread::get_id()), + "simulation", options); + }(); return node; } - static auto publisher() -> rclcpp::Publisher & + static auto publisher() -> auto & { - static auto publisher = node().create_publisher( - "/simulation/events", rclcpp::QoS(1).reliable()); + static auto publisher = + node().template create_publisher( + "events", rclcpp::QoS(1).reliable()); return *publisher; } + template + explicit ApplyFaultInjectionAction(Ts &&... xs) : CustomCommand(std::forward(xs)...) + { + publisher(); + } + auto start(const Scope &) -> void override { static_assert(0 < Version and Version <= 2); From 3dfb24057eec7c711e56350db39e314c6ea623b0 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 29 May 2023 16:21:58 +0900 Subject: [PATCH 002/118] Lipsticks Signed-off-by: yamacir-kit --- .../src/syntax/custom_command_action.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp b/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp index f65e6fe8a93..d76f12f3c1e 100644 --- a/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp @@ -260,10 +260,9 @@ struct ForkExecCommand : public CustomCommand auto makeCustomCommand(const std::string & type, const std::string & content) -> std::shared_ptr { -#define ELEMENT(NAME, TYPE) \ - std::make_pair(NAME, [](auto &&... xs) { \ - return std::shared_ptr(new TYPE(std::forward(xs)...)); \ - }) +#define ELEMENT(NAME, TYPE) \ + std::make_pair( \ + NAME, [](auto &&... xs) { return std::make_shared(std::forward(xs)...); }) static const std::unordered_map< std::string, std::function(const std::vector &)>> From 1e65bed0d61b74dd23006ac2abc83e1538f23ed1 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 29 May 2023 17:32:17 +0900 Subject: [PATCH 003/118] Add `ConfiguringPerceptionTopics` page to documentation Signed-off-by: yamacir-kit --- mkdocs.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/mkdocs.yml b/mkdocs.yml index c5cdf94e481..804cb493300 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -75,6 +75,7 @@ nav: - developer_guide/AutowareAPI.md - developer_guide/NPCBehavior.md - developer_guide/BehaviorPlugin.md + - developer_guide/ConfiguringPerceptionTopics.md # - developer_guide/ErrorCategories.md - developer_guide/OpenSCENARIOSupport.md - developer_guide/SimpleSensorSimulator.md From 88e1cb3eec3217af62361d74c58cf7570c8fd38a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= Date: Thu, 22 Jun 2023 11:52:49 +0200 Subject: [PATCH 004/118] fix(action): fix fault injection - history_depth in qos --- .../src/syntax/custom_command_action.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp b/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp index 75fec401270..63be11934f1 100644 --- a/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include // std::distance #include #include #include @@ -21,6 +20,8 @@ #include #include #include + +#include // std::distance #include namespace openscenario_interpreter @@ -46,7 +47,7 @@ struct ApplyFaultInjectionAction : public CustomCommand { static auto publisher = node().template create_publisher( - "events", rclcpp::QoS(1).reliable()); + "events", rclcpp::QoS(rclcpp::KeepLast(10)).reliable()); return *publisher; } From e36b3b83bc86b4904e9d284ebf03e7aa13201839 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= Date: Thu, 22 Jun 2023 11:57:50 +0200 Subject: [PATCH 005/118] ref(action): apply clang_format --- .../src/syntax/custom_command_action.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp b/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp index 63be11934f1..8d1e13e6056 100644 --- a/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include // std::distance #include #include #include @@ -20,8 +21,6 @@ #include #include #include - -#include // std::distance #include namespace openscenario_interpreter From 82a08815f7fa557e9dfed3511f0889d21fd51ece Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 10 Apr 2024 11:20:55 +0200 Subject: [PATCH 006/118] floating_point comparison --- common/math/arithmetic/CMakeLists.txt | 8 + common/math/arithmetic/test/CMakeLists.txt | 1 + .../test/src/floating_point/CMakeLists.txt | 1 + .../src/floating_point/test_comparison.cpp | 158 ++++++++++++++++++ 4 files changed, 168 insertions(+) create mode 100644 common/math/arithmetic/test/CMakeLists.txt create mode 100644 common/math/arithmetic/test/src/floating_point/CMakeLists.txt create mode 100644 common/math/arithmetic/test/src/floating_point/test_comparison.cpp diff --git a/common/math/arithmetic/CMakeLists.txt b/common/math/arithmetic/CMakeLists.txt index a7983644b76..fbc34158cf1 100644 --- a/common/math/arithmetic/CMakeLists.txt +++ b/common/math/arithmetic/CMakeLists.txt @@ -18,4 +18,12 @@ install( DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ DESTINATION include/) +if(BUILD_TESTING) + include_directories(include) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) +endif() + ament_auto_package() diff --git a/common/math/arithmetic/test/CMakeLists.txt b/common/math/arithmetic/test/CMakeLists.txt new file mode 100644 index 00000000000..8bc9826fbbc --- /dev/null +++ b/common/math/arithmetic/test/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(src/floating_point) diff --git a/common/math/arithmetic/test/src/floating_point/CMakeLists.txt b/common/math/arithmetic/test/src/floating_point/CMakeLists.txt new file mode 100644 index 00000000000..18b78b072c6 --- /dev/null +++ b/common/math/arithmetic/test/src/floating_point/CMakeLists.txt @@ -0,0 +1 @@ +ament_add_gtest(test_comparison test_comparison.cpp) \ No newline at end of file diff --git a/common/math/arithmetic/test/src/floating_point/test_comparison.cpp b/common/math/arithmetic/test/src/floating_point/test_comparison.cpp new file mode 100644 index 00000000000..951b03a7687 --- /dev/null +++ b/common/math/arithmetic/test/src/floating_point/test_comparison.cpp @@ -0,0 +1,158 @@ +#include + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST(Comparison, isApproximatelyEqualTo_equal) +{ + const float a = 0.1; + const float b = 0.10000000000000001; + + EXPECT_TRUE(math::arithmetic::isApproximatelyEqualTo(a, b)); + + const double af = -1000.1; + const double bf = -1000.10000000000000001; + + EXPECT_TRUE(math::arithmetic::isApproximatelyEqualTo(af, bf)); +} + +TEST(Comparison, isApproximatelyEqualTo_notEqual) +{ + const float a = 0.1; + const float b = 0.11; + + EXPECT_FALSE(math::arithmetic::isApproximatelyEqualTo(a, b)); + + const double af = -1000.1; + const double bf = -1000.11; + + EXPECT_FALSE(math::arithmetic::isApproximatelyEqualTo(af, bf)); +} + +TEST(Comparison, isEssentiallyEqualTo_equal) +{ + const float a = 0.1; + const float b = 0.10000000000000001; + + EXPECT_TRUE(math::arithmetic::isApproximatelyEqualTo(a, b)); + + const double af = -1000.1; + const double bf = -1000.10000000000000001; + + EXPECT_TRUE(math::arithmetic::isApproximatelyEqualTo(af, bf)); +} + +TEST(Comparison, isEssentiallyEqualTo_notEqual) +{ + const float a = 0.1; + const float b = 0.1000001; + + EXPECT_FALSE(math::arithmetic::isEssentiallyEqualTo(a, b)); + + const double af = -1000.1; + const double bf = -1000.1000001; + + EXPECT_FALSE(math::arithmetic::isEssentiallyEqualTo(af, bf)); +} + +TEST(Comparison, isDefinitelyLessThan_lessTwo) +{ + const float a = 0.1; + const float b = 0.2; + + EXPECT_TRUE(math::arithmetic::isDefinitelyLessThan(a, b)); + + const double af = -1.2; + const double bf = -1.1; + + EXPECT_TRUE(math::arithmetic::isDefinitelyLessThan(af, bf)); +} + +TEST(Comparison, isDefinitelyLessThan_lessThree) +{ + const float a = 0.1; + const float b = 0.2; + const float c = 0.3; + + EXPECT_TRUE(math::arithmetic::isDefinitelyLessThan(a, b, c)); + + const double af = -1.3; + const double bf = -1.2; + const double cf = -1.1; + + EXPECT_TRUE(math::arithmetic::isDefinitelyLessThan(af, bf, cf)); +} + +TEST(Comparison, isDefinitelyLessThan_moreTwo) +{ + const float a = 0.2; + const float b = 0.1; + + EXPECT_FALSE(math::arithmetic::isDefinitelyLessThan(a, b)); + + const double af = -1.1; + const double bf = -1.2; + + EXPECT_FALSE(math::arithmetic::isDefinitelyLessThan(af, bf)); +} + +TEST(Comparison, isDefinitelyLessThan_moreThree) +{ + const float a = 0.3; + const float b = 0.2; + const float c = 0.1; + + EXPECT_FALSE(math::arithmetic::isDefinitelyLessThan(a, b, c)); + + const double af = -1.1; + const double bf = -1.2; + const double cf = -1.3; + + EXPECT_FALSE(math::arithmetic::isDefinitelyLessThan(af, bf, cf)); +} + +TEST(Comparison, isDefinitelyLessThan_partlyMoreThree) +{ + const float a = 0.1; + const float b = 0.2; + const float c = 0.1; + + EXPECT_FALSE(math::arithmetic::isDefinitelyLessThan(a, b, c)); + + const double af = -1.1; + const double bf = -1.2; + const double cf = -1.1; + + EXPECT_FALSE(math::arithmetic::isDefinitelyLessThan(af, bf, cf)); +} + +TEST(Comparison, isDefinitelyGreaterThan_greater) +{ + const float a = 0.2; + const float b = 0.1; + + EXPECT_TRUE(math::arithmetic::isDefinitelyGreaterThan(a, b)); + + const double af = -1.1; + const double bf = -1.2; + + EXPECT_TRUE(math::arithmetic::isDefinitelyGreaterThan(af, bf)); +} + +TEST(Comparison, isDefinitelyGreaterThan_notGreater) +{ + const float a = 0.1; + const float b = 0.2; + + EXPECT_FALSE(math::arithmetic::isDefinitelyGreaterThan(a, b)); + + const double af = -1.2; + const double bf = -1.1; + + EXPECT_FALSE(math::arithmetic::isDefinitelyGreaterThan(af, bf)); +} \ No newline at end of file From 6a355d38ac655eb087c99c3d415112cc46c49d62 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 10 Apr 2024 11:22:43 +0200 Subject: [PATCH 007/118] simple_sensr_simulation unit tests --- .../simple_sensor_simulator/CMakeLists.txt | 2 + .../test/CMakeLists.txt | 6 + .../test/src/sensor_simulation/CMakeLists.txt | 2 + .../sensor_simulation/lidar/CMakeLists.txt | 5 + .../lidar/test_lidar_sensor.cpp | 24 + .../lidar/test_raycaster.cpp | 61 +++ .../occupancy_grid/CMakeLists.txt | 8 + .../occupancy_grid/test_grid_traversal.cpp | 65 +++ .../test_occupancy_grid_builder.cpp | 81 ++++ .../test_occupancy_grid_sensor.cpp | 9 + .../primitives/CMakeLists.txt | 8 + .../sensor_simulation/primitives/test_box.cpp | 60 +++ .../primitives/test_primitive.cpp | 456 ++++++++++++++++++ .../primitives/test_vertex.cpp | 86 ++++ .../test_sensor_simulation.cpp | 55 +++ .../vehicle_model/CMakeLists.txt | 11 + .../test_sim_model_ideal_steer_acc.cpp | 91 ++++ .../test_sim_model_ideal_steer_acc_geared.cpp | 91 ++++ .../test_sim_model_ideal_steer_vel.cpp | 87 ++++ .../test_sim_model_interface.cpp | 9 + 20 files changed, 1217 insertions(+) create mode 100644 simulation/simple_sensor_simulator/test/CMakeLists.txt create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/CMakeLists.txt create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_grid_traversal.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/CMakeLists.txt create mode 100644 simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc_geared.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_vel.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_interface.cpp diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt index 8e784fecd3a..ee20b432ae4 100644 --- a/simulation/simple_sensor_simulator/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/CMakeLists.txt @@ -86,6 +86,8 @@ EXPORT export_simple_sensor_simulator_component if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) endif() ament_auto_package() diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt new file mode 100644 index 00000000000..5d184534ffa --- /dev/null +++ b/simulation/simple_sensor_simulator/test/CMakeLists.txt @@ -0,0 +1,6 @@ +add_subdirectory(src/sensor_simulation/primitives) +add_subdirectory(src/sensor_simulation/occupancy_grid) +add_subdirectory(src/sensor_simulation/lidar) +add_subdirectory(src/sensor_simulation) + +add_subdirectory(src/vehicle_simulation/vehicle_model) \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/CMakeLists.txt new file mode 100644 index 00000000000..5508fb23d39 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_sensor_simulation test_sensor_simulation.cpp) +target_link_libraries(test_sensor_simulation simple_sensor_simulator_component) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt new file mode 100644 index 00000000000..53bb19d7837 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt @@ -0,0 +1,5 @@ +ament_add_gtest(test_lidar_sensor test_lidar_sensor.cpp) +target_link_libraries(test_lidar_sensor simple_sensor_simulator_component) + +ament_add_gtest(test_raycaster test_raycaster.cpp) +target_link_libraries(test_raycaster simple_sensor_simulator_component) \ No newline at end of file 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 new file mode 100644 index 00000000000..d4fe5a7ceb6 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp @@ -0,0 +1,24 @@ +#include + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST(LidarSensor, update_noEgo) +{ + const double current_simulation_time{}; + const simulation_api_schema::LidarConfiguration configuration{}; + const rclcpp::Publisher::SharedPtr publisher_ptr{}; + + auto sensor = simple_sensor_simulator::LidarSensor( + current_simulation_time, configuration, publisher_ptr); + std::vector status{}; + const rclcpp::Time current_ros_time{}; + + EXPECT_THROW( + sensor.update(current_simulation_time + 1, status, current_ros_time), std::runtime_error); +} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.cpp new file mode 100644 index 00000000000..97b6a6cd186 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.cpp @@ -0,0 +1,61 @@ +#include + +#include +#include +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST(Raycaster, addPrimitive_box) +{ + auto rc = simple_sensor_simulator::Raycaster(); + + const std::string type("name"); + geometry_msgs::msg::Pose pose; + pose.position.x = 9.0; + pose.position.y = 11.0; + pose.position.z = 17.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + + const float depth = 19.0; + const float width = 23.0; + const float height = 29.0; + simple_sensor_simulator::primitives::Box box(depth, width, height, pose); + + EXPECT_NO_THROW( + rc.addPrimitive(std::string("box"), box)); +} + +TEST(Raycaster, addPrimitive_twoIdenticalNames) +{ + auto rc = simple_sensor_simulator::Raycaster(); + + const std::string type("name"); + geometry_msgs::msg::Pose pose; + pose.position.x = 9.0; + pose.position.y = 11.0; + pose.position.z = 17.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + + const float depth = 19.0; + const float width = 23.0; + const float height = 29.0; + simple_sensor_simulator::primitives::Box box_0(depth, width, height, pose); + simple_sensor_simulator::primitives::Box box_1(depth, width, height, pose); + + EXPECT_NO_THROW( + rc.addPrimitive(std::string("box"), box_0)); + EXPECT_THROW( + rc.addPrimitive(std::string("box"), box_1), + std::runtime_error); +} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt new file mode 100644 index 00000000000..95b15765cbb --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt @@ -0,0 +1,8 @@ +ament_add_gtest(test_grid_traversal test_grid_traversal.cpp) +target_link_libraries(test_grid_traversal simple_sensor_simulator_component) + +ament_add_gtest(test_occupancy_grid_builder test_occupancy_grid_builder.cpp) +target_link_libraries(test_occupancy_grid_builder simple_sensor_simulator_component) + +ament_add_gtest(test_occupancy_grid_sensor test_occupancy_grid_sensor.cpp) +target_link_libraries(test_occupancy_grid_sensor simple_sensor_simulator_component) \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_grid_traversal.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_grid_traversal.cpp new file mode 100644 index 00000000000..24bb2908239 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_grid_traversal.cpp @@ -0,0 +1,65 @@ +#include + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST(GridTraversal, begin) +{ + double start_x = 2.2; + double start_y = 3.3; + double end_x = 5.5; + double end_y = 7.7; + auto traversal = simple_sensor_simulator::GridTraversal(start_x, start_y, end_x, end_y); + + auto it = traversal.begin(); + + EXPECT_TRUE((*it).first == static_cast(start_x)); + EXPECT_TRUE((*it).second == static_cast(start_y)); +} + +TEST(GridTraversal, operatorIncrement) +{ + double start_x = 2.2; + double start_y = 3.3; + double end_x = 5.5; + double end_y = 7.7; + auto traversal = simple_sensor_simulator::GridTraversal(start_x, start_y, end_x, end_y); + + auto it = traversal.begin(); + + ++it; + EXPECT_TRUE((*it).first == static_cast(start_x)); + EXPECT_TRUE((*it).second == static_cast(start_y) + 1); + ++it; + EXPECT_TRUE((*it).first == static_cast(start_x) + 1); + EXPECT_TRUE((*it).second == static_cast(start_y) + 1); +} + +TEST(GridTraversal, operatorNotEqual) +{ + double start_x = 0.0; + double start_y = 0.0; + double end_x = 10.5; + double end_y = 0.0; + auto traversal = simple_sensor_simulator::GridTraversal(start_x, start_y, end_x, end_y); + + auto it = traversal.begin(); + + int curr = 0; + for (int i = 0; i <= end_x; i++) { + EXPECT_TRUE(it != traversal.end()); + EXPECT_TRUE(curr == (*it).first); + EXPECT_TRUE(0 == (*it).second); + ++it; + curr++; + } + + EXPECT_FALSE(it != traversal.end()); + EXPECT_TRUE(std::ceil(end_x) == (*it).first); + EXPECT_TRUE(std::ceil(end_x) == curr); +} diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp new file mode 100644 index 00000000000..9db93e12275 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp @@ -0,0 +1,81 @@ +#include + +#include +#include + +auto getBox() -> simple_sensor_simulator::primitives::Box +{ + const std::string type("box"); + geometry_msgs::msg::Pose pose; + pose.position.x = 9.0; + pose.position.y = 11.0; + pose.position.z = 17.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + + const float depth = 19.0; + const float width = 23.0; + const float height = 29.0; + simple_sensor_simulator::primitives::Box prim(depth, width, height, pose); + return prim; +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST(OccupancyGridBuilder, add_overLimit) +{ + const double resolution = 1000.0f; + const size_t height = 11; + const size_t width = 13; + const int8_t occupied_cost = 17; + const int8_t invisible_cost = 19; + auto builder = simple_sensor_simulator::OccupancyGridBuilder( + resolution, height, width, occupied_cost, invisible_cost); + auto count_max = std::numeric_limits::max(); + for (int i = 0; i < count_max; i++) { + EXPECT_NO_THROW(builder.add(getBox())); + } + EXPECT_THROW(builder.add(getBox()), std::runtime_error); +} + +TEST(OccupancyGridBuilder, reset) +{ + const double resolution = 1000.0f; + const size_t height = 11; + const size_t width = 13; + const int8_t occupied_cost = 17; + const int8_t invisible_cost = 19; + auto builder = simple_sensor_simulator::OccupancyGridBuilder( + resolution, height, width, occupied_cost, invisible_cost); + auto count_max = std::numeric_limits::max(); + for (int i = 0; i < count_max; i++) { + EXPECT_NO_THROW(builder.add(getBox())); + } + builder.reset(geometry_msgs::msg::Pose{}); + for (int i = 0; i < count_max; i++) { + EXPECT_NO_THROW(builder.add(getBox())); + } + EXPECT_THROW(builder.add(getBox()), std::runtime_error); +} + +TEST(OccupancyGridBuilder, build_empty) +{ + const double resolution = 1000.0f; + const size_t height = 11; + const size_t width = 13; + const int8_t occupied_cost = 17; + const int8_t invisible_cost = 19; + auto builder = simple_sensor_simulator::OccupancyGridBuilder( + resolution, height, width, occupied_cost, invisible_cost); + + builder.build(); + auto grid = builder.get(); + + EXPECT_TRUE(std::all_of(grid.begin(), grid.end(), [&](int elem) { return 0 == elem; })); +} diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp new file mode 100644 index 00000000000..79366a900f2 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp @@ -0,0 +1,9 @@ +#include + +#include + +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/primitives/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt new file mode 100644 index 00000000000..ec4943146b1 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt @@ -0,0 +1,8 @@ +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) + +ament_add_gtest(test_box test_box.cpp) +target_link_libraries(test_box simple_sensor_simulator_component) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp new file mode 100644 index 00000000000..9959173b775 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp @@ -0,0 +1,60 @@ +#include + +#include +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST(Box, Box) +{ + const std::string type("name"); + geometry_msgs::msg::Pose pose; + pose.position.x = 9.0; + pose.position.y = 11.0; + pose.position.z = 17.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + + const float depth = 19.0; + const float width = 23.0; + const float height = 29.0; + simple_sensor_simulator::primitives::Box prim(depth, width, height, pose); + + const std::vector hull = prim.get2DConvexHull(); + auto p_min_x = std::min_element( + hull.begin(), hull.end(), + [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) { + return a.x < b.x; + }); + auto p_min_y = std::min_element( + hull.begin(), hull.end(), + [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) { + return a.y < b.y; + }); + auto p_max_x = std::min_element( + hull.begin(), hull.end(), + [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) { + return a.x > b.x; + }); + auto p_max_y = std::min_element( + hull.begin(), hull.end(), + [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) { + return a.y > b.y; + }); + + EXPECT_FALSE(p_min_x == hull.end()); + EXPECT_FALSE(p_min_y == hull.end()); + EXPECT_FALSE(p_max_x == hull.end()); + EXPECT_FALSE(p_max_y == hull.end()); + + EXPECT_TRUE(p_min_x->x == pose.position.x - depth / 2); + EXPECT_TRUE(p_min_y->y == pose.position.y - width / 2); + EXPECT_TRUE(p_max_x->x == pose.position.x + depth / 2); + EXPECT_TRUE(p_max_y->y == pose.position.y + width / 2); +} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.cpp new file mode 100644 index 00000000000..e17ec492460 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.cpp @@ -0,0 +1,456 @@ +#include + +#include +#include + +class JaggedSquare : public simple_sensor_simulator::primitives::Primitive +{ +public: + explicit JaggedSquare( + float depth, float width, float height, const geometry_msgs::msg::Pose & pose) + : simple_sensor_simulator::primitives::Primitive("Box", pose), + depth(depth), + width(width), + height(height) + { + vertices_ = std::vector(8); + + vertices_[0].x = -0.5 * depth; + vertices_[0].y = -0.5 * width; + vertices_[0].z = 0.0 * height; + + vertices_[1].x = 0.0 * depth; + vertices_[1].y = -0.5 * width; + vertices_[1].z = 0.0 * height; + + vertices_[2].x = 0.5 * depth; + vertices_[2].y = -0.5 * width; + vertices_[2].z = 0.0 * height; + + vertices_[3].x = -0.5 * depth; + vertices_[3].y = 0.0 * width; + vertices_[3].z = 0.0 * height; + + vertices_[4].x = 0.0 * depth; + vertices_[4].y = 0.0 * width; + vertices_[4].z = 0.0 * height; + + vertices_[5].x = 0.5 * depth; + vertices_[5].y = 0.0 * width; + vertices_[5].z = 0.0 * height; + + vertices_[6].x = -0.5 * depth; + vertices_[6].y = 0.5 * width; + vertices_[6].z = 0.0 * height; + + vertices_[7].x = 0.0 * depth; + vertices_[7].y = 0.5 * width; + vertices_[7].z = 0.0 * height; + + triangles_ = std::vector(6); + + triangles_[0].v0 = 0; + triangles_[0].v1 = 1; + triangles_[0].v2 = 4; + + triangles_[1].v0 = 1; + triangles_[1].v1 = 4; + triangles_[1].v2 = 3; + + triangles_[2].v0 = 1; + triangles_[2].v1 = 2; + triangles_[2].v2 = 5; + + triangles_[3].v0 = 1; + triangles_[3].v1 = 5; + triangles_[3].v2 = 4; + + triangles_[4].v0 = 3; + triangles_[4].v1 = 4; + triangles_[4].v2 = 7; + + triangles_[5].v0 = 3; + triangles_[5].v1 = 7; + triangles_[5].v2 = 6; + } + std::vector getVertices(geometry_msgs::msg::Pose pose) + { + return transform(pose); + } + ~JaggedSquare() = default; + const float depth; + const float width; + const float height; +}; + +bool is2DSubset( + const std::vector & superset, + const std::vector & subset) +{ + for (const simple_sensor_simulator::Vertex & v_subset : subset) { + auto it = std::find_if( + superset.begin(), superset.end(), [&](const simple_sensor_simulator::Vertex & v_superset) { + return v_superset.x == v_subset.x && v_superset.y == v_subset.y; + }); + if (it == superset.end()) { + return false; + } + } + return true; +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST(Primitive, get2DConvexHull_normal) +{ + const std::string type("name"); + geometry_msgs::msg::Pose pose; + pose.position.x = 9.0; + pose.position.y = 11.0; + pose.position.z = 17.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + + const float depth = 19.0; + const float width = 23.0; + const float height = 29.0; + JaggedSquare prim(depth, width, height, pose); + + std::vector hull = + simple_sensor_simulator::toVertex(prim.get2DConvexHull()); + std::vector hull_superset = prim.getVertex(); + hull_superset.erase(hull_superset.begin() + 4); + + std::vector hull_subset = prim.getVertex(); + hull_subset.erase(hull_subset.begin() + 4); + hull_subset.erase(hull_subset.begin() + 3); + hull_subset.erase(hull_subset.begin() + 1); + + EXPECT_TRUE(hull.size() >= 5); + EXPECT_TRUE(hull_superset.size() == 7); + EXPECT_TRUE(hull_subset.size() == 5); + EXPECT_TRUE(is2DSubset(hull_superset, hull)); + EXPECT_TRUE(is2DSubset(hull, hull_subset)); +} + +TEST(Primitive, get2DConvexHull_withTransform) +{ + const std::string type("name"); + geometry_msgs::msg::Pose pose; + pose.position.x = 9.0; + pose.position.y = 11.0; + pose.position.z = 17.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + + const float depth = 19.0; + const float width = 23.0; + const float height = 29.0; + JaggedSquare prim(depth, width, height, pose); + + geometry_msgs::msg::Pose sensor_pose{}; + sensor_pose.position.x = 11.0; + sensor_pose.position.y = 13.0; + sensor_pose.position.z = 17.0; + sensor_pose.orientation.x = 0.944; + sensor_pose.orientation.y = 0.187; + sensor_pose.orientation.z = 0.187; + sensor_pose.orientation.w = 0.199; + + std::vector hull = + simple_sensor_simulator::toVertex(prim.get2DConvexHull(sensor_pose)); + std::vector hull_superset = prim.getVertices(sensor_pose); + std::vector hull_subset = prim.getVertices(sensor_pose); + + hull_superset.erase(hull_superset.begin() + 4); + + hull_subset.erase(hull_subset.begin() + 4); + hull_subset.erase(hull_subset.begin() + 3); + hull_subset.erase(hull_subset.begin() + 1); + + EXPECT_TRUE(hull.size() >= 5); + EXPECT_TRUE(hull_superset.size() == 7); + EXPECT_TRUE(hull_subset.size() == 5); + EXPECT_TRUE(is2DSubset(hull_superset, hull)); + EXPECT_TRUE(is2DSubset(hull, hull_subset)); +} + +TEST(Primitive, getMin_noTransform) +{ + const std::string type("name"); + geometry_msgs::msg::Pose pose; + pose.position.x = 9.0; + pose.position.y = 11.0; + pose.position.z = 17.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + + const float depth = 19.0; + const float width = 23.0; + const float height = 29.0; + simple_sensor_simulator::primitives::Box prim(depth, width, height, pose); + + const auto axis_x = math::geometry::Axis::X; + const auto axis_y = math::geometry::Axis::Y; + const auto axis_z = math::geometry::Axis::Z; + + const std::optional min_x = prim.getMin(axis_x); + const std::optional min_y = prim.getMin(axis_y); + const std::optional min_z = prim.getMin(axis_z); + + EXPECT_TRUE(min_x.has_value()); + EXPECT_TRUE(min_y.has_value()); + EXPECT_TRUE(min_z.has_value()); + EXPECT_TRUE(min_x.value() == pose.position.x - depth / 2); + EXPECT_TRUE(min_y.value() == pose.position.y - width / 2); + EXPECT_TRUE(min_z.value() == pose.position.z - height / 2); +} + +TEST(Primitive, getMin_emptyPrimitive) +{ + const std::string type("name"); + geometry_msgs::msg::Pose pose; + pose.position.x = 11.0; + pose.position.y = 13.0; + pose.position.z = 17.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + simple_sensor_simulator::primitives::Primitive prim(type, pose); + + const auto axis_x = math::geometry::Axis::X; + const auto axis_y = math::geometry::Axis::Y; + const auto axis_z = math::geometry::Axis::Z; + + const std::optional min_x = prim.getMin(axis_x); + const std::optional min_y = prim.getMin(axis_y); + const std::optional min_z = prim.getMin(axis_z); + + EXPECT_FALSE(min_x.has_value()); + EXPECT_FALSE(min_y.has_value()); + EXPECT_FALSE(min_z.has_value()); +} + +TEST(Primitive, getMin_emptyPrimitiveWithPose) +{ + const std::string type("name"); + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + pose.position.z = 0.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + simple_sensor_simulator::primitives::Primitive prim(type, pose); + geometry_msgs::msg::Pose sensor_pose{}; + sensor_pose.position.x = 11.0; + sensor_pose.position.y = 13.0; + sensor_pose.position.z = 17.0; + sensor_pose.orientation.x = -0.5; + sensor_pose.orientation.y = 0.5; + sensor_pose.orientation.z = 0.5; + sensor_pose.orientation.w = 0.5; + + const auto axis_x = math::geometry::Axis::X; + const auto axis_y = math::geometry::Axis::Y; + const auto axis_z = math::geometry::Axis::Z; + + const std::optional min_x = prim.getMin(axis_x, sensor_pose); + const std::optional min_y = prim.getMin(axis_y, sensor_pose); + const std::optional min_z = prim.getMin(axis_z, sensor_pose); + + EXPECT_FALSE(min_x.has_value()); + EXPECT_FALSE(min_y.has_value()); + EXPECT_FALSE(min_z.has_value()); +} + +TEST(Primitive, getMin_withTransform) +{ + const std::string type("name"); + geometry_msgs::msg::Pose pose; + pose.position.x = 3.0; + pose.position.y = 5.0; + pose.position.z = 7.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + + const float depth = 19.0; + const float width = 23.0; + const float height = 29.0; + simple_sensor_simulator::primitives::Box prim(depth, width, height, pose); + + geometry_msgs::msg::Pose sensor_pose{}; + sensor_pose.position.x = -11.0; + sensor_pose.position.y = -13.0; + sensor_pose.position.z = -17.0; + sensor_pose.orientation.x = -0.5; + sensor_pose.orientation.y = 0.5; + sensor_pose.orientation.z = 0.5; + sensor_pose.orientation.w = 0.5; + + const auto axis_x = math::geometry::Axis::X; + const auto axis_y = math::geometry::Axis::Y; + const auto axis_z = math::geometry::Axis::Z; + + const std::optional min_x = prim.getMin(axis_x, sensor_pose); + const std::optional min_y = prim.getMin(axis_y, sensor_pose); + const std::optional min_z = prim.getMin(axis_z, sensor_pose); + + EXPECT_TRUE(min_x.has_value()); + EXPECT_TRUE(min_y.has_value()); + EXPECT_TRUE(min_z.has_value()); + + EXPECT_TRUE(min_x.value() == pose.position.x - sensor_pose.position.x - height / 2); + EXPECT_TRUE(min_y.value() == pose.position.y - sensor_pose.position.y - depth / 2); + EXPECT_TRUE(min_z.value() == pose.position.z - sensor_pose.position.z - width / 2); +} + +TEST(Primitive, getMax_noTransform) +{ + const std::string type("name"); + geometry_msgs::msg::Pose pose; + pose.position.x = 9.0; + pose.position.y = 11.0; + pose.position.z = 17.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + + const float depth = 19.0; + const float width = 23.0; + const float height = 29.0; + simple_sensor_simulator::primitives::Box prim(depth, width, height, pose); + + const auto axis_x = math::geometry::Axis::X; + const auto axis_y = math::geometry::Axis::Y; + const auto axis_z = math::geometry::Axis::Z; + + const std::optional max_x = prim.getMax(axis_x); + const std::optional max_y = prim.getMax(axis_y); + const std::optional max_z = prim.getMax(axis_z); + + EXPECT_TRUE(max_x.has_value()); + EXPECT_TRUE(max_y.has_value()); + EXPECT_TRUE(max_z.has_value()); + EXPECT_TRUE(max_x.value() == pose.position.x + depth / 2); + EXPECT_TRUE(max_y.value() == pose.position.y + width / 2); + EXPECT_TRUE(max_z.value() == pose.position.z + height / 2); +} + +TEST(Primitive, getMax_emptyPrimitive) +{ + const std::string type("name"); + geometry_msgs::msg::Pose pose; + pose.position.x = 11.0; + pose.position.y = 13.0; + pose.position.z = 17.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + simple_sensor_simulator::primitives::Primitive prim(type, pose); + + const auto axis_x = math::geometry::Axis::X; + const auto axis_y = math::geometry::Axis::Y; + const auto axis_z = math::geometry::Axis::Z; + + const std::optional max_x = prim.getMax(axis_x); + const std::optional max_y = prim.getMax(axis_y); + const std::optional max_z = prim.getMax(axis_z); + + EXPECT_FALSE(max_x.has_value()); + EXPECT_FALSE(max_y.has_value()); + EXPECT_FALSE(max_z.has_value()); +} + +TEST(Primitive, getMax_emptyPrimitiveWithPose) +{ + const std::string type("name"); + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + pose.position.z = 0.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + simple_sensor_simulator::primitives::Primitive prim(type, pose); + geometry_msgs::msg::Pose sensor_pose{}; + sensor_pose.position.x = 11.0; + sensor_pose.position.y = 13.0; + sensor_pose.position.z = 17.0; + sensor_pose.orientation.x = -0.5; + sensor_pose.orientation.y = 0.5; + sensor_pose.orientation.z = 0.5; + sensor_pose.orientation.w = 0.5; + + const auto axis_x = math::geometry::Axis::X; + const auto axis_y = math::geometry::Axis::Y; + const auto axis_z = math::geometry::Axis::Z; + + const std::optional max_x = prim.getMax(axis_x, sensor_pose); + const std::optional max_y = prim.getMax(axis_y, sensor_pose); + const std::optional max_z = prim.getMax(axis_z, sensor_pose); + + EXPECT_FALSE(max_x.has_value()); + EXPECT_FALSE(max_y.has_value()); + EXPECT_FALSE(max_z.has_value()); +} + +TEST(Primitive, getMax_withTransform) +{ + const std::string type("name"); + geometry_msgs::msg::Pose pose; + pose.position.x = 3.0; + pose.position.y = 5.0; + pose.position.z = 7.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + + const float depth = 19.0; + const float width = 23.0; + const float height = 29.0; + simple_sensor_simulator::primitives::Box prim(depth, width, height, pose); + + geometry_msgs::msg::Pose sensor_pose{}; + sensor_pose.position.x = -11.0; + sensor_pose.position.y = -13.0; + sensor_pose.position.z = -17.0; + sensor_pose.orientation.x = -0.5; + sensor_pose.orientation.y = 0.5; + sensor_pose.orientation.z = 0.5; + sensor_pose.orientation.w = 0.5; + + const auto axis_x = math::geometry::Axis::X; + const auto axis_y = math::geometry::Axis::Y; + const auto axis_z = math::geometry::Axis::Z; + + const std::optional max_x = prim.getMax(axis_x, sensor_pose); + const std::optional max_y = prim.getMax(axis_y, sensor_pose); + const std::optional max_z = prim.getMax(axis_z, sensor_pose); + + EXPECT_TRUE(max_x.has_value()); + EXPECT_TRUE(max_y.has_value()); + EXPECT_TRUE(max_z.has_value()); + + EXPECT_TRUE(max_x.value() == pose.position.x - sensor_pose.position.x + height / 2); + EXPECT_TRUE(max_y.value() == pose.position.y - sensor_pose.position.y + depth / 2); + EXPECT_TRUE(max_z.value() == pose.position.z - sensor_pose.position.z + width / 2); +} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp new file mode 100644 index 00000000000..f9bb369fa0c --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp @@ -0,0 +1,86 @@ +#include + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST(Vertex, toVertex_onePoint) +{ + geometry_msgs::msg::Point point{}; + point.x = -11.0; + point.y = -13.0; + point.z = -17.0; + + simple_sensor_simulator::Vertex v = simple_sensor_simulator::toVertex(point); + + EXPECT_EQ(point.x, v.x); + EXPECT_EQ(point.y, v.y); + EXPECT_EQ(point.z, v.z); +} + +TEST(Vertex, toVertex_manyPoints) +{ + std::vector points{}; + for (int i = 0; i < 10; i++) { + geometry_msgs::msg::Point point{}; + point.x = -11.0 * i; + point.y = -13.0 * i; + point.z = -17.0 * i; + points.push_back(point); + } + + std::vector vertices = simple_sensor_simulator::toVertex(points); + + for (int i = 0; i < 10; i++) { + EXPECT_EQ(points[i].x, vertices[i].x); + EXPECT_EQ(points[i].y, vertices[i].y); + EXPECT_EQ(points[i].z, vertices[i].z); + } +} + +TEST(Vertex, toVertex_empty) +{ + std::vector points{}; + + std::vector vertices = simple_sensor_simulator::toVertex(points); + + EXPECT_TRUE(0 == vertices.size()); +} + +TEST(Vertex, toPoint_oneVertex) +{ + simple_sensor_simulator::Vertex v{}; + v.x = -11.0; + v.y = -13.0; + v.z = -17.0; + + geometry_msgs::msg::Point point = simple_sensor_simulator::toPoint(v); + + EXPECT_EQ(point.x, v.x); + EXPECT_EQ(point.y, v.y); + EXPECT_EQ(point.z, v.z); +} + +TEST(Vertex, toPoints_manyVertices) +{ + std::vector vertices{}; + for (int i = 0; i < 10; i++) { + simple_sensor_simulator::Vertex v{}; + v.x = -11.0 * i; + v.y = -13.0 * i; + v.z = -17.0 * i; + vertices.push_back(v); + } + + std::vector points = simple_sensor_simulator::toPoints(vertices); + + for (int i = 0; i < 10; i++) { + EXPECT_EQ(points[i].x, vertices[i].x); + EXPECT_EQ(points[i].y, vertices[i].y); + EXPECT_EQ(points[i].z, vertices[i].z); + } +} diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp new file mode 100644 index 00000000000..00e5136961b --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp @@ -0,0 +1,55 @@ +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} + +TEST(SensorSimulation, attachLidarSensor_wrongArchitecture) +{ + const double current_simulation_time = 3.0; + const simulation_api_schema::LidarConfiguration configuration{}; + + rclcpp::NodeOptions options; + rclcpp::Node node{"name", options}; + + auto sim = simple_sensor_simulator::SensorSimulation(); + + EXPECT_THROW( + sim.attachLidarSensor(current_simulation_time, configuration, node), std::runtime_error); +} + +TEST(SensorSimulation, attachDetectionSensor_wrongArchitecture) +{ + const double current_simulation_time = 3.0; + const simulation_api_schema::DetectionSensorConfiguration configuration{}; + + rclcpp::NodeOptions options; + rclcpp::Node node{"name", options}; + + auto sim = simple_sensor_simulator::SensorSimulation(); + + EXPECT_THROW( + sim.attachDetectionSensor(current_simulation_time, configuration, node), std::runtime_error); +} + +TEST(SensorSimulation, attachOccupancyGridSensor_wrongArchitecture) +{ + const double current_simulation_time = 3.0; + const simulation_api_schema::OccupancyGridSensorConfiguration configuration{}; + + rclcpp::NodeOptions options; + rclcpp::Node node{"name", options}; + + auto sim = simple_sensor_simulator::SensorSimulation(); + + EXPECT_THROW( + sim.attachOccupancyGridSensor(current_simulation_time, configuration, node), + std::runtime_error); +} diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/CMakeLists.txt new file mode 100644 index 00000000000..24a41741ce5 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/CMakeLists.txt @@ -0,0 +1,11 @@ +ament_add_gtest(test_sim_model_interface test_sim_model_interface.cpp) +target_link_libraries(test_sim_model_interface simple_sensor_simulator_component) + +ament_add_gtest(test_sim_model_ideal_steer_vel test_sim_model_ideal_steer_vel.cpp) +target_link_libraries(test_sim_model_ideal_steer_vel simple_sensor_simulator_component) + +ament_add_gtest(test_sim_model_ideal_steer_acc test_sim_model_ideal_steer_acc.cpp) +target_link_libraries(test_sim_model_ideal_steer_acc simple_sensor_simulator_component) + +ament_add_gtest(test_sim_model_ideal_steer_acc_geared test_sim_model_ideal_steer_acc_geared.cpp) +target_link_libraries(test_sim_model_ideal_steer_acc_geared simple_sensor_simulator_component) \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc.cpp new file mode 100644 index 00000000000..6cbc471c58f --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc.cpp @@ -0,0 +1,91 @@ +#include + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST(SimModelIdealSteerAcc, update_nonZeroDT) +{ + const double wheelbase = 1.0; + auto model = SimModelIdealSteerAcc(wheelbase); + SimModelInterface * p_model = &model; + Eigen::VectorXd x_state(p_model->getDimX()); + Eigen::VectorXd u_input(p_model->getDimU()); + const double start_x = 3.0; + const double start_y = 5.0; + const double start_yaw = 0.0; + const double start_vx = 11.0; + const double start_ax_des = 13.0; + const double start_steer_des = 0.0; + x_state << start_x, start_y, start_yaw, start_vx; + u_input << start_ax_des, start_steer_des; + + p_model->setInput(u_input); + p_model->setState(x_state); + + const double dt = 1.0; + for (int i = 0; i < 10; i++) { + double current_vx = start_vx + i * dt * start_ax_des; + double current_x = start_x + i * dt * start_vx + 0.5 * i * i * dt * start_ax_des; + EXPECT_TRUE(std::abs(current_x - p_model->getX()) < 1e-5); + EXPECT_TRUE(std::abs(current_vx - p_model->getVx()) < 1e-5); + EXPECT_TRUE(start_y == p_model->getY()); + EXPECT_TRUE(start_yaw == p_model->getYaw()); + p_model->update(dt); + } +} + +TEST(SimModelIdealSteerAcc, update_zeroDT) +{ + const double wheelbase = 1.0; + auto model = SimModelIdealSteerAcc(wheelbase); + SimModelInterface * p_model = &model; + Eigen::VectorXd x_state(p_model->getDimX()); + Eigen::VectorXd u_input(p_model->getDimU()); + const double start_x = 3.0; + const double start_y = 5.0; + const double start_yaw = 7.0; + const double start_vx = 11.0; + const double start_ax_des = 13.0; + const double start_steer_des = 17.0; + x_state << start_x, start_y, start_yaw, start_vx; + u_input << start_ax_des, start_steer_des; + + p_model->setInput(u_input); + p_model->setState(x_state); + + const double dt = 0.0; + for (int i = 0; i < 10; i++) { + EXPECT_TRUE(start_x == p_model->getX()); + EXPECT_TRUE(start_y == p_model->getY()); + EXPECT_TRUE(start_yaw == p_model->getYaw()); + EXPECT_TRUE(start_vx == p_model->getVx()); + p_model->update(dt); + } +} + +TEST(SimModelIdealSteerAcc, calcModel_zeroWheelbase) +{ + const double wheelbase = 0.0; + auto model = SimModelIdealSteerAcc(wheelbase); + SimModelInterface * p_model = &model; + Eigen::VectorXd x_state(p_model->getDimX()); + Eigen::VectorXd u_input(p_model->getDimU()); + const double start_x = 3.0; + const double start_y = 5.0; + const double start_yaw = 7.0; + const double start_vx = 11.0; + const double start_ax_des = 13.0; + const double start_steer_des = 17.0; + x_state << start_x, start_y, start_yaw, start_vx; + u_input << start_ax_des, start_steer_des; + + auto out_x_state = p_model->calcModel(x_state, u_input); + p_model->setState(out_x_state); + + EXPECT_TRUE(p_model->getYaw() < std::numeric_limits::infinity()); +} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc_geared.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc_geared.cpp new file mode 100644 index 00000000000..0db9635539c --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc_geared.cpp @@ -0,0 +1,91 @@ +#include + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST(SimModelIdealSteerAccGeared, update_nonZeroDT) +{ + const double wheelbase = 1.0; + auto model = SimModelIdealSteerAccGeared(wheelbase); + SimModelInterface * p_model = &model; + Eigen::VectorXd x_state(p_model->getDimX()); + Eigen::VectorXd u_input(p_model->getDimU()); + const double start_x = 3.0; + const double start_y = 5.0; + const double start_yaw = 0.0; + const double start_vx = 11.0; + const double start_ax_des = 13.0; + const double start_steer_des = 0.0; + x_state << start_x, start_y, start_yaw, start_vx; + u_input << start_ax_des, start_steer_des; + + p_model->setInput(u_input); + p_model->setState(x_state); + + const double dt = 1.0; + for (int i = 0; i < 10; i++) { + double current_vx = start_vx + i * dt * start_ax_des; + double current_x = start_x + i * dt * start_vx + 0.5 * i * i * dt * start_ax_des; + EXPECT_TRUE(std::abs(current_x - p_model->getX()) < 1e-5); + EXPECT_TRUE(std::abs(current_vx - p_model->getVx()) < 1e-5); + EXPECT_TRUE(start_y == p_model->getY()); + EXPECT_TRUE(start_yaw == p_model->getYaw()); + p_model->update(dt); + } +} + +TEST(SimModelIdealSteerAccGeared, update_zeroDT) +{ + const double wheelbase = 1.0; + auto model = SimModelIdealSteerAccGeared(wheelbase); + SimModelInterface * p_model = &model; + Eigen::VectorXd x_state(p_model->getDimX()); + Eigen::VectorXd u_input(p_model->getDimU()); + const double start_x = 3.0; + const double start_y = 5.0; + const double start_yaw = 7.0; + const double start_vx = 11.0; + const double start_ax_des = 13.0; + const double start_steer_des = 17.0; + x_state << start_x, start_y, start_yaw, start_vx; + u_input << start_ax_des, start_steer_des; + + p_model->setInput(u_input); + p_model->setState(x_state); + + const double dt = 0.0; + for (int i = 0; i < 10; i++) { + EXPECT_TRUE(start_x == p_model->getX()); + EXPECT_TRUE(start_y == p_model->getY()); + EXPECT_TRUE(start_yaw == p_model->getYaw()); + EXPECT_TRUE(start_vx == p_model->getVx()); + p_model->update(dt); + } +} + +TEST(SimModelIdealSteerAccGeared, calcModel_zeroWheelbase) +{ + const double wheelbase = 0.0; + auto model = SimModelIdealSteerAccGeared(wheelbase); + SimModelInterface * p_model = &model; + Eigen::VectorXd x_state(p_model->getDimX()); + Eigen::VectorXd u_input(p_model->getDimU()); + const double start_x = 3.0; + const double start_y = 5.0; + const double start_yaw = 7.0; + const double start_vx = 11.0; + const double start_ax_des = 13.0; + const double start_steer_des = 17.0; + x_state << start_x, start_y, start_yaw, start_vx; + u_input << start_ax_des, start_steer_des; + + auto out_x_state = p_model->calcModel(x_state, u_input); + p_model->setState(out_x_state); + + EXPECT_TRUE(p_model->getYaw() < std::numeric_limits::infinity()); +} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_vel.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_vel.cpp new file mode 100644 index 00000000000..f0089fddbe1 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_vel.cpp @@ -0,0 +1,87 @@ +#include + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +/* +0. dt: division by 0 +1. wheelbase: division by 0 +2. unintuitive choice of storing the parameters +3. unsure of how calcModel should work. initial position does not influence the result. +*/ + +TEST(SimModelIdealSteerVel, update_nonZeroDT) +{ + const double wheelbase = 1.0; + auto model = SimModelIdealSteerVel(wheelbase); + SimModelInterface * p_model = &model; + const double dt = 0.01; + Eigen::VectorXd x_state(p_model->getDimX()); + Eigen::VectorXd u_input(p_model->getDimU()); + const double start_vx = 3.0; + const double start_x = 5.0; + x_state << start_x, 0.0, 0.0; + u_input << start_vx, 0.0; + p_model->setInput(u_input); + p_model->setState(x_state); + + for (int i = 0; i < 10; i++) { + EXPECT_TRUE(std::abs(start_x + i * dt * start_vx - p_model->getX()) < 1e-5); + EXPECT_TRUE(0.0 == p_model->getY()); + EXPECT_TRUE(0.0 == p_model->getYaw()); + EXPECT_TRUE(start_vx == p_model->getVx()); + p_model->update(dt); + } + EXPECT_TRUE(p_model->getAx() < std::numeric_limits::infinity()); +} + +TEST(SimModelIdealSteerVel, update_zeroDT) +{ + const double wheelbase = 1.0; + auto model = SimModelIdealSteerVel(wheelbase); + SimModelInterface * p_model = &model; + const double dt = 0.0; + Eigen::VectorXd x_state(p_model->getDimX()); + Eigen::VectorXd u_input(p_model->getDimU()); + const double start_vx = 3.0; + const double start_x = 5.0; + x_state << start_x, 0.0, 0.0; + u_input << start_vx, 0.0; + p_model->setInput(u_input); + p_model->setState(x_state); + + for (int i = 0; i < 10; i++) { + EXPECT_TRUE(std::abs(start_x + i * dt * start_vx - p_model->getX()) < 1e-5); + EXPECT_TRUE(0.0 == p_model->getY()); + EXPECT_TRUE(0.0 == p_model->getYaw()); + EXPECT_TRUE(start_vx == p_model->getVx()); + p_model->update(dt); + } + EXPECT_TRUE(p_model->getAx() < std::numeric_limits::infinity()); +} + +TEST(SimModelIdealSteerVel, calcModel_zeroWheelbase) +{ + const double wheelbase = 0.0; + auto model = SimModelIdealSteerVel(wheelbase); + SimModelInterface * p_model = &model; + Eigen::VectorXd x_state(p_model->getDimX()); + Eigen::VectorXd u_input(p_model->getDimU()); + const double start_x = 3.0; + const double start_y = 5.0; + const double start_yaw = 7.0; + const double start_vx = 11.0; + const double start_steer = 13.0; + x_state << start_x, start_y, start_yaw; + u_input << start_vx, start_steer; + + auto out_x_state = p_model->calcModel(x_state, u_input); + p_model->setState(out_x_state); + + EXPECT_TRUE(p_model->getYaw() < std::numeric_limits::infinity()); +} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_interface.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_interface.cpp new file mode 100644 index 00000000000..034f4113904 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_interface.cpp @@ -0,0 +1,9 @@ +#include + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file From b6516429817a144b2bc6f1e0961ac228d68de99e Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 10 Apr 2024 11:23:47 +0200 Subject: [PATCH 008/118] traffic_simulator unit tests --- .../traffic_simulator/test/CMakeLists.txt | 1 + .../test/src/behavior/CMakeLists.txt | 2 + .../test_longitudinal_speed_planning.cpp | 486 ++++++++++++++++++ 3 files changed, 489 insertions(+) create mode 100644 simulation/traffic_simulator/test/src/behavior/CMakeLists.txt create mode 100644 simulation/traffic_simulator/test/src/behavior/test_longitudinal_speed_planning.cpp diff --git a/simulation/traffic_simulator/test/CMakeLists.txt b/simulation/traffic_simulator/test/CMakeLists.txt index 510659b8245..911179a95a4 100644 --- a/simulation/traffic_simulator/test/CMakeLists.txt +++ b/simulation/traffic_simulator/test/CMakeLists.txt @@ -1,6 +1,7 @@ add_subdirectory(src/traffic_lights) add_subdirectory(src/helper) add_subdirectory(src/entity) +add_subdirectory(src/behavior) ament_add_gtest(test_hdmap_utils src/test_hdmap_utils.cpp) target_link_libraries(test_hdmap_utils traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt b/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt new file mode 100644 index 00000000000..224ef897e54 --- /dev/null +++ b/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_longitudinal_speed_planning test_longitudinal_speed_planning.cpp) +target_link_libraries(test_longitudinal_speed_planning traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/behavior/test_longitudinal_speed_planning.cpp b/simulation/traffic_simulator/test/src/behavior/test_longitudinal_speed_planning.cpp new file mode 100644 index 00000000000..2c059662856 --- /dev/null +++ b/simulation/traffic_simulator/test/src/behavior/test_longitudinal_speed_planning.cpp @@ -0,0 +1,486 @@ +#include + +#include +#include + +#include "../expect_eq_macros.hpp" + +#define EXPECT_CONSTRAINTS_BOUNDED(DATA, lower, upper) \ + EXPECT_TRUE(new_constraints.max_speed >= lower); \ + EXPECT_TRUE(new_constraints.max_acceleration >= lower); \ + EXPECT_TRUE(new_constraints.max_deceleration >= lower); \ + EXPECT_TRUE(new_constraints.max_acceleration_rate >= lower); \ + EXPECT_TRUE(new_constraints.max_deceleration_rate >= lower); \ + EXPECT_TRUE(new_constraints.max_speed < upper); \ + EXPECT_TRUE(new_constraints.max_acceleration < upper); \ + EXPECT_TRUE(new_constraints.max_deceleration < upper); \ + EXPECT_TRUE(new_constraints.max_acceleration_rate < upper); \ + EXPECT_TRUE(new_constraints.max_deceleration_rate < upper); + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST(LongitudinalSpeedPlanner, isAccelerating_true) +{ + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + current_twist.linear.x = 1.0; + + const double target_speed = 2.0; + EXPECT_TRUE(planner.isAccelerating(target_speed, current_twist)); +} + +TEST(LongitudinalSpeedPlanner, isAccelerating_false) +{ + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + current_twist.linear.x = 3.0; + + const double target_speed = 2.0; + EXPECT_FALSE(planner.isAccelerating(target_speed, current_twist)); +} + +TEST(LongitudinalSpeedPlanner, isDecelerating_true) +{ + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + current_twist.linear.x = 3.0; + + const double target_speed = 2.0; + EXPECT_TRUE(planner.isDecelerating(target_speed, current_twist)); +} + +TEST(LongitudinalSpeedPlanner, isDecelerating_false) +{ + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + current_twist.linear.x = 1.0; + + const double target_speed = 2.0; + EXPECT_FALSE(planner.isDecelerating(target_speed, current_twist)); +} + +TEST(LongitudinalSpeedPlanner, getAccelerationDuration_acceleration) +{ + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + geometry_msgs::msg::Accel current_accel{}; + traffic_simulator_msgs::msg::DynamicConstraints constraints{}; + current_twist.linear.x = 1.0; + current_accel.linear.x = 1.0; + constraints.max_speed = 10.0; + constraints.max_acceleration = 2.0; + constraints.max_acceleration_rate = 1.0; + + const double epsilon = 1e-5; + + const double expected_duration = 4.0; + const double target_speed = 8.5f; + + const double result_duration = + planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel); + EXPECT_TRUE(std::abs(expected_duration - result_duration) < epsilon); +} + +TEST(LongitudinalSpeedPlanner, getAccelerationDuration_zero) +{ + // possible negative duration: + // longitudinal_speed_planning: 185, 199 + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + geometry_msgs::msg::Accel current_accel{}; + traffic_simulator_msgs::msg::DynamicConstraints constraints{}; + current_twist.linear.x = 1.0; + current_accel.linear.x = 1.0; + constraints.max_speed = 10.0; + constraints.max_acceleration = 1.0; + constraints.max_deceleration = 1.0; + constraints.max_acceleration_rate = 1.0; + constraints.max_deceleration_rate = 1.0; + + const double epsilon = 1e-5; + + double target_speed = current_twist.linear.x + epsilon; + double result_duration = + planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel); + EXPECT_TRUE(result_duration >= 0); + EXPECT_TRUE(result_duration < epsilon); + + constraints.max_speed = 100.0; + result_duration = + planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel); + EXPECT_TRUE(result_duration >= 0); + EXPECT_TRUE(result_duration < epsilon); + + current_twist.linear.x = 0.0; + target_speed = current_twist.linear.x + epsilon; + result_duration = + planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel); + EXPECT_TRUE(result_duration >= 0); + EXPECT_TRUE(result_duration < epsilon); + + target_speed = current_twist.linear.x + 0.0101; + result_duration = + planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel); + EXPECT_TRUE(result_duration >= 0); + EXPECT_TRUE(result_duration <= 0.0101); + target_speed = current_twist.linear.x + 0.0099; + result_duration = + planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel); + EXPECT_TRUE(result_duration >= 0); + EXPECT_TRUE(result_duration <= 0.0099); +} + +TEST(LongitudinalSpeedPlanner, planConstraintsFromJerkAndTimeConstraint_jerk) +{ + // possible sqrt of a negative number, results in a nan + // longitudinal_speed_planning: 59, 68 + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + geometry_msgs::msg::Accel current_accel{}; + traffic_simulator_msgs::msg::DynamicConstraints constraints{}; + current_twist.linear.x = 10.0; + current_accel.linear.x = 1.0; + constraints.max_speed = 10.0; + constraints.max_acceleration = 1.0; + constraints.max_deceleration = 1.0; + constraints.max_acceleration_rate = 1.0; + constraints.max_deceleration_rate = 1.0; + + double target_speed = 5.0; + double acceleration_duration = 1.0; + auto new_constraints = planner.planConstraintsFromJerkAndTimeConstraint( + target_speed, current_twist, current_accel, acceleration_duration, constraints); + + const double plausible_bound = 1e2; + EXPECT_CONSTRAINTS_BOUNDED(constraints, 0, plausible_bound); + + current_twist.linear.x = 1.0; + new_constraints = planner.planConstraintsFromJerkAndTimeConstraint( + target_speed, current_twist, current_accel, acceleration_duration, constraints); + EXPECT_CONSTRAINTS_BOUNDED(constraints, 0, plausible_bound); +} + +TEST(LongitudinalSpeedPlanner, planConstraintsFromJerkAndTimeConstraint_acceleration) +{ + // possible sqrt of a negative number, results in a nan + // longitudinal_speed_planning: 59, 68 + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + geometry_msgs::msg::Accel current_accel{}; + traffic_simulator_msgs::msg::DynamicConstraints constraints{}; + current_twist.linear.x = 10.0; + current_accel.linear.x = 1.0; + constraints.max_speed = 10.0; + constraints.max_acceleration = 1.0; + constraints.max_deceleration = 1.0; + constraints.max_acceleration_rate = 1.0; + constraints.max_deceleration_rate = 1.0; + + const double epsilon = 1e5; + double target_speed = current_twist.linear.x + epsilon; + double acceleration_duration = 1.0; + auto new_constraints = planner.planConstraintsFromJerkAndTimeConstraint( + target_speed, current_twist, current_accel, acceleration_duration, constraints); + + const double plausible_bound = 1e2; + EXPECT_CONSTRAINTS_BOUNDED(constraints, 0, plausible_bound); + + current_twist.linear.x = 0.0; + target_speed = current_twist.linear.x + epsilon; + new_constraints = planner.planConstraintsFromJerkAndTimeConstraint( + target_speed, current_twist, current_accel, acceleration_duration, constraints); + EXPECT_CONSTRAINTS_BOUNDED(constraints, 0, plausible_bound); +} + +TEST(LongitudinalSpeedPlanner, planConstraintsFromJerkAndTimeConstraint_deceleration) +{ + // possible sqrt of a negative number, results in a nan + // longitudinal_speed_planning: 59, 68 + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + geometry_msgs::msg::Accel current_accel{}; + traffic_simulator_msgs::msg::DynamicConstraints constraints{}; + current_twist.linear.x = 10.0; + current_accel.linear.x = 1.0; + constraints.max_speed = 10.0; + constraints.max_acceleration = 1.0; + constraints.max_deceleration = 1.0; + constraints.max_acceleration_rate = 1.0; + constraints.max_deceleration_rate = 1.0; + + const double epsilon = 1e5; + double target_speed = current_twist.linear.x - epsilon; + double acceleration_duration = 1.0; + auto new_constraints = planner.planConstraintsFromJerkAndTimeConstraint( + target_speed, current_twist, current_accel, acceleration_duration, constraints); + + const double plausible_bound = 1e2; + EXPECT_CONSTRAINTS_BOUNDED(constraints, 0, plausible_bound); + + current_twist.linear.x = 0.0; + target_speed = current_twist.linear.x - epsilon; + new_constraints = planner.planConstraintsFromJerkAndTimeConstraint( + target_speed, current_twist, current_accel, acceleration_duration, constraints); + EXPECT_CONSTRAINTS_BOUNDED(constraints, 0, plausible_bound); +} + +TEST(LongitudinalSpeedPlanner, getDynamicStates_targetSpeedOverLimit) +{ + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + geometry_msgs::msg::Accel current_accel{}; + traffic_simulator_msgs::msg::DynamicConstraints constraints{}; + current_twist.linear.x = 10.0; + current_accel.linear.x = 1.0; + constraints.max_speed = 20.0; + constraints.max_acceleration = 1.0; + constraints.max_deceleration = 1.0; + constraints.max_acceleration_rate = 1.0; + constraints.max_deceleration_rate = 1.0; + + double target_speed = 100.0; + auto [result0_twist, result0_accel, result0_jerk] = + planner.getDynamicStates(target_speed, constraints, current_twist, current_accel); + + target_speed = constraints.max_speed; + auto [result1_twist, result1_accel, result1_jerk] = + planner.getDynamicStates(target_speed, constraints, current_twist, current_accel); + + EXPECT_ACCEL_EQ(result0_accel, result1_accel); + EXPECT_TWIST_EQ(result0_twist, result1_twist); + EXPECT_DOUBLE_EQ(result0_jerk, result1_jerk); +} + +TEST(LongitudinalSpeedPlanner, getDynamicStates_maxJerk) +{ + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + geometry_msgs::msg::Accel current_accel{}; + traffic_simulator_msgs::msg::DynamicConstraints constraints{}; + current_twist.linear.x = 0.0; + current_accel.linear.x = 0.0; + constraints.max_speed = 20.0; + constraints.max_acceleration = 5.0; + constraints.max_deceleration = 5.0; + constraints.max_acceleration_rate = 1.0; + constraints.max_deceleration_rate = 1.0; + + double target_speed = constraints.max_speed; + + double result0_jerk = + std::get<2>(planner.getDynamicStates(target_speed, constraints, current_twist, current_accel)); + EXPECT_DOUBLE_EQ(result0_jerk, constraints.max_acceleration_rate); + + constraints.max_acceleration_rate = 2.0; + double result1_jerk = + std::get<2>(planner.getDynamicStates(target_speed, constraints, current_twist, current_accel)); + EXPECT_DOUBLE_EQ(result1_jerk, constraints.max_acceleration_rate); +} + +TEST(LongitudinalSpeedPlanner, getDynamicStates_shortAccel) +{ + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + geometry_msgs::msg::Accel current_accel{}; + traffic_simulator_msgs::msg::DynamicConstraints constraints{}; + current_twist.linear.x = 10.0; + current_accel.linear.x = 0.0; + constraints.max_speed = 20.0; + constraints.max_acceleration = 5.0; + constraints.max_deceleration = 5.0; + constraints.max_acceleration_rate = 5.0; + constraints.max_deceleration_rate = 5.0; + + const double epsilon = 1e-8; + double target_speed = current_twist.linear.x - epsilon; + + double result0_jerk = + std::get<2>(planner.getDynamicStates(target_speed, constraints, current_twist, current_accel)); + EXPECT_FALSE(result0_jerk == -constraints.max_deceleration_rate); + + constraints.max_deceleration_rate = 2.0; + double result1_jerk = + std::get<2>(planner.getDynamicStates(target_speed, constraints, current_twist, current_accel)); + EXPECT_FALSE(result1_jerk == -constraints.max_deceleration_rate); + + current_twist.linear.x = 0; + target_speed = current_twist.linear.x - epsilon; + double result2_jerk = + std::get<2>(planner.getDynamicStates(target_speed, constraints, current_twist, current_accel)); + EXPECT_FALSE(result2_jerk == -constraints.max_deceleration_rate); +} + +TEST(LongitudinalSpeedPlanner, isTargetSpeedReached_different) +{ + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + current_twist.linear.x = 10.0; + double target_speed = 15.0; + double tolerance = 3.0; + + EXPECT_FALSE(planner.isTargetSpeedReached(target_speed, current_twist, tolerance)); + + target_speed = 12.0; + EXPECT_TRUE(planner.isTargetSpeedReached(target_speed, current_twist, tolerance)); +} + +TEST(LongitudinalSpeedPlanner, isTargetSpeedReached_same) +{ + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + current_twist.linear.x = 10.0; + double target_speed = 10.0; + double tolerance = 1.0; + + EXPECT_TRUE(planner.isTargetSpeedReached(target_speed, current_twist, tolerance)); + + tolerance = 0.0; + EXPECT_TRUE(planner.isTargetSpeedReached(target_speed, current_twist, tolerance)); +} + +TEST(LongitudinalSpeedPlanner, getRunningDistance_shortTime) +{ + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + geometry_msgs::msg::Accel current_accel{}; + traffic_simulator_msgs::msg::DynamicConstraints constraints{}; + current_twist.linear.x = 10.0; + current_accel.linear.x = 0.0; + constraints.max_speed = 70.0; + constraints.max_acceleration = 18.0; + constraints.max_deceleration = 18.0; + constraints.max_acceleration_rate = 6.0; + constraints.max_deceleration_rate = 6.0; + + const double epsilon = 0.1; + double target_speed = current_twist.linear.x - epsilon; + double current_linear_jerk = 0.0; + double distance = planner.getRunningDistance( + target_speed, constraints, current_twist, current_accel, current_linear_jerk); + + EXPECT_TRUE(distance > 0); + double quad_time = constraints.max_deceleration / constraints.max_deceleration_rate; + double lin_time = (current_twist.linear.x - target_speed) / constraints.max_deceleration; + double distance_upper_bound = current_twist.linear.x * std::max(quad_time, lin_time); + EXPECT_TRUE(distance < distance_upper_bound); +} + +TEST(LongitudinalSpeedPlanner, getRunningDistance_longTime) +{ + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + geometry_msgs::msg::Accel current_accel{}; + traffic_simulator_msgs::msg::DynamicConstraints constraints{}; + current_twist.linear.x = 60.0; + current_accel.linear.x = 0.0; + constraints.max_speed = 70.0; + constraints.max_acceleration = 1.0; + constraints.max_deceleration = 1.0; + constraints.max_acceleration_rate = 5.0; + constraints.max_deceleration_rate = 5.0; + + double target_speed = 0.0; + double current_linear_jerk = 0.0; + double distance = planner.getRunningDistance( + target_speed, constraints, current_twist, current_accel, current_linear_jerk); + + EXPECT_TRUE(distance > 0); + double quad_time = constraints.max_deceleration / constraints.max_deceleration_rate; + double lin_time = (current_twist.linear.x - target_speed) / constraints.max_deceleration; + double distance_upper_bound = current_twist.linear.x * std::max(quad_time, lin_time); + EXPECT_TRUE(distance < distance_upper_bound); +} + +TEST(LongitudinalSpeedPlanner, getRunningDistance_zero) +{ + const double step_time = 0.001; + const std::string entity = "entity"; + traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ + step_time, entity}; + + geometry_msgs::msg::Twist current_twist{}; + geometry_msgs::msg::Accel current_accel{}; + traffic_simulator_msgs::msg::DynamicConstraints constraints{}; + current_twist.linear.x = 10.0; + current_accel.linear.x = 0.0; + constraints.max_speed = 20.0; + constraints.max_acceleration = 5.0; + constraints.max_deceleration = 5.0; + constraints.max_acceleration_rate = 5.0; + constraints.max_deceleration_rate = 5.0; + + double target_speed = current_twist.linear.x; + double current_linear_jerk = 1.0; + double distance = planner.getRunningDistance( + target_speed, constraints, current_twist, current_accel, current_linear_jerk); + + EXPECT_TRUE(distance == 0); +} From c132545a61d608371691b49eb420c6d6cb305cbe Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 10 Apr 2024 11:24:15 +0200 Subject: [PATCH 009/118] simulation_interface unit tests --- .../simulation_interface/CMakeLists.txt | 3 +- .../simulation_interface/conversions.hpp | 9 +- .../simulation_interface/test/CMakeLists.txt | 2 + .../{ => include}/expect_equal_macros.hpp | 0 .../test/src/test_conversions.cpp | 1504 +++++++++++++++++ .../test/test_conversions.cpp | 534 ------ 6 files changed, 1510 insertions(+), 542 deletions(-) create mode 100644 simulation/simulation_interface/test/CMakeLists.txt rename simulation/simulation_interface/test/{ => include}/expect_equal_macros.hpp (100%) create mode 100644 simulation/simulation_interface/test/src/test_conversions.cpp delete mode 100644 simulation/simulation_interface/test/test_conversions.cpp diff --git a/simulation/simulation_interface/CMakeLists.txt b/simulation/simulation_interface/CMakeLists.txt index 6e2cc1fb292..c00c98db57f 100644 --- a/simulation/simulation_interface/CMakeLists.txt +++ b/simulation/simulation_interface/CMakeLists.txt @@ -72,8 +72,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(test_conversion test/test_conversions.cpp) - target_link_libraries(test_conversion simulation_interface) + add_subdirectory(test) endif() ament_auto_package() diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 4ec8320616b..bd66bb1e583 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -192,8 +192,7 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr - { + auto convert_color = [](auto color) constexpr { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -208,8 +207,7 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr - { + auto convert_shape = [](auto shape) constexpr { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -237,8 +235,7 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr - { + auto convert_status = [](auto status) constexpr { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; diff --git a/simulation/simulation_interface/test/CMakeLists.txt b/simulation/simulation_interface/test/CMakeLists.txt new file mode 100644 index 00000000000..faf6a8f76c7 --- /dev/null +++ b/simulation/simulation_interface/test/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_conversion src/test_conversions.cpp) +target_link_libraries(test_conversion simulation_interface) diff --git a/simulation/simulation_interface/test/expect_equal_macros.hpp b/simulation/simulation_interface/test/include/expect_equal_macros.hpp similarity index 100% rename from simulation/simulation_interface/test/expect_equal_macros.hpp rename to simulation/simulation_interface/test/include/expect_equal_macros.hpp diff --git a/simulation/simulation_interface/test/src/test_conversions.cpp b/simulation/simulation_interface/test/src/test_conversions.cpp new file mode 100644 index 00000000000..6c7715e5e26 --- /dev/null +++ b/simulation/simulation_interface/test/src/test_conversions.cpp @@ -0,0 +1,1504 @@ +// 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 +#include + +#include "../include/expect_equal_macros.hpp" + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST(Conversion, toProto_Point) +{ + geometry_msgs::msg::Point src_msg; + geometry_msgs::Point dst_proto; + src_msg.y = 3.0; + src_msg.z = 5.0; + src_msg.z = 7.0; + simulation_interface::toProto(src_msg, dst_proto); + EXPECT_POINT_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_Point) +{ + geometry_msgs::msg::Point init_msg; + geometry_msgs::Point src_proto; + geometry_msgs::msg::Point dst_msg; + init_msg.y = 3.0; + init_msg.z = 5.0; + init_msg.z = 7.0; + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + EXPECT_POINT_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_Quaternion) +{ + geometry_msgs::msg::Quaternion src_msg; + geometry_msgs::Quaternion dst_proto; + src_msg.x = 3.0; + src_msg.y = 5.0; + src_msg.z = 7.0; + src_msg.w = 11.0; + simulation_interface::toProto(src_msg, dst_proto); + EXPECT_QUATERNION_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_Quaternion) +{ + geometry_msgs::msg::Quaternion init_msg; + geometry_msgs::Quaternion src_proto; + geometry_msgs::msg::Quaternion dst_msg; + init_msg.x = 3.0; + init_msg.y = 5.0; + init_msg.z = 7.0; + init_msg.w = 11.0; + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + EXPECT_QUATERNION_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_Pose) +{ + geometry_msgs::msg::Pose src_msg; + geometry_msgs::Pose dst_proto; + src_msg.position.x = 3.0; + src_msg.position.y = 5.0; + src_msg.position.z = 7.0; + src_msg.orientation.x = 11.0; + src_msg.orientation.y = 13.0; + src_msg.orientation.z = 17.0; + src_msg.orientation.w = 19.0; + simulation_interface::toProto(src_msg, dst_proto); + EXPECT_POSE_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_Pose) +{ + geometry_msgs::msg::Pose init_msg; + geometry_msgs::Pose src_proto; + geometry_msgs::msg::Pose dst_msg; + init_msg.position.x = 3.0; + init_msg.position.y = 5.0; + init_msg.position.z = 7.0; + init_msg.orientation.x = 11.0; + init_msg.orientation.y = 13.0; + init_msg.orientation.z = 17.0; + init_msg.orientation.w = 19.0; + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + EXPECT_POSE_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_Vector) +{ + geometry_msgs::msg::Vector3 src_msg; + geometry_msgs::Vector3 dst_proto; + src_msg.x = 3.0; + src_msg.y = 5.0; + src_msg.z = 7.0; + simulation_interface::toProto(src_msg, dst_proto); + EXPECT_VECTOR3_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_Vector) +{ + geometry_msgs::msg::Vector3 init_msg; + geometry_msgs::Vector3 src_proto; + geometry_msgs::msg::Vector3 dst_msg; + init_msg.x = 3.0; + init_msg.y = 5.0; + init_msg.z = 7.0; + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + EXPECT_VECTOR3_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_Twist) +{ + geometry_msgs::msg::Twist src_msg; + geometry_msgs::Twist dst_proto; + src_msg.linear.x = 3.0; + src_msg.linear.y = 5.0; + src_msg.linear.z = 7.0; + simulation_interface::toProto(src_msg, dst_proto); + EXPECT_TWIST_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_Twist) +{ + geometry_msgs::msg::Twist init_msg; + geometry_msgs::Twist src_proto; + geometry_msgs::msg::Twist dst_msg; + init_msg.linear.x = 3.0; + init_msg.linear.y = 5.0; + init_msg.linear.z = 7.0; + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + EXPECT_TWIST_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_Accel) +{ + geometry_msgs::msg::Accel src_msg; + geometry_msgs::Accel dst_proto; + src_msg.linear.x = 3.0; + src_msg.linear.y = 5.0; + src_msg.linear.z = 7.0; + simulation_interface::toProto(src_msg, dst_proto); + EXPECT_ACCEL_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_Accel) +{ + geometry_msgs::msg::Accel init_msg; + geometry_msgs::Accel src_proto; + geometry_msgs::msg::Accel dst_msg; + init_msg.linear.x = 3.0; + init_msg.linear.y = 5.0; + init_msg.linear.z = 7.0; + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + EXPECT_ACCEL_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_Performance) +{ + traffic_simulator_msgs::msg::Performance src_msg; + traffic_simulator_msgs::Performance dst_proto; + src_msg.max_speed = 3.0; + src_msg.max_deceleration = 5.0; + src_msg.max_acceleration = 7.0; + src_msg.max_deceleration_rate = 11.0; + src_msg.max_acceleration_rate = 13.0; + simulation_interface::toProto(src_msg, dst_proto); + EXPECT_PERFORMANCE_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_Performance) +{ + traffic_simulator_msgs::msg::Performance init_msg; + traffic_simulator_msgs::Performance src_proto; + traffic_simulator_msgs::msg::Performance dst_msg; + init_msg.max_speed = 3.0; + init_msg.max_deceleration = 5.0; + init_msg.max_acceleration = 7.0; + init_msg.max_deceleration_rate = 11.0; + init_msg.max_acceleration_rate = 13.0; + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + EXPECT_PERFORMANCE_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_Axle) +{ + traffic_simulator_msgs::msg::Axle src_msg; + traffic_simulator_msgs::Axle dst_proto; + src_msg.max_steering = 3.0; + src_msg.position_x = 5.0; + src_msg.position_z = 7.0; + src_msg.track_width = 11.0; + src_msg.wheel_diameter = 13.0; + simulation_interface::toProto(src_msg, dst_proto); + EXPECT_AXLE_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_Axle) +{ + traffic_simulator_msgs::msg::Axle init_msg; + traffic_simulator_msgs::Axle src_proto; + traffic_simulator_msgs::msg::Axle dst_msg; + init_msg.max_steering = 3.0; + init_msg.position_x = 5.0; + init_msg.position_z = 7.0; + init_msg.track_width = 11.0; + init_msg.wheel_diameter = 13.0; + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + EXPECT_AXLE_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_Axles) +{ + traffic_simulator_msgs::msg::Axles src_msg; + traffic_simulator_msgs::Axles dst_proto; + src_msg.front_axle.max_steering = 3.0; + src_msg.front_axle.position_x = 5.0; + src_msg.front_axle.position_z = 7.0; + src_msg.front_axle.track_width = 11.0; + src_msg.front_axle.wheel_diameter = 13.0; + src_msg.rear_axle.max_steering = 17.0; + src_msg.rear_axle.position_x = 19.0; + src_msg.rear_axle.position_z = 23.0; + src_msg.rear_axle.track_width = 29.0; + src_msg.rear_axle.wheel_diameter = 31.0; + simulation_interface::toProto(src_msg, dst_proto); + EXPECT_AXLES_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_Axles) +{ + traffic_simulator_msgs::msg::Axles init_msg; + traffic_simulator_msgs::Axles src_proto; + traffic_simulator_msgs::msg::Axles dst_msg; + init_msg.front_axle.max_steering = 3.0; + init_msg.front_axle.position_x = 5.0; + init_msg.front_axle.position_z = 7.0; + init_msg.front_axle.track_width = 11.0; + init_msg.front_axle.wheel_diameter = 13.0; + init_msg.rear_axle.max_steering = 17.0; + init_msg.rear_axle.position_x = 19.0; + init_msg.rear_axle.position_z = 23.0; + init_msg.rear_axle.track_width = 29.0; + init_msg.rear_axle.wheel_diameter = 31.0; + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + EXPECT_AXLES_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_BoundingBox) +{ + traffic_simulator_msgs::msg::BoundingBox src_msg; + traffic_simulator_msgs::BoundingBox dst_proto; + src_msg.center.x = 3.0; + src_msg.center.y = 5.0; + src_msg.center.z = 7.0; + src_msg.dimensions.x = 11.0; + src_msg.dimensions.y = 13.0; + src_msg.dimensions.z = 17.0; + simulation_interface::toProto(src_msg, dst_proto); + EXPECT_BOUNDING_BOX_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_BoundingBox) +{ + traffic_simulator_msgs::msg::BoundingBox init_msg; + traffic_simulator_msgs::BoundingBox src_proto; + traffic_simulator_msgs::msg::BoundingBox dst_msg; + init_msg.center.x = 3.0; + init_msg.center.y = 5.0; + init_msg.center.z = 7.0; + init_msg.dimensions.x = 11.0; + init_msg.dimensions.y = 13.0; + init_msg.dimensions.z = 17.0; + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + EXPECT_BOUNDING_BOX_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_VehicleParameters) +{ + traffic_simulator_msgs::msg::VehicleParameters src_msg; + traffic_simulator_msgs::VehicleParameters dst_proto; + src_msg.name = "test"; + traffic_simulator_msgs::msg::BoundingBox box_msg; + box_msg.center.x = 3.0; + box_msg.center.y = 5.0; + box_msg.center.z = 7.0; + box_msg.dimensions.x = 11.0; + box_msg.dimensions.y = 13.0; + box_msg.dimensions.z = 17.0; + src_msg.bounding_box = box_msg; + traffic_simulator_msgs::msg::Performance performance_msg; + performance_msg.max_speed = 19.0; + performance_msg.max_deceleration = 23.0; + src_msg.performance = performance_msg; + traffic_simulator_msgs::msg::Axle axle_msg; + axle_msg.max_steering = 29.0; + axle_msg.position_x = 31.0; + axle_msg.position_z = 37.0; + axle_msg.track_width = 41.0; + axle_msg.wheel_diameter = 43.0; + src_msg.axles.front_axle = axle_msg; + src_msg.axles.rear_axle = axle_msg; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_VEHICLE_PARAMETERS_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_VehicleParameters) +{ + traffic_simulator_msgs::msg::VehicleParameters init_msg; + traffic_simulator_msgs::VehicleParameters src_proto; + traffic_simulator_msgs::msg::VehicleParameters dst_msg; + init_msg.name = "test"; + traffic_simulator_msgs::msg::BoundingBox box_msg; + box_msg.center.x = 3.0; + box_msg.center.y = 5.0; + box_msg.center.z = 7.0; + box_msg.dimensions.x = 11.0; + box_msg.dimensions.y = 13.0; + box_msg.dimensions.z = 17.0; + init_msg.bounding_box = box_msg; + traffic_simulator_msgs::msg::Performance performance_msg; + performance_msg.max_speed = 19.0; + performance_msg.max_deceleration = 23.0; + init_msg.performance = performance_msg; + traffic_simulator_msgs::msg::Axle axle_msg; + axle_msg.max_steering = 29.0; + axle_msg.position_x = 31.0; + axle_msg.position_z = 37.0; + axle_msg.track_width = 41.0; + axle_msg.wheel_diameter = 43.0; + init_msg.axles.front_axle = axle_msg; + init_msg.axles.rear_axle = axle_msg; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_VEHICLE_PARAMETERS_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_PedestrianParameters) +{ + traffic_simulator_msgs::msg::PedestrianParameters src_msg; + traffic_simulator_msgs::PedestrianParameters dst_proto; + src_msg.name = "test"; + traffic_simulator_msgs::msg::BoundingBox box_msg; + box_msg.center.x = 3.0; + box_msg.center.y = 5.0; + box_msg.center.z = 7.0; + box_msg.dimensions.x = 11.0; + box_msg.dimensions.y = 13.0; + box_msg.dimensions.z = 17.0; + src_msg.bounding_box = box_msg; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_PEDESTRIAN_PARAMETERS_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_PedestrianParameters) +{ + traffic_simulator_msgs::msg::PedestrianParameters init_msg; + traffic_simulator_msgs::PedestrianParameters src_proto; + traffic_simulator_msgs::msg::PedestrianParameters dst_msg; + init_msg.name = "test"; + traffic_simulator_msgs::msg::BoundingBox box_msg; + box_msg.center.x = 3.0; + box_msg.center.y = 5.0; + box_msg.center.z = 7.0; + box_msg.dimensions.x = 11.0; + box_msg.dimensions.y = 13.0; + box_msg.dimensions.z = 17.0; + init_msg.bounding_box = box_msg; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_PEDESTRIAN_PARAMETERS_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_MiscObjectParameters) +{ + traffic_simulator_msgs::msg::MiscObjectParameters src_msg; + traffic_simulator_msgs::MiscObjectParameters dst_proto; + traffic_simulator_msgs::msg::BoundingBox box_msg; + box_msg.center.x = 3.0; + box_msg.center.y = 5.0; + box_msg.center.z = 7.0; + box_msg.dimensions.x = 11.0; + box_msg.dimensions.y = 13.0; + box_msg.dimensions.z = 17.0; + src_msg.bounding_box = box_msg; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_MISC_OBJECT_PARAMETERS_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_MiscObjectParameters) +{ + traffic_simulator_msgs::msg::MiscObjectParameters init_msg; + traffic_simulator_msgs::MiscObjectParameters src_proto; + traffic_simulator_msgs::msg::MiscObjectParameters dst_msg; + traffic_simulator_msgs::msg::BoundingBox box_msg; + box_msg.center.x = 3.0; + box_msg.center.y = 5.0; + box_msg.center.z = 7.0; + box_msg.dimensions.x = 11.0; + box_msg.dimensions.y = 13.0; + box_msg.dimensions.z = 17.0; + init_msg.bounding_box = box_msg; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_MISC_OBJECT_PARAMETERS_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_ActionStatus) +{ + traffic_simulator_msgs::msg::ActionStatus src_msg; + traffic_simulator_msgs::ActionStatus dst_proto; + + src_msg.current_action = "test"; + src_msg.twist.linear.x = 3.0; + src_msg.twist.linear.y = 5.0; + src_msg.twist.linear.z = 7.0; + src_msg.twist.angular.x = 11.0; + src_msg.twist.angular.y = 13.0; + src_msg.twist.angular.z = 17.0; + + src_msg.accel.linear.x = 19.0; + src_msg.accel.linear.y = 23.0; + src_msg.accel.linear.z = 29.0; + src_msg.accel.angular.x = 31.0; + src_msg.accel.angular.y = 37.0; + src_msg.accel.angular.z = 41.0; + + simulation_interface::toProto(src_msg, dst_proto); + + EXPECT_ACTION_STATUS_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_ActionStatus) +{ + traffic_simulator_msgs::msg::ActionStatus init_msg; + traffic_simulator_msgs::ActionStatus src_proto; + traffic_simulator_msgs::msg::ActionStatus dst_msg; + + init_msg.current_action = "test"; + init_msg.twist.linear.x = 3.0; + init_msg.twist.linear.y = 5.0; + init_msg.twist.linear.z = 7.0; + init_msg.twist.angular.x = 11.0; + init_msg.twist.angular.y = 13.0; + init_msg.twist.angular.z = 17.0; + + init_msg.accel.linear.x = 19.0; + init_msg.accel.linear.y = 23.0; + init_msg.accel.linear.z = 29.0; + init_msg.accel.angular.x = 31.0; + init_msg.accel.angular.y = 37.0; + init_msg.accel.angular.z = 41.0; + + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + + EXPECT_ACTION_STATUS_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_EntityStatus) +{ + traffic_simulator_msgs::msg::EntityStatus src_msg; + traffic_simulator_msgs::EntityStatus dst_proto; + + src_msg.name = "test"; + src_msg.time = 3.0; + + traffic_simulator_msgs::msg::BoundingBox box_msg; + box_msg.center.x = 5.0; + box_msg.center.y = 7.0; + box_msg.center.z = 11.0; + box_msg.dimensions.x = 13.0; + box_msg.dimensions.y = 17.0; + box_msg.dimensions.z = 19.0; + src_msg.bounding_box = box_msg; + + traffic_simulator_msgs::msg::ActionStatus action_msg; + action_msg.current_action = "test"; + action_msg.twist.linear.x = 23.0; + action_msg.twist.linear.y = 29.0; + action_msg.twist.linear.z = 31.0; + action_msg.twist.angular.x = 37.0; + action_msg.twist.angular.y = 41.0; + action_msg.twist.angular.z = 43.0; + action_msg.accel.linear.x = 47.0; + action_msg.accel.linear.y = 53.0; + action_msg.accel.linear.z = 59.0; + action_msg.accel.angular.x = 61.0; + action_msg.accel.angular.y = 67.0; + action_msg.accel.angular.z = 71.0; + src_msg.action_status = action_msg; + + geometry_msgs::msg::Pose pose_msg; + pose_msg.position.x = 73.0; + pose_msg.position.y = 79.0; + pose_msg.position.z = 83.0; + pose_msg.orientation.x = 89.0; + pose_msg.orientation.y = 97.0; + pose_msg.orientation.z = 101.0; + pose_msg.orientation.w = 103.0; + src_msg.pose = pose_msg; + + traffic_simulator_msgs::msg::LaneletPose lanelet_pose_msg; + lanelet_pose_msg.lanelet_id = 107; + lanelet_pose_msg.s = 109.0; + lanelet_pose_msg.offset = 113.0; + lanelet_pose_msg.rpy.x = 127.0; + lanelet_pose_msg.rpy.y = 131.0; + lanelet_pose_msg.rpy.z = 137.0; + src_msg.lanelet_pose = lanelet_pose_msg; + src_msg.lanelet_pose_valid = true; + + simulation_interface::toProto(src_msg, dst_proto); + + EXPECT_ENTITY_STATUS_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_EntityStatus) +{ + traffic_simulator_msgs::msg::EntityStatus init_msg; + traffic_simulator_msgs::EntityStatus src_proto; + traffic_simulator_msgs::msg::EntityStatus dst_msg; + + init_msg.name = "test"; + init_msg.time = 3.0; + + traffic_simulator_msgs::msg::BoundingBox box_msg; + box_msg.center.x = 5.0; + box_msg.center.y = 7.0; + box_msg.center.z = 11.0; + box_msg.dimensions.x = 13.0; + box_msg.dimensions.y = 17.0; + box_msg.dimensions.z = 19.0; + init_msg.bounding_box = box_msg; + + traffic_simulator_msgs::msg::ActionStatus action_msg; + action_msg.current_action = "test"; + action_msg.twist.linear.x = 23.0; + action_msg.twist.linear.y = 29.0; + action_msg.twist.linear.z = 31.0; + action_msg.twist.angular.x = 37.0; + action_msg.twist.angular.y = 41.0; + action_msg.twist.angular.z = 43.0; + action_msg.accel.linear.x = 47.0; + action_msg.accel.linear.y = 53.0; + action_msg.accel.linear.z = 59.0; + action_msg.accel.angular.x = 61.0; + action_msg.accel.angular.y = 67.0; + action_msg.accel.angular.z = 71.0; + init_msg.action_status = action_msg; + + geometry_msgs::msg::Pose pose_msg; + pose_msg.position.x = 73.0; + pose_msg.position.y = 79.0; + pose_msg.position.z = 83.0; + pose_msg.orientation.x = 89.0; + pose_msg.orientation.y = 97.0; + pose_msg.orientation.z = 101.0; + pose_msg.orientation.w = 103.0; + init_msg.pose = pose_msg; + + traffic_simulator_msgs::msg::LaneletPose lanelet_pose_msg; + lanelet_pose_msg.lanelet_id = 107; + lanelet_pose_msg.s = 109.0; + lanelet_pose_msg.offset = 113.0; + lanelet_pose_msg.rpy.x = 127.0; + lanelet_pose_msg.rpy.y = 131.0; + lanelet_pose_msg.rpy.z = 137.0; + init_msg.lanelet_pose = lanelet_pose_msg; + init_msg.lanelet_pose_valid = true; + + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + + EXPECT_ENTITY_STATUS_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_SentEntityStatus) +{ + traffic_simulator_msgs::msg::EntityStatus src_msg; + simulation_api_schema::EntityStatus dst_proto; + + src_msg.name = "test"; + src_msg.time = 3.0; + + traffic_simulator_msgs::msg::ActionStatus action_msg; + action_msg.current_action = "test"; + action_msg.twist.linear.x = 5.0; + action_msg.twist.linear.y = 7.0; + action_msg.twist.linear.z = 11.0; + action_msg.twist.angular.x = 13.0; + action_msg.twist.angular.y = 17.0; + action_msg.twist.angular.z = 19.0; + action_msg.accel.linear.x = 23.0; + action_msg.accel.linear.y = 29.0; + action_msg.accel.linear.z = 31.0; + action_msg.accel.angular.x = 37.0; + action_msg.accel.angular.y = 41.0; + action_msg.accel.angular.z = 43.0; + src_msg.action_status = action_msg; + + geometry_msgs::msg::Pose pose_msg; + pose_msg.position.x = 47.0; + pose_msg.position.y = 53.0; + pose_msg.position.z = 59.0; + pose_msg.orientation.x = 61.0; + pose_msg.orientation.y = 67.0; + pose_msg.orientation.z = 71.0; + pose_msg.orientation.w = 73.0; + src_msg.pose = pose_msg; + + simulation_interface::toProto(src_msg, dst_proto); + + EXPECT_SENT_ENTITY_STATUS_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_SentEntityStatus) +{ + traffic_simulator_msgs::msg::EntityStatus init_msg; + simulation_api_schema::EntityStatus src_proto; + traffic_simulator_msgs::msg::EntityStatus dst_msg; + + init_msg.name = "test"; + init_msg.time = 3.0; + + traffic_simulator_msgs::msg::ActionStatus action_msg; + action_msg.current_action = "test"; + action_msg.twist.linear.x = 5.0; + action_msg.twist.linear.y = 7.0; + action_msg.twist.linear.z = 11.0; + action_msg.twist.angular.x = 13.0; + action_msg.twist.angular.y = 17.0; + action_msg.twist.angular.z = 19.0; + action_msg.accel.linear.x = 23.0; + action_msg.accel.linear.y = 29.0; + action_msg.accel.linear.z = 31.0; + action_msg.accel.angular.x = 37.0; + action_msg.accel.angular.y = 41.0; + action_msg.accel.angular.z = 43.0; + init_msg.action_status = action_msg; + + geometry_msgs::msg::Pose pose_msg; + pose_msg.position.x = 47.0; + pose_msg.position.y = 53.0; + pose_msg.position.z = 59.0; + pose_msg.orientation.x = 61.0; + pose_msg.orientation.y = 67.0; + pose_msg.orientation.z = 71.0; + pose_msg.orientation.w = 73.0; + init_msg.pose = pose_msg; + + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + + EXPECT_SENT_ENTITY_STATUS_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_Time) +{ + builtin_interfaces::msg::Time src_msg; + builtin_interfaces::Time dst_proto; + + src_msg.nanosec = 3; + src_msg.sec = 5; + + simulation_interface::toProto(src_msg, dst_proto); + + EXPECT_TIME_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_Time) +{ + builtin_interfaces::msg::Time init_msg; + builtin_interfaces::Time src_proto; + builtin_interfaces::msg::Time dst_msg; + + init_msg.nanosec = 3; + init_msg.sec = 5; + + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + + EXPECT_TIME_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_Duration) +{ + builtin_interfaces::msg::Duration src_msg; + builtin_interfaces::Duration dst_proto; + + src_msg.nanosec = 3; + src_msg.sec = 5; + + simulation_interface::toProto(src_msg, dst_proto); + + EXPECT_DURATION_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_Duration) +{ + builtin_interfaces::msg::Duration init_msg; + builtin_interfaces::Duration src_proto; + builtin_interfaces::msg::Duration dst_msg; + + init_msg.nanosec = 3; + init_msg.sec = 5; + + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + + EXPECT_DURATION_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_Header) +{ + std_msgs::msg::Header src_msg; + std_msgs::Header dst_proto; + + src_msg.frame_id = "test"; + src_msg.stamp.nanosec = 3; + src_msg.stamp.sec = 5; + + simulation_interface::toProto(src_msg, dst_proto); + + EXPECT_HEADER_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_Header) +{ + std_msgs::msg::Header init_msg; + std_msgs::Header src_proto; + std_msgs::msg::Header dst_msg; + + init_msg.frame_id = "test"; + init_msg.stamp.nanosec = 3; + init_msg.stamp.sec = 5; + + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + + EXPECT_HEADER_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_Clock) +{ + rosgraph_msgs::msg::Clock src_msg; + rosgraph_msgs::Clock dst_proto; + + src_msg.clock.nanosec = 3; + src_msg.clock.sec = 5; + + simulation_interface::toProto(src_msg, dst_proto); + + EXPECT_CLOCK_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_Clock) +{ + rosgraph_msgs::msg::Clock init_msg; + rosgraph_msgs::Clock src_proto; + rosgraph_msgs::msg::Clock dst_msg; + + init_msg.clock.nanosec = 3; + init_msg.clock.sec = 5; + + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + + EXPECT_CLOCK_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_AckermannControlCommand) +{ + autoware_auto_control_msgs::msg::AckermannControlCommand src_msg; + autoware_auto_control_msgs::AckermannControlCommand dst_proto; + + src_msg.longitudinal.acceleration = 3.0; + src_msg.lateral.steering_tire_angle = 5.0; + src_msg.lateral.steering_tire_rotation_rate = 7.0; + src_msg.longitudinal.speed = 11.0; + + simulation_interface::toProto(src_msg, dst_proto); + + EXPECT_CONTROL_COMMAND_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_AckermannControlCommand) +{ + autoware_auto_control_msgs::msg::AckermannControlCommand init_msg; + autoware_auto_control_msgs::AckermannControlCommand src_proto; + autoware_auto_control_msgs::msg::AckermannControlCommand dst_msg; + + init_msg.longitudinal.acceleration = 3.0; + init_msg.lateral.steering_tire_angle = 5.0; + init_msg.lateral.steering_tire_rotation_rate = 7.0; + init_msg.longitudinal.speed = 11.0; + + simulation_interface::toProto(init_msg, src_proto); + simulation_interface::toMsg(src_proto, dst_msg); + + EXPECT_CONTROL_COMMAND_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_EntityTypeEgo) +{ + traffic_simulator_msgs::msg::EntityType src_msg; + traffic_simulator_msgs::EntityType dst_proto; + src_msg.type = traffic_simulator_msgs::msg::EntityType::EGO; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.type(), traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_EGO); +} + +TEST(Conversion, toMsg_EntityTypeEgo) +{ + traffic_simulator_msgs::msg::EntityType init_msg; + traffic_simulator_msgs::EntityType src_proto; + traffic_simulator_msgs::msg::EntityType dst_msg; + init_msg.type = traffic_simulator_msgs::msg::EntityType::EGO; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.type, traffic_simulator_msgs::msg::EntityType::EGO); +} + +TEST(Conversion, toProto_EntityTypeVehicle) +{ + traffic_simulator_msgs::msg::EntityType src_msg; + traffic_simulator_msgs::EntityType dst_proto; + src_msg.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.type(), traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_VEHICLE); +} + +TEST(Conversion, toMsg_EntityTypeVehicle) +{ + traffic_simulator_msgs::msg::EntityType init_msg; + traffic_simulator_msgs::EntityType src_proto; + traffic_simulator_msgs::msg::EntityType dst_msg; + init_msg.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.type, traffic_simulator_msgs::msg::EntityType::VEHICLE); +} + +TEST(Conversion, toProto_EntityTypePedestrian) +{ + traffic_simulator_msgs::msg::EntityType src_msg; + traffic_simulator_msgs::EntityType dst_proto; + src_msg.type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.type(), traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_PEDESTRIAN); +} + +TEST(Conversion, toMsg_EntityTypePedestrian) +{ + traffic_simulator_msgs::msg::EntityType init_msg; + traffic_simulator_msgs::EntityType src_proto; + traffic_simulator_msgs::msg::EntityType dst_msg; + init_msg.type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.type, traffic_simulator_msgs::msg::EntityType::PEDESTRIAN); +} + +TEST(Conversion, toProto_EntityTypeMiscObject) +{ + traffic_simulator_msgs::msg::EntityType src_msg; + traffic_simulator_msgs::EntityType dst_proto; + src_msg.type = traffic_simulator_msgs::msg::EntityType::MISC_OBJECT; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.type(), traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_MISC_OBJECT); +} + +TEST(Conversion, toMsg_EntityTypeMiscObject) +{ + traffic_simulator_msgs::msg::EntityType init_msg; + traffic_simulator_msgs::EntityType src_proto; + traffic_simulator_msgs::msg::EntityType dst_msg; + init_msg.type = traffic_simulator_msgs::msg::EntityType::MISC_OBJECT; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.type, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT); +} + +TEST(Conversion, toProto_EntitySubtypeUnknown) +{ + traffic_simulator_msgs::msg::EntitySubtype src_msg; + traffic_simulator_msgs::EntitySubtype dst_proto; + src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ( + dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_UNKNOWN); +} + +TEST(Conversion, toMsg_EntitySubtypeUnknown) +{ + traffic_simulator_msgs::msg::EntitySubtype init_msg; + traffic_simulator_msgs::EntitySubtype src_proto; + traffic_simulator_msgs::msg::EntitySubtype dst_msg; + init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN); +} + +TEST(Conversion, toProto_EntitySubtypeCar) +{ + traffic_simulator_msgs::msg::EntitySubtype src_msg; + traffic_simulator_msgs::EntitySubtype dst_proto; + src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR); +} + +TEST(Conversion, toMsg_EntitySubtypeCar) +{ + traffic_simulator_msgs::msg::EntitySubtype init_msg; + traffic_simulator_msgs::EntitySubtype src_proto; + traffic_simulator_msgs::msg::EntitySubtype dst_msg; + init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::CAR); +} + +TEST(Conversion, toProto_EntitySubtypeTruck) +{ + traffic_simulator_msgs::msg::EntitySubtype src_msg; + traffic_simulator_msgs::EntitySubtype dst_proto; + src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::TRUCK; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ( + dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRUCK); +} + +TEST(Conversion, toMsg_EntitySubtypeTruck) +{ + traffic_simulator_msgs::msg::EntitySubtype init_msg; + traffic_simulator_msgs::EntitySubtype src_proto; + traffic_simulator_msgs::msg::EntitySubtype dst_msg; + init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::TRUCK; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::TRUCK); +} + +TEST(Conversion, toProto_EntitySubtypeBus) +{ + traffic_simulator_msgs::msg::EntitySubtype src_msg; + traffic_simulator_msgs::EntitySubtype dst_proto; + src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::BUS; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BUS); +} + +TEST(Conversion, toMsg_EntitySubtypeBus) +{ + traffic_simulator_msgs::msg::EntitySubtype init_msg; + traffic_simulator_msgs::EntitySubtype src_proto; + traffic_simulator_msgs::msg::EntitySubtype dst_msg; + init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::BUS; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::BUS); +} + +TEST(Conversion, toProto_EntitySubtypeTrailer) +{ + traffic_simulator_msgs::msg::EntitySubtype src_msg; + traffic_simulator_msgs::EntitySubtype dst_proto; + src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::TRAILER; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ( + dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRAILER); +} + +TEST(Conversion, toMsg_EntitySubtypeTrailer) +{ + traffic_simulator_msgs::msg::EntitySubtype init_msg; + traffic_simulator_msgs::EntitySubtype src_proto; + traffic_simulator_msgs::msg::EntitySubtype dst_msg; + init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::TRAILER; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::TRAILER); +} + +TEST(Conversion, toProto_EntitySubtypeMotorcycle) +{ + traffic_simulator_msgs::msg::EntitySubtype src_msg; + traffic_simulator_msgs::EntitySubtype dst_proto; + src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::MOTORCYCLE; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ( + dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_MOTORCYCLE); +} + +TEST(Conversion, toMsg_EntitySubtypeMotorcycle) +{ + traffic_simulator_msgs::msg::EntitySubtype init_msg; + traffic_simulator_msgs::EntitySubtype src_proto; + traffic_simulator_msgs::msg::EntitySubtype dst_msg; + init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::MOTORCYCLE; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::MOTORCYCLE); +} + +TEST(Conversion, toProto_EntitySubtypeBicycle) +{ + traffic_simulator_msgs::msg::EntitySubtype src_msg; + traffic_simulator_msgs::EntitySubtype dst_proto; + src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::BICYCLE; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ( + dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BICYCLE); +} + +TEST(Conversion, toMsg_EntitySubtypeBicycle) +{ + traffic_simulator_msgs::msg::EntitySubtype init_msg; + traffic_simulator_msgs::EntitySubtype src_proto; + traffic_simulator_msgs::msg::EntitySubtype dst_msg; + init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::BICYCLE; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::BICYCLE); +} + +TEST(Conversion, toProto_EntitySubtypePedestrian) +{ + traffic_simulator_msgs::msg::EntitySubtype src_msg; + traffic_simulator_msgs::EntitySubtype dst_proto; + src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::PEDESTRIAN; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ( + dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_PEDESTRIAN); +} + +TEST(Conversion, toMsg_EntitySubtypePedestrian) +{ + traffic_simulator_msgs::msg::EntitySubtype init_msg; + traffic_simulator_msgs::EntitySubtype src_proto; + traffic_simulator_msgs::msg::EntitySubtype dst_msg; + init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::PEDESTRIAN; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::PEDESTRIAN); +} + +TEST(Conversion, toProto_LaneletPose) +{ + traffic_simulator_msgs::msg::LaneletPose src_msg; + traffic_simulator_msgs::LaneletPose dst_proto; + src_msg.lanelet_id = 3; + src_msg.s = 5.0; + src_msg.offset = 7.0; + src_msg.rpy.x = 11.0; + src_msg.rpy.y = 13.0; + src_msg.rpy.z = 17.0; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_LANELET_POSE_EQ(src_msg, dst_proto); +} + +TEST(Conversion, toMsg_LaneletPose) +{ + traffic_simulator_msgs::msg::LaneletPose init_msg; + traffic_simulator_msgs::LaneletPose src_proto; + traffic_simulator_msgs::msg::LaneletPose dst_msg; + init_msg.lanelet_id = 3; + init_msg.s = 5.0; + init_msg.offset = 7.0; + init_msg.rpy.x = 11.0; + init_msg.rpy.y = 13.0; + init_msg.rpy.z = 17.0; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_LANELET_POSE_EQ(dst_msg, src_proto); +} + +TEST(Conversion, toProto_GearCommandNone) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::NONE; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::NONE); +} + +TEST(Conversion, toMsg_GearCommandNone) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::NONE; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::NONE); +} + +TEST(Conversion, toProto_GearCommandNeutral) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::NEUTRAL; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::NEUTRAL); +} + +TEST(Conversion, toMsg_GearCommandNeutral) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::NEUTRAL; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::NEUTRAL); +} + +TEST(Conversion, toProto_GearCommandDrive) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE); +} + +TEST(Conversion, toMsg_GearCommandDrive) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE); +} + +TEST(Conversion, toProto_GearCommandDrive2) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_2; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_2); +} + +TEST(Conversion, toMsg_GearCommandDrive2) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_2; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_2); +} + +TEST(Conversion, toProto_GearCommandDrive3) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_3; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_3); +} + +TEST(Conversion, toMsg_GearCommandDrive3) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_3; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_3); +} + +TEST(Conversion, toProto_GearCommandDrive4) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_4; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_4); +} + +TEST(Conversion, toMsg_GearCommandDrive4) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_4; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_4); +} + +TEST(Conversion, toProto_GearCommandDrive5) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_5; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_5); +} + +TEST(Conversion, toMsg_GearCommandDrive5) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_5; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_5); +} + +TEST(Conversion, toProto_GearCommandDrive6) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_6; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_6); +} + +TEST(Conversion, toMsg_GearCommandDrive6) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_6; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_6); +} + +TEST(Conversion, toProto_GearCommandDrive7) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_7; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_7); +} + +TEST(Conversion, toMsg_GearCommandDrive7) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_7; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_7); +} + +TEST(Conversion, toProto_GearCommandDrive8) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_8; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_8); +} + +TEST(Conversion, toMsg_GearCommandDrive8) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_8; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_8); +} + +TEST(Conversion, toProto_GearCommandDrive9) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_9; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_9); +} + +TEST(Conversion, toMsg_GearCommandDrive9) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_9; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_9); +} + +TEST(Conversion, toProto_GearCommandDrive10) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_10; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_10); +} + +TEST(Conversion, toMsg_GearCommandDrive10) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_10; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_10); +} + +TEST(Conversion, toProto_GearCommandDrive11) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_11; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_11); +} + +TEST(Conversion, toMsg_GearCommandDrive11) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_11; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_11); +} + +TEST(Conversion, toProto_GearCommandDrive12) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_12; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_12); +} + +TEST(Conversion, toMsg_GearCommandDrive12) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_12; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_12); +} + +TEST(Conversion, toProto_GearCommandDrive13) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_13; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_13); +} + +TEST(Conversion, toMsg_GearCommandDrive13) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_13; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_13); +} + +TEST(Conversion, toProto_GearCommandDrive14) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_14; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_14); +} + +TEST(Conversion, toMsg_GearCommandDrive14) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_14; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_14); +} + +TEST(Conversion, toProto_GearCommandDrive15) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_15; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_15); +} + +TEST(Conversion, toMsg_GearCommandDrive15) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_15; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_15); +} + +TEST(Conversion, toProto_GearCommandDrive16) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_16; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_16); +} + +TEST(Conversion, toMsg_GearCommandDrive16) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_16; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_16); +} + +TEST(Conversion, toProto_GearCommandDrive17) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_17; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_17); +} + +TEST(Conversion, toMsg_GearCommandDrive17) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_17; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_17); +} + +TEST(Conversion, toProto_GearCommandDrive18) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_18; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_18); +} + +TEST(Conversion, toMsg_GearCommandDrive18) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_18; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_18); +} \ No newline at end of file diff --git a/simulation/simulation_interface/test/test_conversions.cpp b/simulation/simulation_interface/test/test_conversions.cpp deleted file mode 100644 index 0a7bf9bccb7..00000000000 --- a/simulation/simulation_interface/test/test_conversions.cpp +++ /dev/null @@ -1,534 +0,0 @@ -// 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 -#include - -#include "expect_equal_macros.hpp" -/** - * @brief Test cases - */ - -TEST(Conversion, Point) -{ - geometry_msgs::Point proto; - geometry_msgs::msg::Point p; - p.x = 1.0; - p.y = 2; - p.z = 3.1; - simulation_interface::toProto(p, proto); - EXPECT_POINT_EQ(p, proto); - p = geometry_msgs::msg::Point(); - EXPECT_DOUBLE_EQ(p.x, 0); - simulation_interface::toMsg(proto, p); - EXPECT_POINT_EQ(p, proto); -} - -TEST(Conversion, Quaternion) -{ - geometry_msgs::Quaternion proto; - geometry_msgs::msg::Quaternion q; - q.x = 1.0; - q.y = 2; - q.z = 3.1; - q.w = -10; - simulation_interface::toProto(q, proto); - EXPECT_QUATERNION_EQ(q, proto); - q = geometry_msgs::msg::Quaternion(); - EXPECT_DOUBLE_EQ(q.x, 0); - simulation_interface::toMsg(proto, q); - EXPECT_QUATERNION_EQ(q, proto); -} - -TEST(Conversion, Pose) -{ - geometry_msgs::Pose proto; - geometry_msgs::msg::Pose p; - p.position.x = 1.0; - p.position.y = 2; - p.position.z = 3.1; - p.orientation.x = 0; - p.orientation.y = 0; - p.orientation.z = 0; - p.orientation.w = 1; - simulation_interface::toProto(p, proto); - EXPECT_POSE_EQ(p, proto); - p = geometry_msgs::msg::Pose(); - EXPECT_DOUBLE_EQ(p.position.x, 0); - simulation_interface::toMsg(proto, p); - EXPECT_POSE_EQ(p, proto); -} - -TEST(Conversion, Vector) -{ - geometry_msgs::Vector3 proto; - geometry_msgs::msg::Vector3 vec; - vec.x = 1; - vec.y = 2; - vec.z = 3.0; - simulation_interface::toProto(vec, proto); - EXPECT_VECTOR3_EQ(vec, proto); - vec = geometry_msgs::msg::Vector3(); - EXPECT_DOUBLE_EQ(vec.x, 0); - simulation_interface::toMsg(proto, vec); - EXPECT_VECTOR3_EQ(vec, proto); -} - -TEST(Conversion, Twist) -{ - geometry_msgs::Twist proto; - geometry_msgs::msg::Twist twist; - twist.linear.x = 1; - twist.linear.y = 2; - twist.linear.z = 3.0; - simulation_interface::toProto(twist, proto); - EXPECT_TWIST_EQ(twist, proto); - twist = geometry_msgs::msg::Twist(); - EXPECT_DOUBLE_EQ(twist.linear.x, 0); - simulation_interface::toMsg(proto, twist); - EXPECT_TWIST_EQ(twist, proto); -} - -TEST(Conversion, Accel) -{ - geometry_msgs::Accel proto; - geometry_msgs::msg::Accel accel; - accel.linear.x = 1; - accel.linear.y = 2; - accel.linear.z = 3.0; - simulation_interface::toProto(accel, proto); - EXPECT_ACCEL_EQ(accel, proto); - accel = geometry_msgs::msg::Accel(); - EXPECT_DOUBLE_EQ(accel.linear.x, 0); - simulation_interface::toMsg(proto, accel); - EXPECT_ACCEL_EQ(accel, proto); -} - -TEST(Conversion, Performance) -{ - traffic_simulator_msgs::Performance proto; - traffic_simulator_msgs::msg::Performance performance; - performance.max_speed = 10; - performance.max_deceleration = 3; - simulation_interface::toProto(performance, proto); - EXPECT_PERFORMANCE_EQ(performance, proto); - EXPECT_DOUBLE_EQ(performance.max_acceleration, proto.max_acceleration()); - EXPECT_DOUBLE_EQ(performance.max_deceleration, proto.max_deceleration()); - EXPECT_DOUBLE_EQ(performance.max_speed, proto.max_speed()); - performance = traffic_simulator_msgs::msg::Performance(); - EXPECT_DOUBLE_EQ(performance.max_speed, 30.0); - simulation_interface::toMsg(proto, performance); - EXPECT_PERFORMANCE_EQ(performance, proto); -} - -TEST(Conversion, Axle) -{ - traffic_simulator_msgs::Axle proto; - traffic_simulator_msgs::msg::Axle axle; - axle.max_steering = 30; - axle.position_x = 3; - axle.position_z = 14; - axle.track_width = -10; - axle.wheel_diameter = 53; - simulation_interface::toProto(axle, proto); - EXPECT_AXLE_EQ(axle, proto); - axle = traffic_simulator_msgs::msg::Axle(); - EXPECT_DOUBLE_EQ(axle.max_steering, 0); - simulation_interface::toMsg(proto, axle); - EXPECT_AXLE_EQ(axle, proto); -} - -TEST(Conversion, Axles) -{ - traffic_simulator_msgs::Axles proto; - traffic_simulator_msgs::msg::Axles axles; - axles.front_axle.max_steering = 3; - axles.front_axle.position_x = 35; - axles.front_axle.position_z = 234; - axles.front_axle.track_width = 1; - axles.front_axle.wheel_diameter = 123; - axles.rear_axle.max_steering = 13; - axles.rear_axle.position_x = 3; - axles.rear_axle.position_z = 23; - axles.rear_axle.track_width = 14; - axles.rear_axle.wheel_diameter = 122; - simulation_interface::toProto(axles, proto); - EXPECT_AXLES_EQ(axles, proto); - axles = traffic_simulator_msgs::msg::Axles(); - EXPECT_DOUBLE_EQ(axles.front_axle.max_steering, 0); - simulation_interface::toMsg(proto, axles); - EXPECT_AXLES_EQ(axles, proto); -} - -TEST(Conversion, BoundingBox) -{ - traffic_simulator_msgs::BoundingBox proto; - traffic_simulator_msgs::msg::BoundingBox box; - box.center.x = 1.0; - box.center.y = 1.23; - box.center.z = 43.0; - box.dimensions.x = 12.3; - box.dimensions.y = 3.9; - box.dimensions.z = 4.0; - simulation_interface::toProto(box, proto); - EXPECT_BOUNDING_BOX_EQ(box, proto); - box = traffic_simulator_msgs::msg::BoundingBox(); - EXPECT_DOUBLE_EQ(box.center.x, 0); - simulation_interface::toMsg(proto, box); - EXPECT_BOUNDING_BOX_EQ(box, proto); -} - -TEST(Conversion, VehicleParameters) -{ - traffic_simulator_msgs::VehicleParameters proto; - traffic_simulator_msgs::msg::VehicleParameters p; - p.name = "foo"; - traffic_simulator_msgs::msg::BoundingBox box; - box.center.x = 1.0; - box.center.y = 1.23; - box.center.z = 43.0; - box.dimensions.x = 12.3; - box.dimensions.y = 3.9; - box.dimensions.z = 4.0; - p.bounding_box = box; - traffic_simulator_msgs::msg::Performance performance; - performance.max_speed = 10; - performance.max_deceleration = 3; - p.performance = performance; - traffic_simulator_msgs::msg::Axle axle; - axle.max_steering = 30; - axle.position_x = 3; - axle.position_z = 14; - axle.track_width = -10; - axle.wheel_diameter = 53; - p.axles.front_axle = axle; - p.axles.rear_axle = axle; - EXPECT_NO_THROW(simulation_interface::toProto(p, proto)); - EXPECT_VEHICLE_PARAMETERS_EQ(p, proto); - p = traffic_simulator_msgs::msg::VehicleParameters(); - EXPECT_DOUBLE_EQ(p.bounding_box.dimensions.x, 0); - EXPECT_NO_THROW(simulation_interface::toMsg(proto, p)); - EXPECT_VEHICLE_PARAMETERS_EQ(p, proto); -} - -TEST(Conversion, PedestrianParameters) -{ - traffic_simulator_msgs::PedestrianParameters proto; - traffic_simulator_msgs::msg::PedestrianParameters p; - p.name = "foo"; - traffic_simulator_msgs::msg::BoundingBox box; - box.center.x = 1.0; - box.center.y = 1.23; - box.center.z = 43.0; - box.dimensions.x = 12.3; - box.dimensions.y = 3.9; - box.dimensions.z = 4.0; - p.bounding_box = box; - EXPECT_NO_THROW(simulation_interface::toProto(p, proto)); - EXPECT_PEDESTRIAN_PARAMETERS_EQ(p, proto); - p = traffic_simulator_msgs::msg::PedestrianParameters(); - EXPECT_DOUBLE_EQ(p.bounding_box.dimensions.x, 0); - EXPECT_NO_THROW(simulation_interface::toMsg(proto, p)); - EXPECT_PEDESTRIAN_PARAMETERS_EQ(p, proto); -} - -TEST(Conversion, MiscObjectParameters) -{ - traffic_simulator_msgs::MiscObjectParameters proto; - traffic_simulator_msgs::msg::MiscObjectParameters p; - traffic_simulator_msgs::msg::BoundingBox box; - box.center.x = 1.0; - box.center.y = 1.23; - box.center.z = 43.0; - box.dimensions.x = 12.3; - box.dimensions.y = 3.9; - box.dimensions.z = 4.0; - p.bounding_box = box; - EXPECT_NO_THROW(simulation_interface::toProto(p, proto)); - EXPECT_MISC_OBJECT_PARAMETERS_EQ(p, proto); - EXPECT_NO_THROW(simulation_interface::toMsg(proto, p)); - EXPECT_MISC_OBJECT_PARAMETERS_EQ(p, proto); -} - -TEST(Conversion, ActionStatus) -{ - traffic_simulator_msgs::ActionStatus proto; - traffic_simulator_msgs::msg::ActionStatus action; - action.current_action = "test"; - action.twist.linear.x = 1.0; - action.twist.linear.y = 2.0; - action.twist.linear.z = 3.0; - action.twist.angular.x = -20; - action.twist.angular.y = -4.2; - action.twist.angular.z = 9; - action.accel.linear.x = 3.0; - action.accel.linear.y = 908; - action.accel.linear.z = 987.0; - action.accel.angular.x = 0.3; - action.accel.angular.y = 0.5; - action.accel.angular.z = 98; - simulation_interface::toProto(action, proto); - EXPECT_ACTION_STATUS_EQ(action, proto); - action = traffic_simulator_msgs::msg::ActionStatus(); - EXPECT_DOUBLE_EQ(action.twist.linear.x, 0); - simulation_interface::toMsg(proto, action); - EXPECT_ACTION_STATUS_EQ(action, proto); -} - -TEST(Conversion, EntityStatus) -{ - traffic_simulator_msgs::EntityStatus proto; - traffic_simulator_msgs::msg::EntityStatus status; - status.name = "test"; - status.time = 3.0; - traffic_simulator_msgs::msg::BoundingBox box; - box.center.x = 1.0; - box.center.y = 1.23; - box.center.z = 43.0; - box.dimensions.x = 12.3; - box.dimensions.y = 3.9; - box.dimensions.z = 4.0; - status.bounding_box = box; - traffic_simulator_msgs::msg::ActionStatus action; - action.current_action = "test"; - action.twist.linear.x = 1.0; - action.twist.linear.y = 2.0; - action.twist.linear.z = 3.0; - action.twist.angular.x = -20; - action.twist.angular.y = -4.2; - action.twist.angular.z = 9; - action.accel.linear.x = 3.0; - action.accel.linear.y = 908; - action.accel.linear.z = 987.0; - action.accel.angular.x = 0.3; - action.accel.angular.y = 0.5; - action.accel.angular.z = 98; - status.action_status = action; - geometry_msgs::msg::Pose pose; - pose.position.x = 4.0; - pose.position.y = 1.2; - pose.position.z = 5.1; - pose.orientation.x = 0.3; - pose.orientation.y = 8.3; - pose.orientation.z = 9.3; - pose.orientation.w = 10.2; - status.pose = pose; - traffic_simulator_msgs::msg::LaneletPose lanelet_pose; - lanelet_pose.lanelet_id = 23; - lanelet_pose.s = 1.0; - lanelet_pose.offset = 3.5; - lanelet_pose.rpy.x = 3.4; - lanelet_pose.rpy.y = 5.1; - lanelet_pose.rpy.z = 1.3; - status.lanelet_pose = lanelet_pose; - status.lanelet_pose_valid = false; - simulation_interface::toProto(status, proto); - EXPECT_ENTITY_STATUS_EQ(status, proto); - status = traffic_simulator_msgs::msg::EntityStatus(); - EXPECT_TRUE(status.lanelet_pose_valid); - EXPECT_FALSE(proto.lanelet_pose_valid()); - simulation_interface::toMsg(proto, status); - EXPECT_ENTITY_STATUS_EQ(status, proto); - EXPECT_FALSE(status.lanelet_pose_valid); -} - -TEST(Conversion, SentEntityStatus) -{ - simulation_api_schema::EntityStatus proto; - traffic_simulator_msgs::msg::EntityStatus status; - status.name = "test"; - status.time = 3.0; - traffic_simulator_msgs::msg::ActionStatus action; - action.current_action = "test"; - action.twist.linear.x = 1.0; - action.twist.linear.y = 2.0; - action.twist.linear.z = 3.0; - action.twist.angular.x = -20; - action.twist.angular.y = -4.2; - action.twist.angular.z = 9; - action.accel.linear.x = 3.0; - action.accel.linear.y = 908; - action.accel.linear.z = 987.0; - action.accel.angular.x = 0.3; - action.accel.angular.y = 0.5; - action.accel.angular.z = 98; - status.action_status = action; - geometry_msgs::msg::Pose pose; - pose.position.x = 4.0; - pose.position.y = 1.2; - pose.position.z = 5.1; - pose.orientation.x = 0.3; - pose.orientation.y = 8.3; - pose.orientation.z = 9.3; - pose.orientation.w = 10.2; - status.pose = pose; - simulation_interface::toProto(status, proto); - EXPECT_SENT_ENTITY_STATUS_EQ(status, proto); - status = traffic_simulator_msgs::msg::EntityStatus(); - simulation_interface::toMsg(proto, status); - EXPECT_SENT_ENTITY_STATUS_EQ(status, proto); -} - -TEST(Conversion, Time) -{ - builtin_interfaces::Time proto; - builtin_interfaces::msg::Time msg; - msg.nanosec = 1; - msg.sec = 2; - simulation_interface::toProto(msg, proto); - EXPECT_TIME_EQ(msg, proto); - msg.nanosec = 0; - msg.sec = 0; - simulation_interface::toMsg(proto, msg); - EXPECT_TIME_EQ(msg, proto); -} - -TEST(Conversion, Duration) -{ - builtin_interfaces::Duration proto; - builtin_interfaces::msg::Duration msg; - msg.nanosec = 1; - msg.sec = 2; - simulation_interface::toProto(msg, proto); - EXPECT_DURATION_EQ(msg, proto); - msg.nanosec = 0; - msg.sec = 0; - simulation_interface::toMsg(proto, msg); - EXPECT_DURATION_EQ(msg, proto); -} - -TEST(Conversion, Header) -{ - std_msgs::Header proto; - std_msgs::msg::Header msg; - msg.frame_id = "base_link"; - msg.stamp.nanosec = 4; - msg.stamp.sec = 1; - simulation_interface::toProto(msg, proto); - EXPECT_HEADER_EQ(msg, proto); - msg.frame_id = ""; - msg.stamp.nanosec = 0; - msg.stamp.sec = 0; - simulation_interface::toMsg(proto, msg); - EXPECT_HEADER_EQ(msg, proto); -} - -TEST(Conversion, Clock) -{ - rosgraph_msgs::msg::Clock msg; - rosgraph_msgs::Clock proto; - msg.clock.nanosec = 12; - msg.clock.sec = 11; - simulation_interface::toProto(msg, proto); - EXPECT_CLOCK_EQ(msg, proto); - msg = rosgraph_msgs::msg::Clock(); - EXPECT_EQ(msg.clock.nanosec, static_cast(0)); - simulation_interface::toMsg(proto, msg); - EXPECT_CLOCK_EQ(msg, proto); -} - -TEST(Conversion, AckermannControlCommand) -{ - autoware_auto_control_msgs::AckermannControlCommand proto; - autoware_auto_control_msgs::msg::AckermannControlCommand msg; - msg.longitudinal.acceleration = 3; - msg.lateral.steering_tire_angle = 1.4; - msg.lateral.steering_tire_rotation_rate = 13.4; - msg.longitudinal.speed = 11.3; - simulation_interface::toProto(msg, proto); - EXPECT_CONTROL_COMMAND_EQ(msg, proto); - msg.longitudinal.acceleration = 0; - msg.lateral.steering_tire_angle = 0; - msg.lateral.steering_tire_rotation_rate = 0; - msg.longitudinal.speed = 0; - simulation_interface::toMsg(proto, msg); - EXPECT_CONTROL_COMMAND_EQ(msg, proto); -} - -TEST(Conversion, EntityType) -{ - traffic_simulator_msgs::EntityType proto; - traffic_simulator_msgs::msg::EntityType msg; - msg.type = msg.VEHICLE; - EXPECT_NO_THROW(simulation_interface::toProto(msg, proto)); - EXPECT_EQ(proto.type(), traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_VEHICLE); - msg.type = msg.MISC_OBJECT; - EXPECT_NO_THROW(simulation_interface::toProto(msg, proto)); - EXPECT_EQ(proto.type(), traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_MISC_OBJECT); - proto.set_type(traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_VEHICLE); - msg.type = msg.EGO; - EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); - EXPECT_EQ(msg.type, traffic_simulator_msgs::msg::EntityType::VEHICLE); - msg.type = msg.VEHICLE; - proto.set_type(traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_EGO); - EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); - EXPECT_EQ(msg.type, traffic_simulator_msgs::msg::EntityType::EGO); - msg.type = msg.VEHICLE; - proto.set_type(traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_PEDESTRIAN); - EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); - EXPECT_EQ(msg.type, traffic_simulator_msgs::msg::EntityType::PEDESTRIAN); - proto.set_type(traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_MISC_OBJECT); - EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); - EXPECT_EQ(msg.type, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT); -} - -TEST(Conversion, EntitySubtype) -{ - traffic_simulator_msgs::EntitySubtype proto; - traffic_simulator_msgs::msg::EntitySubtype msg; - msg.value = msg.CAR; - EXPECT_NO_THROW(simulation_interface::toProto(msg, proto)); - EXPECT_EQ(proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR); - msg.value = msg.UNKNOWN; - EXPECT_NO_THROW(simulation_interface::toProto(msg, proto)); - EXPECT_EQ(proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_UNKNOWN); - proto.set_value(traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR); - msg.value = msg.UNKNOWN; - EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); - EXPECT_EQ(msg.value, traffic_simulator_msgs::msg::EntitySubtype::CAR); - msg.value = msg.CAR; - proto.set_value(traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR); - EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); - EXPECT_EQ(msg.value, traffic_simulator_msgs::msg::EntitySubtype::CAR); - msg.value = msg.CAR; - proto.set_value(traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_PEDESTRIAN); - EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); - EXPECT_EQ(msg.value, traffic_simulator_msgs::msg::EntitySubtype::PEDESTRIAN); - proto.set_value(traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_UNKNOWN); - EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); - EXPECT_EQ(msg.value, traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN); -} - -TEST(Conversion, LaneletPose) -{ - traffic_simulator_msgs::msg::LaneletPose pose; - traffic_simulator_msgs::LaneletPose proto; - pose.lanelet_id = 23; - pose.s = 1.0; - pose.offset = 3.5; - pose.rpy.x = 3.4; - pose.rpy.y = 5.1; - pose.rpy.z = 1.3; - EXPECT_NO_THROW(simulation_interface::toProto(pose, proto)); - EXPECT_LANELET_POSE_EQ(pose, proto); - pose = traffic_simulator_msgs::msg::LaneletPose(); - EXPECT_NO_THROW(simulation_interface::toMsg(proto, pose)); - EXPECT_LANELET_POSE_EQ(pose, proto); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} From 2aae69e2882c4853337f8e9626a492e4df5dbdb8 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 10 Apr 2024 17:57:04 +0200 Subject: [PATCH 010/118] conversions cleanup --- .../test/src/test_conversions.cpp | 112 +++++++++++++++++- 1 file changed, 106 insertions(+), 6 deletions(-) diff --git a/simulation/simulation_interface/test/src/test_conversions.cpp b/simulation/simulation_interface/test/src/test_conversions.cpp index 6c7715e5e26..cf64821f647 100644 --- a/simulation/simulation_interface/test/src/test_conversions.cpp +++ b/simulation/simulation_interface/test/src/test_conversions.cpp @@ -105,7 +105,7 @@ TEST(Conversion, toMsg_Pose) EXPECT_POSE_EQ(dst_msg, src_proto); } -TEST(Conversion, toProto_Vector) +TEST(Conversion, toProto_Vector3) { geometry_msgs::msg::Vector3 src_msg; geometry_msgs::Vector3 dst_proto; @@ -116,7 +116,7 @@ TEST(Conversion, toProto_Vector) EXPECT_VECTOR3_EQ(src_msg, dst_proto); } -TEST(Conversion, toMsg_Vector) +TEST(Conversion, toMsg_Vector3) { geometry_msgs::msg::Vector3 init_msg; geometry_msgs::Vector3 src_proto; @@ -483,7 +483,7 @@ TEST(Conversion, toMsg_ActionStatus) EXPECT_ACTION_STATUS_EQ(dst_msg, src_proto); } -TEST(Conversion, toProto_EntityStatus) +TEST(Conversion, toProto_trafficSimulatorMsgsEntityStatus) { traffic_simulator_msgs::msg::EntityStatus src_msg; traffic_simulator_msgs::EntityStatus dst_proto; @@ -541,7 +541,7 @@ TEST(Conversion, toProto_EntityStatus) EXPECT_ENTITY_STATUS_EQ(src_msg, dst_proto); } -TEST(Conversion, toMsg_EntityStatus) +TEST(Conversion, toMsg_trafficSimulatorMsgsEntityStatus) { traffic_simulator_msgs::msg::EntityStatus init_msg; traffic_simulator_msgs::EntityStatus src_proto; @@ -601,7 +601,7 @@ TEST(Conversion, toMsg_EntityStatus) EXPECT_ENTITY_STATUS_EQ(dst_msg, src_proto); } -TEST(Conversion, toProto_SentEntityStatus) +TEST(Conversion, toProto_simulationApiSchemaEntityStatus) { traffic_simulator_msgs::msg::EntityStatus src_msg; simulation_api_schema::EntityStatus dst_proto; @@ -640,7 +640,7 @@ TEST(Conversion, toProto_SentEntityStatus) EXPECT_SENT_ENTITY_STATUS_EQ(src_msg, dst_proto); } -TEST(Conversion, toMsg_SentEntityStatus) +TEST(Conversion, toMsg_simulationApiSchemaEntityStatus) { traffic_simulator_msgs::msg::EntityStatus init_msg; simulation_api_schema::EntityStatus src_proto; @@ -1501,4 +1501,104 @@ TEST(Conversion, toMsg_GearCommandDrive18) EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_18); +} + +TEST(Conversion, toProto_GearCommandReverse) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::REVERSE); +} + +TEST(Conversion, toMsg_GearCommandReverse) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE); +} + +TEST(Conversion, toProto_GearCommandReverse2) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE_2; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::REVERSE_2); +} + +TEST(Conversion, toMsg_GearCommandReverse2) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE_2; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE_2); +} + +TEST(Conversion, toProto_GearCommandPark) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::PARK; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::PARK); +} + +TEST(Conversion, toMsg_GearCommandPark) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::PARK; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::PARK); +} + +TEST(Conversion, toProto_GearCommandLow) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::LOW; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::LOW); +} + +TEST(Conversion, toMsg_GearCommandLow) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::LOW; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::LOW); +} + +TEST(Conversion, toProto_GearCommandLow2) +{ + autoware_auto_vehicle_msgs::msg::GearCommand src_msg; + autoware_auto_vehicle_msgs::GearCommand dst_proto; + src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::LOW_2; + EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); + EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::LOW_2); +} + +TEST(Conversion, toMsg_GearCommandLow2) +{ + autoware_auto_vehicle_msgs::msg::GearCommand init_msg; + autoware_auto_vehicle_msgs::GearCommand src_proto; + autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; + init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::LOW_2; + EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); + EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); + EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::LOW_2); } \ No newline at end of file From 3b2a3aaf2ce3cbe863c76be7d15504d705dd41a5 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 10 Apr 2024 17:57:59 +0200 Subject: [PATCH 011/118] traffic_light status shape color tests --- .../src/traffic_lights/test_traffic_light.cpp | 235 +++++++++++++++++- 1 file changed, 232 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp index dfe01019f5e..008ce432760 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp @@ -20,6 +20,12 @@ #include #include +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + TEST(TrafficLight, Color) { using Color = traffic_simulator::TrafficLight::Color; @@ -367,8 +373,231 @@ TEST(TrafficLight, TrafficLight) } } -int main(int argc, char ** argv) +TEST(TrafficLight, Color_make) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + using Color = traffic_simulator::TrafficLight::Color; + { + const auto color = Color::make("green"); + + EXPECT_TRUE(color == Color::green); + EXPECT_TRUE(color.is(Color::green)); + EXPECT_TRUE(boost::lexical_cast("green") == Color::green); + EXPECT_TRUE(boost::lexical_cast(color) == "green"); + } + + { + const auto color = Color::make("yellow"); + + EXPECT_TRUE(color == Color::yellow); + EXPECT_TRUE(color.is(Color::yellow)); + EXPECT_TRUE(boost::lexical_cast("yellow") == Color::yellow); + EXPECT_TRUE(boost::lexical_cast(color) == "yellow"); + } + + { + const auto color = Color::make("red"); + + EXPECT_TRUE(color == Color::red); + EXPECT_TRUE(color.is(Color::red)); + EXPECT_TRUE(boost::lexical_cast("red") == Color::red); + EXPECT_TRUE(boost::lexical_cast(color) == "red"); + } + + { + const auto color = Color::make("white"); + + EXPECT_TRUE(color == Color::white); + EXPECT_TRUE(color.is(Color::white)); + EXPECT_TRUE(boost::lexical_cast("white") == Color::white); + EXPECT_TRUE(boost::lexical_cast(color) == "white"); + } + + { + const auto color = Color::make("amber"); + + EXPECT_TRUE(color == Color::yellow); + EXPECT_TRUE(color.is(Color::yellow)); + EXPECT_TRUE(boost::lexical_cast("amber") == Color::yellow); + EXPECT_TRUE(boost::lexical_cast(color) == "yellow"); + } +} + +TEST(TrafficLight, Shape_make) +{ + using Shape = traffic_simulator::TrafficLight::Shape; + + { + const auto shape = Shape::make("circle"); + + EXPECT_TRUE(shape == Shape::circle); + EXPECT_TRUE(shape.is(Shape::circle)); + EXPECT_TRUE(shape.is(Shape::Category::circle)); + EXPECT_TRUE(shape.category() == Shape::Category::circle); + EXPECT_TRUE(boost::lexical_cast("circle") == Shape::circle); + EXPECT_TRUE(boost::lexical_cast(shape) == "circle"); + } + + { + const auto shape = Shape::make("cross"); + + EXPECT_TRUE(shape == Shape::cross); + EXPECT_TRUE(shape.is(Shape::cross)); + EXPECT_TRUE(shape.is(Shape::Category::cross)); + EXPECT_TRUE(shape.category() == Shape::Category::cross); + EXPECT_TRUE(boost::lexical_cast("cross") == Shape::cross); + EXPECT_TRUE(boost::lexical_cast(shape) == "cross"); + } + + { + const auto shape = Shape::make("left"); + + EXPECT_TRUE(shape == Shape::left); + EXPECT_TRUE(shape.is(Shape::left)); + EXPECT_TRUE(shape.is(Shape::Category::arrow)); + EXPECT_TRUE(shape.category() == Shape::Category::arrow); + EXPECT_TRUE(boost::lexical_cast("left") == Shape::left); + EXPECT_TRUE(boost::lexical_cast(shape) == "left"); + } + + { + const auto shape = Shape::make("down"); + + EXPECT_TRUE(shape == Shape::down); + EXPECT_TRUE(shape.is(Shape::down)); + EXPECT_TRUE(shape.is(Shape::Category::arrow)); + EXPECT_TRUE(shape.category() == Shape::Category::arrow); + EXPECT_TRUE(boost::lexical_cast("down") == Shape::down); + EXPECT_TRUE(boost::lexical_cast(shape) == "down"); + } + + { + const auto shape = Shape::make("up"); + + EXPECT_TRUE(shape == Shape::up); + EXPECT_TRUE(shape.is(Shape::up)); + EXPECT_TRUE(shape.is(Shape::Category::arrow)); + EXPECT_TRUE(shape.category() == Shape::Category::arrow); + EXPECT_TRUE(boost::lexical_cast("up") == Shape::up); + EXPECT_TRUE(boost::lexical_cast(shape) == "up"); + } + + { + const auto shape = Shape::make("right"); + + EXPECT_TRUE(shape == Shape::right); + EXPECT_TRUE(shape.is(Shape::right)); + EXPECT_TRUE(shape.is(Shape::Category::arrow)); + EXPECT_TRUE(shape.category() == Shape::Category::arrow); + EXPECT_TRUE(boost::lexical_cast("right") == Shape::right); + EXPECT_TRUE(boost::lexical_cast(shape) == "right"); + } + + { + const auto shape = Shape::make("lowerLeft"); + + EXPECT_TRUE(shape == Shape::lower_left); + EXPECT_TRUE(shape.is(Shape::lower_left)); + EXPECT_TRUE(shape.is(Shape::Category::arrow)); + EXPECT_TRUE(shape.category() == Shape::Category::arrow); + EXPECT_TRUE(boost::lexical_cast("lowerLeft") == Shape::lower_left); + EXPECT_TRUE(boost::lexical_cast(shape) == "lowerLeft"); + } + + { + const auto shape = Shape::make("upperLeft"); + + EXPECT_TRUE(shape == Shape::upper_left); + EXPECT_TRUE(shape.is(Shape::upper_left)); + EXPECT_TRUE(shape.is(Shape::Category::arrow)); + EXPECT_TRUE(shape.category() == Shape::Category::arrow); + EXPECT_TRUE(boost::lexical_cast("upperLeft") == Shape::upper_left); + EXPECT_TRUE(boost::lexical_cast(shape) == "upperLeft"); + } + + { + const auto shape = Shape::make("lowerRight"); + + EXPECT_TRUE(shape == Shape::lower_right); + EXPECT_TRUE(shape.is(Shape::lower_right)); + EXPECT_TRUE(shape.is(Shape::Category::arrow)); + EXPECT_TRUE(shape.category() == Shape::Category::arrow); + EXPECT_TRUE(boost::lexical_cast("lowerRight") == Shape::lower_right); + EXPECT_TRUE(boost::lexical_cast(shape) == "lowerRight"); + } + + { + const auto shape = Shape::make("upperRight"); + + EXPECT_TRUE(shape == Shape::upper_right); + EXPECT_TRUE(shape.is(Shape::upper_right)); + EXPECT_TRUE(shape.is(Shape::Category::arrow)); + EXPECT_TRUE(shape.category() == Shape::Category::arrow); + EXPECT_TRUE(boost::lexical_cast("upperRight") == Shape::upper_right); + EXPECT_TRUE(boost::lexical_cast(shape) == "upperRight"); + } +} + +TEST(TrafficLight, Status_make) +{ + using Status = traffic_simulator::TrafficLight::Status; + + { + const auto status = Status::make("solidOn"); + + EXPECT_TRUE(status == Status::solid_on); + EXPECT_TRUE(status.is(Status::solid_on)); + EXPECT_TRUE(boost::lexical_cast("solidOn") == Status::solid_on); + EXPECT_TRUE(boost::lexical_cast(status) == "solidOn"); + } + + { + const auto status = Status::make("solidOff"); + + EXPECT_TRUE(status == Status::solid_off); + EXPECT_TRUE(status.is(Status::solid_off)); + EXPECT_TRUE(boost::lexical_cast("solidOff") == Status::solid_off); + EXPECT_TRUE(boost::lexical_cast(status) == "solidOff"); + } + + { + const auto status = Status::make("flashing"); + + EXPECT_TRUE(status == Status::flashing); + EXPECT_TRUE(status.is(Status::flashing)); + EXPECT_TRUE(boost::lexical_cast("flashing") == Status::flashing); + EXPECT_TRUE(boost::lexical_cast(status) == "flashing"); + } + + { + const auto status = Status::make("unknown"); + + EXPECT_TRUE(status == Status::unknown); + EXPECT_TRUE(status.is(Status::unknown)); + EXPECT_TRUE(boost::lexical_cast("unknown") == Status::unknown); + EXPECT_TRUE(boost::lexical_cast(status) == "unknown"); + } +} + +TEST(TrafficLight, Color_make_wrong) +{ + using Color = traffic_simulator::TrafficLight::Color; + Color color; + EXPECT_THROW(color = Color::make("karmazynowy"), std::runtime_error); + EXPECT_FALSE(boost::lexical_cast(color) == "karmazynowy"); +} + +TEST(TrafficLight, Shape_make_wrong) +{ + using Shape = traffic_simulator::TrafficLight::Shape; + Shape shape; + EXPECT_THROW(shape = Shape::make("dwunastoscian foremny"), std::runtime_error); + EXPECT_FALSE(boost::lexical_cast(shape) == "dwunastoscian foremny"); } + +TEST(TrafficLight, Status_make_wrong) +{ + using Status = traffic_simulator::TrafficLight::Status; + Status status; + EXPECT_THROW(status = Status::make("quo"), std::runtime_error); + EXPECT_FALSE(boost::lexical_cast(status) == "quo"); +} \ No newline at end of file From ad29c422efbd14bf8bfec61e1fb5cffd99317b65 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 11 Apr 2024 09:09:30 +0200 Subject: [PATCH 012/118] traffic light bulb tests --- .../src/traffic_lights/test_traffic_light.cpp | 186 +++++++++++++++--- 1 file changed, 163 insertions(+), 23 deletions(-) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp index 008ce432760..f7e9d22b32c 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp @@ -241,26 +241,26 @@ TEST(TrafficLight, Bulb) using Bulb = TrafficLight::Bulb; // clang-format off - static_assert(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); - static_assert(Bulb(Color::yellow, Status::solid_on, Shape::circle ).hash() == 0b0000'0001'0000'0000'0000'0000'0000'0000); - static_assert(Bulb(Color::red, Status::solid_on, Shape::circle ).hash() == 0b0000'0010'0000'0000'0000'0000'0000'0000); - static_assert(Bulb(Color::white, Status::solid_on, Shape::circle ).hash() == 0b0000'0011'0000'0000'0000'0000'0000'0000); - - static_assert(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); - static_assert(Bulb(Color::green, Status::solid_off, Shape::circle ).hash() == 0b0000'0000'0000'0001'0000'0000'0000'0000); - static_assert(Bulb(Color::green, Status::flashing, Shape::circle ).hash() == 0b0000'0000'0000'0010'0000'0000'0000'0000); - static_assert(Bulb(Color::green, Status::unknown, Shape::circle ).hash() == 0b0000'0000'0000'0011'0000'0000'0000'0000); - - static_assert(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); - static_assert(Bulb(Color::green, Status::solid_on, Shape::cross ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0001); - static_assert(Bulb(Color::green, Status::solid_on, Shape::left ).hash() == 0b0000'0000'0000'0000'0000'1000'0000'0010); - static_assert(Bulb(Color::green, Status::solid_on, Shape::down ).hash() == 0b0000'0000'0000'0000'0000'0100'0000'0010); - static_assert(Bulb(Color::green, Status::solid_on, Shape::up ).hash() == 0b0000'0000'0000'0000'0000'0010'0000'0010); - static_assert(Bulb(Color::green, Status::solid_on, Shape::right ).hash() == 0b0000'0000'0000'0000'0000'0001'0000'0010); - static_assert(Bulb(Color::green, Status::solid_on, Shape::lower_left ).hash() == 0b0000'0000'0000'0000'0000'1100'0000'0010); - static_assert(Bulb(Color::green, Status::solid_on, Shape::upper_left ).hash() == 0b0000'0000'0000'0000'0000'1010'0000'0010); - static_assert(Bulb(Color::green, Status::solid_on, Shape::lower_right).hash() == 0b0000'0000'0000'0000'0000'0101'0000'0010); - static_assert(Bulb(Color::green, Status::solid_on, Shape::upper_right).hash() == 0b0000'0000'0000'0000'0000'0011'0000'0010); + EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); + EXPECT_TRUE(Bulb(Color::yellow, Status::solid_on, Shape::circle ).hash() == 0b0000'0001'0000'0000'0000'0000'0000'0000); + EXPECT_TRUE(Bulb(Color::red, Status::solid_on, Shape::circle ).hash() == 0b0000'0010'0000'0000'0000'0000'0000'0000); + EXPECT_TRUE(Bulb(Color::white, Status::solid_on, Shape::circle ).hash() == 0b0000'0011'0000'0000'0000'0000'0000'0000); + + EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); + EXPECT_TRUE(Bulb(Color::green, Status::solid_off, Shape::circle ).hash() == 0b0000'0000'0000'0001'0000'0000'0000'0000); + EXPECT_TRUE(Bulb(Color::green, Status::flashing, Shape::circle ).hash() == 0b0000'0000'0000'0010'0000'0000'0000'0000); + EXPECT_TRUE(Bulb(Color::green, Status::unknown, Shape::circle ).hash() == 0b0000'0000'0000'0011'0000'0000'0000'0000); + + EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); + EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::cross ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0001); + EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::left ).hash() == 0b0000'0000'0000'0000'0000'1000'0000'0010); + EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::down ).hash() == 0b0000'0000'0000'0000'0000'0100'0000'0010); + EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::up ).hash() == 0b0000'0000'0000'0000'0000'0010'0000'0010); + EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::right ).hash() == 0b0000'0000'0000'0000'0000'0001'0000'0010); + EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::lower_left ).hash() == 0b0000'0000'0000'0000'0000'1100'0000'0010); + EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::upper_left ).hash() == 0b0000'0000'0000'0000'0000'1010'0000'0010); + EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::lower_right).hash() == 0b0000'0000'0000'0000'0000'0101'0000'0010); + EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::upper_right).hash() == 0b0000'0000'0000'0000'0000'0011'0000'0010); // clang-format on { @@ -578,6 +578,135 @@ TEST(TrafficLight, Status_make) } } +TEST(TrafficLight, Bulb_make) +{ + using TrafficLight = traffic_simulator::TrafficLight; + using Color = TrafficLight::Color; + using Status = TrafficLight::Status; + using Shape = TrafficLight::Shape; + using Bulb = TrafficLight::Bulb; + { + constexpr auto bulb = Bulb(Color::red, Status::flashing, Shape::circle); + + EXPECT_TRUE(bulb.is(Color::red)); + EXPECT_TRUE(bulb.is(Status::flashing)); + EXPECT_TRUE(bulb.is(Shape::circle)); + EXPECT_TRUE(bulb.is(Shape::Category::circle)); + } + + { + constexpr auto bulb = Bulb(Color::green, Status::solid_on, Shape::right); + + EXPECT_TRUE(bulb.is(Color::green)); + EXPECT_TRUE(bulb.is(Status::solid_on)); + EXPECT_TRUE(bulb.is(Shape::right)); + EXPECT_TRUE(bulb.is(Shape::Category::arrow)); + } + + { + const auto bulb = Bulb("red flashing circle"); + + EXPECT_TRUE(bulb.is(Color::red)); + EXPECT_TRUE(bulb.is(Status::flashing)); + EXPECT_TRUE(bulb.is(Shape::circle)); + EXPECT_TRUE(bulb.is(Shape::Category::circle)); + } + + { + const auto bulb = Bulb("red flashing"); + + EXPECT_TRUE(bulb.is(Color::red)); + EXPECT_TRUE(bulb.is(Status::flashing)); + EXPECT_TRUE(bulb.is(Shape::circle)); + EXPECT_TRUE(bulb.is(Shape::Category::circle)); + } + + { + const auto bulb = Bulb("red"); + + EXPECT_TRUE(bulb.is(Color::red)); + EXPECT_TRUE(bulb.is(Status::solid_on)); + EXPECT_TRUE(bulb.is(Shape::circle)); + EXPECT_TRUE(bulb.is(Shape::Category::circle)); + } + + { + const auto bulb = Bulb("green solidOn right"); + + EXPECT_TRUE(bulb.is(Color::green)); + EXPECT_TRUE(bulb.is(Status::solid_on)); + EXPECT_TRUE(bulb.is(Shape::right)); + EXPECT_TRUE(bulb.is(Shape::Category::arrow)); + } + + { + const auto bulb = Bulb("green right"); + + EXPECT_TRUE(bulb.is(Color::green)); + EXPECT_TRUE(bulb.is(Status::solid_on)); + EXPECT_TRUE(bulb.is(Shape::right)); + EXPECT_TRUE(bulb.is(Shape::Category::arrow)); + } +} + +TEST(TrafficLight, Bulb_trafficLightMessageConversion) +{ + using TrafficLight = traffic_simulator::TrafficLight; + using Color = TrafficLight::Color; + using Status = TrafficLight::Status; + using Shape = TrafficLight::Shape; + using Bulb = TrafficLight::Bulb; + + { + constexpr auto bulb = Bulb(Color::red, Status::flashing, Shape::circle); + std::ostringstream oss; + oss << bulb; + EXPECT_TRUE(oss.str() == "red flashing circle"); + } + + { + constexpr auto bulb = Bulb(Color::green, Status::solid_on, Shape::right); + std::ostringstream oss; + oss << bulb; + EXPECT_TRUE(oss.str() == "green solidOn right"); + } + + { + const auto bulb = Bulb("red flashing circle"); + std::ostringstream oss; + oss << bulb; + EXPECT_TRUE(oss.str() == "red flashing circle"); + } + + { + const auto bulb = Bulb("red flashing"); + std::ostringstream oss; + oss << bulb; + EXPECT_TRUE(oss.str() == "red flashing circle"); + } + + { + const auto bulb = Bulb("red"); + std::ostringstream oss; + oss << bulb; + EXPECT_TRUE(oss.str() == "red solidOn circle"); + } + + { + const auto bulb = Bulb("green solidOn right"); + std::ostringstream oss; + oss << bulb; + EXPECT_TRUE(oss.str() == "green solidOn right"); + } + + { + const auto bulb = Bulb("green right"); + std::ostringstream oss; + oss << bulb; + EXPECT_TRUE(oss.str() == "green solidOn right"); + } +} + TEST(TrafficLight, Color_make_wrong) { using Color = traffic_simulator::TrafficLight::Color; @@ -598,6 +727,17 @@ TEST(TrafficLight, Status_make_wrong) { using Status = traffic_simulator::TrafficLight::Status; Status status; - EXPECT_THROW(status = Status::make("quo"), std::runtime_error); - EXPECT_FALSE(boost::lexical_cast(status) == "quo"); -} \ No newline at end of file + EXPECT_THROW(status = Status::make("ekonomiczny"), std::runtime_error); + EXPECT_FALSE(boost::lexical_cast(status) == "ekonomiczny"); +} + +TEST(TrafficLight, Bulb_make_wrong) +{ + using Bulb = traffic_simulator::TrafficLight::Bulb; + + EXPECT_THROW(Bulb("red flashing dwunastoscian foremny"), std::runtime_error); + EXPECT_THROW(Bulb("red ekonomiczny circle"), std::runtime_error); + EXPECT_THROW(Bulb("karmazynowy flashing circle"), std::runtime_error); + + EXPECT_THROW(Bulb("karmazynowy ekonomiczny dwunastoscian foremny"), std::runtime_error); +} From e9af35f5b623c65672f33220205367d44f2c257a Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 11 Apr 2024 13:14:10 +0200 Subject: [PATCH 013/118] vertex, toPoints --- .../src/sensor_simulation/primitives/test_vertex.cpp | 9 +++++++++ simulation/traffic_simulator/test/CMakeLists.txt | 1 + .../test/src/data_type/CMakeLists.txt | 2 ++ .../test/src/data_type/test_speed_change.cpp | 12 ++++++++++++ 4 files changed, 24 insertions(+) create mode 100644 simulation/traffic_simulator/test/src/data_type/CMakeLists.txt create mode 100644 simulation/traffic_simulator/test/src/data_type/test_speed_change.cpp diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp index f9bb369fa0c..2a235899afb 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp @@ -84,3 +84,12 @@ TEST(Vertex, toPoints_manyVertices) EXPECT_EQ(points[i].z, vertices[i].z); } } + +TEST(Vertex, toPoints_empty) +{ + std::vector vertices{}; + + std::vector points = simple_sensor_simulator::toPoints(vertices); + + EXPECT_EQ(points.size(), 0); +} diff --git a/simulation/traffic_simulator/test/CMakeLists.txt b/simulation/traffic_simulator/test/CMakeLists.txt index 911179a95a4..e9d0e5793d2 100644 --- a/simulation/traffic_simulator/test/CMakeLists.txt +++ b/simulation/traffic_simulator/test/CMakeLists.txt @@ -2,6 +2,7 @@ add_subdirectory(src/traffic_lights) add_subdirectory(src/helper) add_subdirectory(src/entity) add_subdirectory(src/behavior) +add_subdirectory(src/data_type) ament_add_gtest(test_hdmap_utils src/test_hdmap_utils.cpp) target_link_libraries(test_hdmap_utils traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/data_type/CMakeLists.txt b/simulation/traffic_simulator/test/src/data_type/CMakeLists.txt new file mode 100644 index 00000000000..92829140b08 --- /dev/null +++ b/simulation/traffic_simulator/test/src/data_type/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_speed_change test_speed_change.cpp) +target_link_libraries(test_speed_change traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/data_type/test_speed_change.cpp b/simulation/traffic_simulator/test/src/data_type/test_speed_change.cpp new file mode 100644 index 00000000000..a77a6c9ee27 --- /dev/null +++ b/simulation/traffic_simulator/test/src/data_type/test_speed_change.cpp @@ -0,0 +1,12 @@ +#include + +#include +#include +#include +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From bec64f22742555560dd66819b23e590c2a212e1c Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 11 Apr 2024 14:02:44 +0200 Subject: [PATCH 014/118] behavior getRequestString --- .../test/src/behavior/CMakeLists.txt | 3 ++ .../test/src/behavior/test_behavior.cpp | 49 +++++++++++++++++++ 2 files changed, 52 insertions(+) create mode 100644 simulation/traffic_simulator/test/src/behavior/test_behavior.cpp diff --git a/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt b/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt index 224ef897e54..a7d988d18b7 100644 --- a/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt @@ -1,2 +1,5 @@ ament_add_gtest(test_longitudinal_speed_planning test_longitudinal_speed_planning.cpp) target_link_libraries(test_longitudinal_speed_planning traffic_simulator) + +ament_add_gtest(test_behavior test_behavior.cpp) +target_link_libraries(test_behavior traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp b/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp new file mode 100644 index 00000000000..03a448df60d --- /dev/null +++ b/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp @@ -0,0 +1,49 @@ +#include + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST(Behavior, getRequestString_none) +{ + const auto req = traffic_simulator::behavior::Request::NONE; + std::string str; + EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); + EXPECT_TRUE("none" == str); +} + +TEST(Behavior, getRequestString_lane_change) +{ + const auto req = traffic_simulator::behavior::Request::LANE_CHANGE; + std::string str; + EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); + EXPECT_TRUE("lane_change" == str); +} + +TEST(Behavior, getRequestString_follow_lane) +{ + const auto req = traffic_simulator::behavior::Request::FOLLOW_LANE; + std::string str; + EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); + EXPECT_TRUE("follow_lane" == str); +} + +TEST(Behavior, getRequestString_follow_polyline_trajectory) +{ + const auto req = traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY; + std::string str; + EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); + EXPECT_TRUE("follow_polyline_trajectory" == str); +} + +TEST(Behavior, getRequestString_walk_straight) +{ + const auto req = traffic_simulator::behavior::Request::WALK_STRAIGHT; + std::string str; + EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); + EXPECT_TRUE("walk_straight" == str); +} \ No newline at end of file From 76d988d4ad321ca87cce5d8e19ad9a8f2d4a3293 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 11 Apr 2024 15:24:27 +0200 Subject: [PATCH 015/118] job and job_list unit tests --- .../include/traffic_simulator/job/job.hpp | 2 +- .../traffic_simulator/test/CMakeLists.txt | 1 + .../test/src/job/CMakeLists.txt | 5 + .../test/src/job/test_job.cpp | 28 ++++++ .../test/src/job/test_job_list.cpp | 98 +++++++++++++++++++ 5 files changed, 133 insertions(+), 1 deletion(-) create mode 100644 simulation/traffic_simulator/test/src/job/CMakeLists.txt create mode 100644 simulation/traffic_simulator/test/src/job/test_job.cpp create mode 100644 simulation/traffic_simulator/test/src/job/test_job_list.cpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/job/job.hpp b/simulation/traffic_simulator/include/traffic_simulator/job/job.hpp index 7731d376f91..e92fc9b2c7e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/job/job.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/job/job.hpp @@ -23,7 +23,7 @@ namespace traffic_simulator namespace job { enum class Type { - UNKOWN = 0, + UNKNOWN = 0, LINEAR_VELOCITY = 1, LINEAR_ACCELERATION = 2, STAND_STILL_DURATION = 3, diff --git a/simulation/traffic_simulator/test/CMakeLists.txt b/simulation/traffic_simulator/test/CMakeLists.txt index e9d0e5793d2..c49034e4784 100644 --- a/simulation/traffic_simulator/test/CMakeLists.txt +++ b/simulation/traffic_simulator/test/CMakeLists.txt @@ -3,6 +3,7 @@ add_subdirectory(src/helper) add_subdirectory(src/entity) add_subdirectory(src/behavior) add_subdirectory(src/data_type) +add_subdirectory(src/job) ament_add_gtest(test_hdmap_utils src/test_hdmap_utils.cpp) target_link_libraries(test_hdmap_utils traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/job/CMakeLists.txt b/simulation/traffic_simulator/test/src/job/CMakeLists.txt new file mode 100644 index 00000000000..8c52603c90a --- /dev/null +++ b/simulation/traffic_simulator/test/src/job/CMakeLists.txt @@ -0,0 +1,5 @@ +ament_add_gtest(test_job test_job.cpp) +target_link_libraries(test_job traffic_simulator) + +ament_add_gtest(test_job_list test_job_list.cpp) +target_link_libraries(test_job_list traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/job/test_job.cpp b/simulation/traffic_simulator/test/src/job/test_job.cpp new file mode 100644 index 00000000000..c68569d2ba1 --- /dev/null +++ b/simulation/traffic_simulator/test/src/job/test_job.cpp @@ -0,0 +1,28 @@ +#include + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST(Job, onUpdate) +{ + bool was_cleanup_func_called = false; + auto update_func = [](const double) { return true; }; + auto cleanup_func = [&was_cleanup_func_called]() + { + was_cleanup_func_called = true; + }; + auto type = traffic_simulator::job::Type::UNKNOWN; + auto event = traffic_simulator::job::Event::POST_UPDATE; + auto is_exclusive = true; + + auto job = traffic_simulator::job::Job(update_func, cleanup_func, type, is_exclusive, event); + const double step_time = 0.0; + job.onUpdate(step_time); + + EXPECT_TRUE(was_cleanup_func_called); +} \ No newline at end of file diff --git a/simulation/traffic_simulator/test/src/job/test_job_list.cpp b/simulation/traffic_simulator/test/src/job/test_job_list.cpp new file mode 100644 index 00000000000..31b1c67bfc3 --- /dev/null +++ b/simulation/traffic_simulator/test/src/job/test_job_list.cpp @@ -0,0 +1,98 @@ +#include + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST(JobList, append) +{ + bool was_cleanup_func_called = false; + auto update_func = [](const double) { return true; }; + auto cleanup_func = [&was_cleanup_func_called]() + { + was_cleanup_func_called = true; + }; + auto type = traffic_simulator::job::Type::UNKNOWN; + auto event = traffic_simulator::job::Event::POST_UPDATE; + auto is_exclusive = true; + + auto job_list = traffic_simulator::job::JobList(); + + job_list.append(update_func, cleanup_func, type, is_exclusive, event); + const double step_time = 0.0; + + job_list.update(step_time, event); + + EXPECT_TRUE(was_cleanup_func_called); +} + +TEST(JobList, append_doubled) +{ + bool first_cleanup = false; + bool first_update = false; + bool second_cleanup = false; + bool second_update = false; + + auto first_update_func = [&first_update](const double) { return first_update = true; }; + auto first_cleanup_func = [&first_cleanup]() { first_cleanup = true; }; + auto second_update_func = [&second_update](const double) { return second_update = true; }; + auto second_cleanup_func = [&second_cleanup]() { second_cleanup = true; }; + + auto type = traffic_simulator::job::Type::UNKNOWN; + auto event = traffic_simulator::job::Event::POST_UPDATE; + auto is_exclusive = true; + + auto job_list = traffic_simulator::job::JobList(); + + job_list.append(first_update_func, first_cleanup_func, type, is_exclusive, event); + job_list.append(second_update_func, second_cleanup_func, type, is_exclusive, event); + + const double step_time = 0.0; + job_list.update(step_time, event); + + EXPECT_TRUE(first_cleanup); + EXPECT_FALSE(first_update); + EXPECT_TRUE(second_cleanup); + EXPECT_TRUE(second_update); +} + +TEST(JobList, update) +{ + int update_count = 0; + int cleanup_count = 0; + + auto update_func = [&update_count](const double) { update_count++; return update_count >= 2; }; + auto cleanup_func = [&cleanup_count]() { cleanup_count++; }; + + auto type = traffic_simulator::job::Type::UNKNOWN; + auto event = traffic_simulator::job::Event::POST_UPDATE; + auto is_exclusive = true; + + auto job_list = traffic_simulator::job::JobList(); + + job_list.append(update_func, cleanup_func, type, is_exclusive, event); + + const double step_time = 0.0; + + EXPECT_EQ(0, update_count); + EXPECT_EQ(0, cleanup_count); + + job_list.update(step_time, event); + + EXPECT_EQ(1, update_count); + EXPECT_EQ(0, cleanup_count); + + job_list.update(step_time, event); + + EXPECT_EQ(2, update_count); + EXPECT_EQ(1, cleanup_count); + + job_list.update(step_time, event); + + EXPECT_EQ(2, update_count); + EXPECT_EQ(1, cleanup_count); +} \ No newline at end of file From 48febb24c3623465a6729f13df4d6d1f98e5f0d8 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 11 Apr 2024 15:30:12 +0200 Subject: [PATCH 016/118] make linter happy --- simulation/traffic_simulator/test/src/job/test_job.cpp | 9 +++------ .../traffic_simulator/test/src/job/test_job_list.cpp | 10 +++++----- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/simulation/traffic_simulator/test/src/job/test_job.cpp b/simulation/traffic_simulator/test/src/job/test_job.cpp index c68569d2ba1..589cf918151 100644 --- a/simulation/traffic_simulator/test/src/job/test_job.cpp +++ b/simulation/traffic_simulator/test/src/job/test_job.cpp @@ -12,17 +12,14 @@ TEST(Job, onUpdate) { bool was_cleanup_func_called = false; auto update_func = [](const double) { return true; }; - auto cleanup_func = [&was_cleanup_func_called]() - { - was_cleanup_func_called = true; - }; + auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; auto type = traffic_simulator::job::Type::UNKNOWN; auto event = traffic_simulator::job::Event::POST_UPDATE; auto is_exclusive = true; - + auto job = traffic_simulator::job::Job(update_func, cleanup_func, type, is_exclusive, event); const double step_time = 0.0; job.onUpdate(step_time); - + EXPECT_TRUE(was_cleanup_func_called); } \ No newline at end of file diff --git a/simulation/traffic_simulator/test/src/job/test_job_list.cpp b/simulation/traffic_simulator/test/src/job/test_job_list.cpp index 31b1c67bfc3..6f396fcde57 100644 --- a/simulation/traffic_simulator/test/src/job/test_job_list.cpp +++ b/simulation/traffic_simulator/test/src/job/test_job_list.cpp @@ -12,10 +12,7 @@ TEST(JobList, append) { bool was_cleanup_func_called = false; auto update_func = [](const double) { return true; }; - auto cleanup_func = [&was_cleanup_func_called]() - { - was_cleanup_func_called = true; - }; + auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; auto type = traffic_simulator::job::Type::UNKNOWN; auto event = traffic_simulator::job::Event::POST_UPDATE; auto is_exclusive = true; @@ -65,7 +62,10 @@ TEST(JobList, update) int update_count = 0; int cleanup_count = 0; - auto update_func = [&update_count](const double) { update_count++; return update_count >= 2; }; + auto update_func = [&update_count](const double) { + update_count++; + return update_count >= 2; + }; auto cleanup_func = [&cleanup_count]() { cleanup_count++; }; auto type = traffic_simulator::job::Type::UNKNOWN; From 56af13ba66511a8ef900b87eb7e94226b2f85dbe Mon Sep 17 00:00:00 2001 From: robomic Date: Fri, 12 Apr 2024 08:45:23 +0200 Subject: [PATCH 017/118] job accessor tests --- .../test/src/job/test_job.cpp | 57 +++++++++++++++++++ 1 file changed, 57 insertions(+) diff --git a/simulation/traffic_simulator/test/src/job/test_job.cpp b/simulation/traffic_simulator/test/src/job/test_job.cpp index 589cf918151..ae1a4dc1fc3 100644 --- a/simulation/traffic_simulator/test/src/job/test_job.cpp +++ b/simulation/traffic_simulator/test/src/job/test_job.cpp @@ -22,4 +22,61 @@ TEST(Job, onUpdate) job.onUpdate(step_time); EXPECT_TRUE(was_cleanup_func_called); +} + +TEST(Job, get_type) +{ + bool was_cleanup_func_called = false; + auto update_func = [](const double) { return true; }; + auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; + auto type = traffic_simulator::job::Type::UNKNOWN; + auto event = traffic_simulator::job::Event::POST_UPDATE; + auto is_exclusive = true; + + auto job = traffic_simulator::job::Job(update_func, cleanup_func, type, is_exclusive, event); + + EXPECT_TRUE(job.type == type); + + const double step_time = 0.0; + job.onUpdate(step_time); + + EXPECT_TRUE(job.type == type); +} + +TEST(Job, get_exclusive) +{ + bool was_cleanup_func_called = false; + auto update_func = [](const double) { return true; }; + auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; + auto type = traffic_simulator::job::Type::UNKNOWN; + auto event = traffic_simulator::job::Event::POST_UPDATE; + auto is_exclusive = true; + + auto job = traffic_simulator::job::Job(update_func, cleanup_func, type, is_exclusive, event); + + EXPECT_TRUE(job.exclusive == is_exclusive); + + const double step_time = 0.0; + job.onUpdate(step_time); + + EXPECT_TRUE(job.exclusive == is_exclusive); +} + +TEST(Job, get_event) +{ + bool was_cleanup_func_called = false; + auto update_func = [](const double) { return true; }; + auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; + auto type = traffic_simulator::job::Type::UNKNOWN; + auto event = traffic_simulator::job::Event::POST_UPDATE; + auto is_exclusive = true; + + auto job = traffic_simulator::job::Job(update_func, cleanup_func, type, is_exclusive, event); + + EXPECT_TRUE(job.event == event); + + const double step_time = 0.0; + job.onUpdate(step_time); + + EXPECT_TRUE(job.event == event); } \ No newline at end of file From 56e31fe703ec700e718a2535f01b8b68071705bd Mon Sep 17 00:00:00 2001 From: robomic Date: Fri, 12 Apr 2024 14:06:15 +0200 Subject: [PATCH 018/118] entity_base dummy class with a couple of tests --- .../test/src/entity/CMakeLists.txt | 3 + .../test/src/entity/test_entity_base.cpp | 229 ++++++++++++++++++ 2 files changed, 232 insertions(+) create mode 100644 simulation/traffic_simulator/test/src/entity/test_entity_base.cpp diff --git a/simulation/traffic_simulator/test/src/entity/CMakeLists.txt b/simulation/traffic_simulator/test/src/entity/CMakeLists.txt index 0e6b9eee47d..ac7189815db 100644 --- a/simulation/traffic_simulator/test/src/entity/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/entity/CMakeLists.txt @@ -1,2 +1,5 @@ ament_add_gtest(test_vehicle_entity test_vehicle_entity.cpp) target_link_libraries(test_vehicle_entity traffic_simulator) + +ament_add_gtest(test_entity_base test_entity_base.cpp) +target_link_libraries(test_entity_base traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp new file mode 100644 index 00000000000..a5add1f24eb --- /dev/null +++ b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp @@ -0,0 +1,229 @@ +#include + +#include +#include +#include +#include + +#include "../catalogs.hpp" +#include "../expect_eq_macros.hpp" + +class DummyEntity : public traffic_simulator::entity::EntityBase +{ +public: + explicit DummyEntity( + const std::string & name, const traffic_simulator::CanonicalizedEntityStatus & entity_status, + const std::shared_ptr & hdmap_utils_ptr) + : EntityBase(name, entity_status, hdmap_utils_ptr) + { + } + + void onUpdate(double, double) override {} + + auto getCurrentAction() const -> std::string override { return {}; } + + auto getDefaultDynamicConstraints() const + -> const traffic_simulator_msgs::msg::DynamicConstraints & override + { + static const auto default_dynamic_constraints = []() { + auto dynamic_constraints = traffic_simulator_msgs::msg::DynamicConstraints(); + dynamic_constraints.max_speed = 0.0; + dynamic_constraints.max_acceleration = 0.0; + dynamic_constraints.max_acceleration_rate = 0.0; + dynamic_constraints.max_deceleration = 0.0; + dynamic_constraints.max_deceleration_rate = 0.0; + return dynamic_constraints; + }(); + + return default_dynamic_constraints; + } + + auto getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & override + { + static traffic_simulator_msgs::msg::EntityType type; + type.type = traffic_simulator_msgs::msg::EntityType::MISC_OBJECT; + return type; + } + + auto getEntityTypename() const -> const std::string & override + { + static const auto str = std::string("dummy"); + return str; + } + + ~DummyEntity() override = default; + + auto getGoalPoses() -> std::vector override { return {}; } + + std::optional getObstacle() override + { + return std::nullopt; + } + + auto getRouteLanelets(double) -> lanelet::Ids override + { + THROW_SEMANTIC_ERROR("getRouteLanelets function cannot not use in MiscObjectEntity"); + } + + auto fillLaneletPose(traffic_simulator::CanonicalizedEntityStatus &) -> void override {} + + auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override + { + return traffic_simulator_msgs::msg::WaypointsArray(); + } + + void requestSpeedChange(double, bool) override {} + + void requestSpeedChange(const traffic_simulator::speed_change::RelativeTargetSpeed &, bool) override {} + + void requestAssignRoute(const std::vector &) override {} + + void requestAssignRoute(const std::vector &) override {} + + void requestAcquirePosition(const traffic_simulator::CanonicalizedLaneletPose &) override {} + + void requestAcquirePosition(const geometry_msgs::msg::Pose &) override {} + + void requestSpeedChange( + const double, const traffic_simulator::speed_change::Transition, const traffic_simulator::speed_change::Constraint, + const bool) override {} + + auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter override + { + THROW_SEMANTIC_ERROR("getBehaviorParameter function does not support in MiscObjectEntity."); + } + + void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) override {} + + void setVelocityLimit(double) override {} + + void setAccelerationLimit(double) override {} + + void setAccelerationRateLimit(double) override {} + + void setDecelerationLimit(double) override {} + + void setDecelerationRateLimit(double) override {} +}; + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +/* + std::string path = + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; + geographic_msgs::msg::GeoPoint origin; + origin.latitude = 35.61836750154; + origin.longitude = 139.78066608243; + auto map = std::make_shared(path, origin); +*/ + +TEST(EntityBase, asFieldOperatorApplication) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + + EXPECT_THROW(dummy.asFieldOperatorApplication(), std::runtime_error); +} + +TEST(EntityBase, startNpcLogic) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + + EXPECT_FALSE(dummy.isNpcLogicStarted()); + dummy.startNpcLogic(); + EXPECT_TRUE(dummy.isNpcLogicStarted()); +} + +TEST(EntityBase, activateOutOfRangeJob_speed) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + + double min_velocity = 0.0; + double max_velocity = 0.0; + double min_acceleration = -100.0; + double max_acceleration = 100.0; + double min_jerk = -100.0; + double max_jerk = 100.0; + double velocity = 1.0; + dummy.setLinearVelocity(velocity); + dummy.activateOutOfRangeJob(min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); + double current_time = 0.0; + double step_time = 0.0; + EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); + EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), std::runtime_error); +} + +TEST(EntityBase, activateOutOfRangeJob_acceleration) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + + double min_velocity = -100.0; + double max_velocity = 100.0; + double min_acceleration = 0.0; + double max_acceleration = 0.0; + double min_jerk = -100.0; + double max_jerk = 100.0; + double acceleration = 1.0; + dummy.setLinearAcceleration(acceleration); + dummy.activateOutOfRangeJob(min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); + double current_time = 0.0; + double step_time = 0.0; + EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); + EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), std::runtime_error); +} + +TEST(EntityBase, activateOutOfRangeJob_jerk) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + + double min_velocity = -100.0; + double max_velocity = 100.0; + double min_acceleration = -100.0; + double max_acceleration = 100.0; + double min_jerk = 0.0; + double max_jerk = 0.0; + double jerk = 1.0; + dummy.setLinearJerk(jerk); + dummy.activateOutOfRangeJob(min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); + double current_time = 0.0; + double step_time = 0.0; + EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); + EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), std::runtime_error); +} \ No newline at end of file From 2a230dae80b22c50c349cf1f0777e46fb382db09 Mon Sep 17 00:00:00 2001 From: robomic Date: Fri, 12 Apr 2024 14:57:42 +0200 Subject: [PATCH 019/118] EntityBase onUpdate. onPostUpdate tests --- .../test/src/entity/test_entity_base.cpp | 87 ++++++++++++++++++- 1 file changed, 86 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp index a5add1f24eb..374f9420ee4 100644 --- a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp @@ -18,7 +18,12 @@ class DummyEntity : public traffic_simulator::entity::EntityBase { } - void onUpdate(double, double) override {} + void appendToJobList(const std::function & func_on_update, + const std::function & func_on_cleanup, traffic_simulator::job::Type type, bool exclusive, + const traffic_simulator::job::Event event) + { + job_list_.append(func_on_update, func_on_cleanup, type, exclusive, event); + } auto getCurrentAction() const -> std::string override { return {}; } @@ -226,4 +231,84 @@ TEST(EntityBase, activateOutOfRangeJob_jerk) double step_time = 0.0; EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), std::runtime_error); +} + +TEST(EntityBase, onUpdate) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + + bool first_cleanup = false; + bool first_update = false; + bool second_cleanup = false; + bool second_update = false; + + auto first_update_func = [&first_update](const double) { return first_update = true; }; + auto first_cleanup_func = [&first_cleanup]() { first_cleanup = true; }; + auto second_update_func = [&second_update](const double) { return second_update = true; }; + auto second_cleanup_func = [&second_cleanup]() { second_cleanup = true; }; + + auto type_first = traffic_simulator::job::Type::LINEAR_VELOCITY; + auto type_second = traffic_simulator::job::Type::LINEAR_ACCELERATION; + auto first_event = traffic_simulator::job::Event::PRE_UPDATE; + auto second_event = traffic_simulator::job::Event::POST_UPDATE; + auto is_exclusive = true; + + dummy.appendToJobList(first_update_func, first_cleanup_func, type_first, is_exclusive, first_event); + dummy.appendToJobList(second_update_func, second_cleanup_func, type_second, is_exclusive, second_event); + + double current_time = 0.0; + double step_time = 0.0; + dummy.onUpdate(current_time, step_time); + + EXPECT_TRUE(first_cleanup); + EXPECT_TRUE(first_update); + EXPECT_FALSE(second_cleanup); + EXPECT_FALSE(second_update); +} + +TEST(EntityBase, onPostUpdate) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + + bool first_cleanup = false; + bool first_update = false; + bool second_cleanup = false; + bool second_update = false; + + auto first_update_func = [&first_update](const double) { return first_update = true; }; + auto first_cleanup_func = [&first_cleanup]() { first_cleanup = true; }; + auto second_update_func = [&second_update](const double) { return second_update = true; }; + auto second_cleanup_func = [&second_cleanup]() { second_cleanup = true; }; + + auto type_first = traffic_simulator::job::Type::LINEAR_VELOCITY; + auto type_second = traffic_simulator::job::Type::LINEAR_ACCELERATION; + auto first_event = traffic_simulator::job::Event::PRE_UPDATE; + auto second_event = traffic_simulator::job::Event::POST_UPDATE; + auto is_exclusive = true; + + dummy.appendToJobList(first_update_func, first_cleanup_func, type_first, is_exclusive, first_event); + dummy.appendToJobList(second_update_func, second_cleanup_func, type_second, is_exclusive, second_event); + + double current_time = 0.0; + double step_time = 0.0; + dummy.onPostUpdate(current_time, step_time); + + EXPECT_FALSE(first_cleanup); + EXPECT_FALSE(first_update); + EXPECT_TRUE(second_cleanup); + EXPECT_TRUE(second_update); } \ No newline at end of file From fd21bcc279e9b242af117f8c36a8f5e8b9692789 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 15 Apr 2024 11:59:28 +0200 Subject: [PATCH 020/118] entity_base tests; code cleanup --- .../test/src/entity/test_entity_base.cpp | 278 ++++++++++++++---- 1 file changed, 226 insertions(+), 52 deletions(-) diff --git a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp index 374f9420ee4..2b899bda8ba 100644 --- a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp @@ -18,9 +18,21 @@ class DummyEntity : public traffic_simulator::entity::EntityBase { } - void appendToJobList(const std::function & func_on_update, - const std::function & func_on_cleanup, traffic_simulator::job::Type type, bool exclusive, - const traffic_simulator::job::Event event) + traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; + auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter override + { + return behavior_parameter; + } + + void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter & params) override + { + behavior_parameter = params; + } + + void appendToJobList( + const std::function & func_on_update, + const std::function & func_on_cleanup, traffic_simulator::job::Type type, + bool exclusive, const traffic_simulator::job::Event event) { job_list_.append(func_on_update, func_on_cleanup, type, exclusive, event); } @@ -32,14 +44,14 @@ class DummyEntity : public traffic_simulator::entity::EntityBase { static const auto default_dynamic_constraints = []() { auto dynamic_constraints = traffic_simulator_msgs::msg::DynamicConstraints(); - dynamic_constraints.max_speed = 0.0; - dynamic_constraints.max_acceleration = 0.0; - dynamic_constraints.max_acceleration_rate = 0.0; - dynamic_constraints.max_deceleration = 0.0; - dynamic_constraints.max_deceleration_rate = 0.0; + dynamic_constraints.max_speed = 3.0; + dynamic_constraints.max_acceleration = 5.0; + dynamic_constraints.max_acceleration_rate = 7.0; + dynamic_constraints.max_deceleration = 11.0; + dynamic_constraints.max_deceleration_rate = 13.0; return dynamic_constraints; }(); - + return default_dynamic_constraints; } @@ -58,7 +70,10 @@ class DummyEntity : public traffic_simulator::entity::EntityBase ~DummyEntity() override = default; - auto getGoalPoses() -> std::vector override { return {}; } + auto getGoalPoses() -> std::vector override + { + return {}; + } std::optional getObstacle() override { @@ -77,38 +92,38 @@ class DummyEntity : public traffic_simulator::entity::EntityBase return traffic_simulator_msgs::msg::WaypointsArray(); } - void requestSpeedChange(double, bool) override {} + void setVelocityLimit(double) override {} - void requestSpeedChange(const traffic_simulator::speed_change::RelativeTargetSpeed &, bool) override {} + void setAccelerationLimit(double) override {} - void requestAssignRoute(const std::vector &) override {} + void setAccelerationRateLimit(double) override {} - void requestAssignRoute(const std::vector &) override {} + void setDecelerationLimit(double) override {} - void requestAcquirePosition(const traffic_simulator::CanonicalizedLaneletPose &) override {} + void setDecelerationRateLimit(double) override {} - void requestAcquirePosition(const geometry_msgs::msg::Pose &) override {} + void requestSpeedChange(double, bool) override {} void requestSpeedChange( - const double, const traffic_simulator::speed_change::Transition, const traffic_simulator::speed_change::Constraint, - const bool) override {} - - auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter override + const traffic_simulator::speed_change::RelativeTargetSpeed &, bool) override { - THROW_SEMANTIC_ERROR("getBehaviorParameter function does not support in MiscObjectEntity."); } - void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) override {} - - void setVelocityLimit(double) override {} + void requestAssignRoute(const std::vector &) override + { + } - void setAccelerationLimit(double) override {} + void requestAssignRoute(const std::vector &) override {} - void setAccelerationRateLimit(double) override {} + void requestAcquirePosition(const traffic_simulator::CanonicalizedLaneletPose &) override {} - void setDecelerationLimit(double) override {} + void requestAcquirePosition(const geometry_msgs::msg::Pose &) override {} - void setDecelerationRateLimit(double) override {} + void requestSpeedChange( + const double, const traffic_simulator::speed_change::Transition, + const traffic_simulator::speed_change::Constraint, const bool) override + { + } }; int main(int argc, char ** argv) @@ -116,14 +131,6 @@ int main(int argc, char ** argv) testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } -/* - std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; - geographic_msgs::msg::GeoPoint origin; - origin.latitude = 35.61836750154; - origin.longitude = 139.78066608243; - auto map = std::make_shared(path, origin); -*/ TEST(EntityBase, asFieldOperatorApplication) { @@ -132,10 +139,11 @@ TEST(EntityBase, asFieldOperatorApplication) entity_status.name = name; entity_status.lanelet_pose_valid = false; - auto canonicalized_entity_status = traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - + EXPECT_THROW(dummy.asFieldOperatorApplication(), std::runtime_error); } @@ -146,10 +154,11 @@ TEST(EntityBase, startNpcLogic) entity_status.name = name; entity_status.lanelet_pose_valid = false; - auto canonicalized_entity_status = traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - + EXPECT_FALSE(dummy.isNpcLogicStarted()); dummy.startNpcLogic(); EXPECT_TRUE(dummy.isNpcLogicStarted()); @@ -162,7 +171,8 @@ TEST(EntityBase, activateOutOfRangeJob_speed) entity_status.name = name; entity_status.lanelet_pose_valid = false; - auto canonicalized_entity_status = traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); @@ -174,7 +184,8 @@ TEST(EntityBase, activateOutOfRangeJob_speed) double max_jerk = 100.0; double velocity = 1.0; dummy.setLinearVelocity(velocity); - dummy.activateOutOfRangeJob(min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); + dummy.activateOutOfRangeJob( + min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); double current_time = 0.0; double step_time = 0.0; EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); @@ -188,7 +199,8 @@ TEST(EntityBase, activateOutOfRangeJob_acceleration) entity_status.name = name; entity_status.lanelet_pose_valid = false; - auto canonicalized_entity_status = traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); @@ -200,7 +212,8 @@ TEST(EntityBase, activateOutOfRangeJob_acceleration) double max_jerk = 100.0; double acceleration = 1.0; dummy.setLinearAcceleration(acceleration); - dummy.activateOutOfRangeJob(min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); + dummy.activateOutOfRangeJob( + min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); double current_time = 0.0; double step_time = 0.0; EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); @@ -214,7 +227,8 @@ TEST(EntityBase, activateOutOfRangeJob_jerk) entity_status.name = name; entity_status.lanelet_pose_valid = false; - auto canonicalized_entity_status = traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); @@ -226,7 +240,8 @@ TEST(EntityBase, activateOutOfRangeJob_jerk) double max_jerk = 0.0; double jerk = 1.0; dummy.setLinearJerk(jerk); - dummy.activateOutOfRangeJob(min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); + dummy.activateOutOfRangeJob( + min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); double current_time = 0.0; double step_time = 0.0; EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); @@ -240,7 +255,8 @@ TEST(EntityBase, onUpdate) entity_status.name = name; entity_status.lanelet_pose_valid = false; - auto canonicalized_entity_status = traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); @@ -260,8 +276,10 @@ TEST(EntityBase, onUpdate) auto second_event = traffic_simulator::job::Event::POST_UPDATE; auto is_exclusive = true; - dummy.appendToJobList(first_update_func, first_cleanup_func, type_first, is_exclusive, first_event); - dummy.appendToJobList(second_update_func, second_cleanup_func, type_second, is_exclusive, second_event); + dummy.appendToJobList( + first_update_func, first_cleanup_func, type_first, is_exclusive, first_event); + dummy.appendToJobList( + second_update_func, second_cleanup_func, type_second, is_exclusive, second_event); double current_time = 0.0; double step_time = 0.0; @@ -280,7 +298,8 @@ TEST(EntityBase, onPostUpdate) entity_status.name = name; entity_status.lanelet_pose_valid = false; - auto canonicalized_entity_status = traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); @@ -300,8 +319,10 @@ TEST(EntityBase, onPostUpdate) auto second_event = traffic_simulator::job::Event::POST_UPDATE; auto is_exclusive = true; - dummy.appendToJobList(first_update_func, first_cleanup_func, type_first, is_exclusive, first_event); - dummy.appendToJobList(second_update_func, second_cleanup_func, type_second, is_exclusive, second_event); + dummy.appendToJobList( + first_update_func, first_cleanup_func, type_first, is_exclusive, first_event); + dummy.appendToJobList( + second_update_func, second_cleanup_func, type_second, is_exclusive, second_event); double current_time = 0.0; double step_time = 0.0; @@ -311,4 +332,157 @@ TEST(EntityBase, onPostUpdate) EXPECT_FALSE(first_update); EXPECT_TRUE(second_cleanup); EXPECT_TRUE(second_update); +} + +TEST(EntityBase, resetDynamicConstraints) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + + auto default_constraints = dummy.getDefaultDynamicConstraints(); + dummy.resetDynamicConstraints(); + auto current_constraints = dummy.getDynamicConstraints(); + + EXPECT_EQ(default_constraints.max_speed, current_constraints.max_speed); + EXPECT_EQ(default_constraints.max_acceleration, current_constraints.max_acceleration); + EXPECT_EQ(default_constraints.max_deceleration, current_constraints.max_deceleration); + EXPECT_EQ(default_constraints.max_acceleration_rate, current_constraints.max_acceleration_rate); + EXPECT_EQ(default_constraints.max_deceleration_rate, current_constraints.max_deceleration_rate); +} + +TEST(EntityBase, setDynamicConstraints) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + traffic_simulator_msgs::msg::DynamicConstraints default_constraints{}; + + default_constraints.max_speed = 5.0; + default_constraints.max_acceleration = 7.0; + default_constraints.max_deceleration = 11.0; + default_constraints.max_acceleration_rate = 13.0; + default_constraints.max_deceleration_rate = 17.0; + dummy.setDynamicConstraints(default_constraints); + auto current_constraints = dummy.getDynamicConstraints(); + + EXPECT_EQ(default_constraints.max_speed, current_constraints.max_speed); + EXPECT_EQ(default_constraints.max_acceleration, current_constraints.max_acceleration); + EXPECT_EQ(default_constraints.max_deceleration, current_constraints.max_deceleration); + EXPECT_EQ(default_constraints.max_acceleration_rate, current_constraints.max_acceleration_rate); + EXPECT_EQ(default_constraints.max_deceleration_rate, current_constraints.max_deceleration_rate); +} + +TEST(EntityBase, requestFollowTrajectory) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + + std::shared_ptr ptr = nullptr; + EXPECT_THROW(dummy.requestFollowTrajectory(ptr), std::runtime_error); +} + +TEST(EntityBase, requestWalkStraight) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + + EXPECT_THROW(dummy.requestWalkStraight(), std::runtime_error); +} + +TEST(EntityBase, updateStandStillDuration_startedMoving) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + dummy.startNpcLogic(); + dummy.setLinearVelocity(3.0); + + EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); +} + +TEST(EntityBase, updateStandStillDuration_notStarted) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + + dummy.setLinearVelocity(3.0); + EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); + + dummy.setLinearVelocity(0.0); + EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); +} + +TEST(EntityBase, updateTraveledDistance_startedMoving) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + + double velocity = 3.0; + double step_time = 0.1; + dummy.startNpcLogic(); + dummy.setLinearVelocity(velocity); + + EXPECT_EQ(step_time * velocity, dummy.updateTraveledDistance(step_time)); +} + +TEST(EntityBase, updateTraveledDistance_notStarted) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + + double velocity = 3.0; + double step_time = 0.1; + dummy.setLinearVelocity(velocity); + EXPECT_EQ(0.0, dummy.updateTraveledDistance(step_time)); + + dummy.setLinearVelocity(0.0); + EXPECT_EQ(0.0, dummy.updateTraveledDistance(step_time)); } \ No newline at end of file From 5cdd159b506f69b65841323de223ab867f9e54aa Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 15 Apr 2024 16:48:40 +0200 Subject: [PATCH 021/118] entity_base tests --- .../test/src/entity/test_entity_base.cpp | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp index 2b899bda8ba..074a417c517 100644 --- a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp @@ -485,4 +485,25 @@ TEST(EntityBase, updateTraveledDistance_notStarted) dummy.setLinearVelocity(0.0); EXPECT_EQ(0.0, dummy.updateTraveledDistance(step_time)); +} + +TEST(EntityBase, stopAtCurrentPosition) +{ + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.lanelet_pose_valid = false; + + auto canonicalized_entity_status = + traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); + auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); + + double velocity = 3.0; + dummy.setLinearVelocity(velocity); + auto curr_twist = dummy.getCurrentTwist(); + EXPECT_TRUE(curr_twist.linear.x == velocity); + + dummy.stopAtCurrentPosition(); + curr_twist = dummy.getCurrentTwist(); + EXPECT_TRUE(curr_twist.linear.x == 0.0); } \ No newline at end of file From 95a604fae18e4442d108a5e47e51079152bf8822 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 16 Apr 2024 14:23:01 +0200 Subject: [PATCH 022/118] simulation clock tests --- .../traffic_simulator/test/CMakeLists.txt | 1 + .../test/src/behavior/CMakeLists.txt | 3 + .../test/src/behavior/test_route_planner.cpp | 12 +++ .../test/src/entity/test_entity_base.cpp | 2 +- .../test/src/simulation_clock/CMakeLists.txt | 2 + .../test_simulation_clock.cpp | 81 +++++++++++++++++++ 6 files changed, 100 insertions(+), 1 deletion(-) create mode 100644 simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp create mode 100644 simulation/traffic_simulator/test/src/simulation_clock/CMakeLists.txt create mode 100644 simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp diff --git a/simulation/traffic_simulator/test/CMakeLists.txt b/simulation/traffic_simulator/test/CMakeLists.txt index c49034e4784..56ae9f04cb7 100644 --- a/simulation/traffic_simulator/test/CMakeLists.txt +++ b/simulation/traffic_simulator/test/CMakeLists.txt @@ -4,6 +4,7 @@ add_subdirectory(src/entity) add_subdirectory(src/behavior) add_subdirectory(src/data_type) add_subdirectory(src/job) +add_subdirectory(src/simulation_clock) ament_add_gtest(test_hdmap_utils src/test_hdmap_utils.cpp) target_link_libraries(test_hdmap_utils traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt b/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt index a7d988d18b7..d8fe02b6706 100644 --- a/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt @@ -3,3 +3,6 @@ target_link_libraries(test_longitudinal_speed_planning traffic_simulator) ament_add_gtest(test_behavior test_behavior.cpp) target_link_libraries(test_behavior traffic_simulator) + +ament_add_gtest(test_route_planner test_route_planner.cpp) +target_link_libraries(test_route_planner traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp new file mode 100644 index 00000000000..17d1c94865e --- /dev/null +++ b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp @@ -0,0 +1,12 @@ +#include + +#include +#include +#include +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp index 074a417c517..4d039e893e1 100644 --- a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp @@ -506,4 +506,4 @@ TEST(EntityBase, stopAtCurrentPosition) dummy.stopAtCurrentPosition(); curr_twist = dummy.getCurrentTwist(); EXPECT_TRUE(curr_twist.linear.x == 0.0); -} \ No newline at end of file +} diff --git a/simulation/traffic_simulator/test/src/simulation_clock/CMakeLists.txt b/simulation/traffic_simulator/test/src/simulation_clock/CMakeLists.txt new file mode 100644 index 00000000000..db5773d24ce --- /dev/null +++ b/simulation/traffic_simulator/test/src/simulation_clock/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_simulation_clock test_simulation_clock.cpp) +target_link_libraries(test_simulation_clock traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp new file mode 100644 index 00000000000..cb71dde0343 --- /dev/null +++ b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp @@ -0,0 +1,81 @@ +#include + +#include + +TEST(SimulationClock, getCurrentRosTime) +{ + constexpr double realtime_factor = 2.0; + constexpr double frame_rate = 10.0; + traffic_simulator::SimulationClock simulation_clock(true, realtime_factor, frame_rate); + simulation_clock.start(); + + const auto initial_time = simulation_clock.getCurrentRosTime(); + + for (int i = 0; i < 5; ++i) { + simulation_clock.update(); + } + + const auto current_time = simulation_clock.getCurrentRosTime(); + const double elapsed_time = (current_time - initial_time).seconds(); + const double expected_elapsed_time = 5.0 * realtime_factor / frame_rate; + + EXPECT_NEAR(elapsed_time, expected_elapsed_time, 1e-6); +} + +TEST(SimulationClock, getCurrentScenarioTime) +{ + traffic_simulator::SimulationClock clock(true, 1.0, 30.0); + + clock.start(); + + EXPECT_DOUBLE_EQ(clock.getCurrentScenarioTime(), 0.0); + + for (int i = 0; i < 10; ++i) { + clock.update(); + const double expected_scenario_time = (i + 1) * clock.getStepTime(); + EXPECT_DOUBLE_EQ(clock.getCurrentScenarioTime(), expected_scenario_time); + } + + for (int i = 0; i < 5; ++i) { + clock.update(); + const double expected_scenario_time = (10 + 1 + i) * clock.getStepTime(); + EXPECT_DOUBLE_EQ(clock.getCurrentScenarioTime(), expected_scenario_time); + } +} + +TEST(SimulationClock, Update) +{ + const bool use_sim_time = true; + const double realtime_factor = 1.0; + const double frame_rate = 10.0; + traffic_simulator::SimulationClock simulation_clock(use_sim_time, realtime_factor, frame_rate); + + simulation_clock.start(); + const double initial_simulation_time = simulation_clock.getCurrentSimulationTime(); + + const int num_updates = 10; + const double step_time = simulation_clock.getStepTime(); + const double tolerance = 1e-6; + + for (int i = 0; i < num_updates; ++i) { + simulation_clock.update(); + const double expected_simulation_time = initial_simulation_time + (i + 1) * step_time; + const double actual_simulation_time = simulation_clock.getCurrentSimulationTime(); + EXPECT_NEAR(actual_simulation_time, expected_simulation_time, tolerance); + } +} + +TEST(SimulationClock, Initialize) +{ + traffic_simulator::SimulationClock sim_clock(true, 1.0, 10.0); + + sim_clock.started(); + sim_clock.update(); + + EXPECT_NO_THROW(sim_clock.start()); + + sim_clock.started(); + sim_clock.update(); + + EXPECT_THROW(sim_clock.start(), std::runtime_error); +} \ No newline at end of file From f601722b88daea137dd80061e8387b73cc98c9d5 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 18 Apr 2024 10:16:37 +0200 Subject: [PATCH 023/118] getRouteLanelets_empty --- .../test/src/behavior/test_route_planner.cpp | 20 +++++++++++++++++++ .../test_simulation_clock.cpp | 6 ++---- 2 files changed, 22 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp index 17d1c94865e..90d0ee34f6a 100644 --- a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp +++ b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp @@ -10,3 +10,23 @@ int main(int argc, char ** argv) testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } + +TEST(RoutePlanner, getRouteLanelets_empty) +{ + std::string path = + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; + geographic_msgs::msg::GeoPoint origin; + origin.latitude = 35.61836750154; + origin.longitude = 139.78066608243; + auto hdmap_utils_ptr = std::make_shared(path, origin); + + traffic_simulator::RoutePlanner route_planner(hdmap_utils_ptr); + route_planner.setWaypoints({}); + + traffic_simulator::lanelet_pose::CanonicalizedLaneletPose entity_pose( + traffic_simulator::helper::constructLaneletPose(120659, 1, 0), hdmap_utils_ptr); + const auto route_lanelets = route_planner.getRouteLanelets(entity_pose, 100.0); + + const auto following_lanelets = hdmap_utils_ptr->getFollowingLanelets(120659, 100.0, true); + EXPECT_EQ(route_lanelets, following_lanelets); +} diff --git a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp index cb71dde0343..370abaf5415 100644 --- a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp +++ b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp @@ -69,13 +69,11 @@ TEST(SimulationClock, Initialize) { traffic_simulator::SimulationClock sim_clock(true, 1.0, 10.0); - sim_clock.started(); + EXPECT_FALSE(sim_clock.started()); sim_clock.update(); - EXPECT_NO_THROW(sim_clock.start()); - sim_clock.started(); + EXPECT_TRUE(sim_clock.started()); sim_clock.update(); - EXPECT_THROW(sim_clock.start(), std::runtime_error); } \ No newline at end of file From 07e587c054815ab059b38ee6b0ec1f6df700aa5a Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 18 Apr 2024 15:03:06 +0200 Subject: [PATCH 024/118] getDistanceToLeftLaneBound --- .../test/src/entity/test_entity_base.cpp | 53 +++++++++++++++++++ 1 file changed, 53 insertions(+) diff --git a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp index 4d039e893e1..45f1961f377 100644 --- a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp @@ -507,3 +507,56 @@ TEST(EntityBase, stopAtCurrentPosition) curr_twist = dummy.getCurrentTwist(); EXPECT_TRUE(curr_twist.linear.x == 0.0); } + +auto makeHdMapUtilsSharedPointer() -> std::shared_ptr +{ + std::string path = + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; + geographic_msgs::msg::GeoPoint origin; + origin.latitude = 35.9037067912303; + origin.longitude = 139.9337945139059; + return std::make_shared(path, origin); +} + +auto makeCanonicalizedLaneletPose(std::shared_ptr ptr, lanelet::Id id = 120659) +{ + return traffic_simulator::lanelet_pose::CanonicalizedLaneletPose( + traffic_simulator::helper::constructLaneletPose(id, 0, 0), ptr); +} + +auto makeCanonicalizedEntityStatus(std::shared_ptr ptr, traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose) +{ + const double bounding_box_dims = 1.0; + const std::string name("test"); + auto entity_status = traffic_simulator::EntityStatus(); + entity_status.name = name; + entity_status.bounding_box = + traffic_simulator_msgs::build() + .center(geometry_msgs::build().x(0).y(0).z(0)) + .dimensions(geometry_msgs::build() + .x(bounding_box_dims) + .y(bounding_box_dims) + .z(bounding_box_dims)); + entity_status.lanelet_pose_valid = true; + entity_status.lanelet_pose = static_cast(pose); + entity_status.pose = ptr->toMapPose(entity_status.lanelet_pose).pose; + + return traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, ptr); +} + +TEST(EntityBase, getDistanceToLeftLaneBound) +{ + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose); + + auto dummy = DummyEntity("test_name", status, hdmap_utils_ptr); + + auto left = dummy.getDistanceToLeftLaneBound(id); + + const double lane_width = 3.0; + const double entity_bounding_box_dims = 1.0; + EXPECT_NEAR(left, (lane_width - entity_bounding_box_dims) / 2, 0.1); +} From e9c650d1521a1d4de023b41a2825b154693933a5 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 27 May 2024 17:42:13 +0200 Subject: [PATCH 025/118] style --- .../traffic_simulator/test/CMakeLists.txt | 1 - .../test/src/behavior/CMakeLists.txt | 6 - .../test/src/behavior/test_behavior.cpp | 14 + .../test_longitudinal_speed_planning.cpp | 486 --------------- .../test/src/behavior/test_route_planner.cpp | 32 - .../test/src/data_type/CMakeLists.txt | 2 - .../test/src/data_type/test_speed_change.cpp | 12 - .../test/src/entity/CMakeLists.txt | 3 - .../test/src/entity/test_entity_base.cpp | 562 ------------------ .../test/src/job/test_job.cpp | 24 +- .../test/src/job/test_job_list.cpp | 32 +- .../test_simulation_clock.cpp | 97 +-- .../src/traffic_lights/test_traffic_light.cpp | 20 +- 13 files changed, 119 insertions(+), 1172 deletions(-) delete mode 100644 simulation/traffic_simulator/test/src/behavior/test_longitudinal_speed_planning.cpp delete mode 100644 simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp delete mode 100644 simulation/traffic_simulator/test/src/data_type/CMakeLists.txt delete mode 100644 simulation/traffic_simulator/test/src/data_type/test_speed_change.cpp delete mode 100644 simulation/traffic_simulator/test/src/entity/test_entity_base.cpp diff --git a/simulation/traffic_simulator/test/CMakeLists.txt b/simulation/traffic_simulator/test/CMakeLists.txt index 56ae9f04cb7..9fb80cdd396 100644 --- a/simulation/traffic_simulator/test/CMakeLists.txt +++ b/simulation/traffic_simulator/test/CMakeLists.txt @@ -2,7 +2,6 @@ add_subdirectory(src/traffic_lights) add_subdirectory(src/helper) add_subdirectory(src/entity) add_subdirectory(src/behavior) -add_subdirectory(src/data_type) add_subdirectory(src/job) add_subdirectory(src/simulation_clock) diff --git a/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt b/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt index d8fe02b6706..e06238b665b 100644 --- a/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt @@ -1,8 +1,2 @@ -ament_add_gtest(test_longitudinal_speed_planning test_longitudinal_speed_planning.cpp) -target_link_libraries(test_longitudinal_speed_planning traffic_simulator) - ament_add_gtest(test_behavior test_behavior.cpp) target_link_libraries(test_behavior traffic_simulator) - -ament_add_gtest(test_route_planner test_route_planner.cpp) -target_link_libraries(test_route_planner traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp b/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp index 03a448df60d..dc05788ad93 100644 --- a/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp +++ b/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp @@ -1,3 +1,17 @@ +// 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 #include diff --git a/simulation/traffic_simulator/test/src/behavior/test_longitudinal_speed_planning.cpp b/simulation/traffic_simulator/test/src/behavior/test_longitudinal_speed_planning.cpp deleted file mode 100644 index 2c059662856..00000000000 --- a/simulation/traffic_simulator/test/src/behavior/test_longitudinal_speed_planning.cpp +++ /dev/null @@ -1,486 +0,0 @@ -#include - -#include -#include - -#include "../expect_eq_macros.hpp" - -#define EXPECT_CONSTRAINTS_BOUNDED(DATA, lower, upper) \ - EXPECT_TRUE(new_constraints.max_speed >= lower); \ - EXPECT_TRUE(new_constraints.max_acceleration >= lower); \ - EXPECT_TRUE(new_constraints.max_deceleration >= lower); \ - EXPECT_TRUE(new_constraints.max_acceleration_rate >= lower); \ - EXPECT_TRUE(new_constraints.max_deceleration_rate >= lower); \ - EXPECT_TRUE(new_constraints.max_speed < upper); \ - EXPECT_TRUE(new_constraints.max_acceleration < upper); \ - EXPECT_TRUE(new_constraints.max_deceleration < upper); \ - EXPECT_TRUE(new_constraints.max_acceleration_rate < upper); \ - EXPECT_TRUE(new_constraints.max_deceleration_rate < upper); - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -TEST(LongitudinalSpeedPlanner, isAccelerating_true) -{ - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - current_twist.linear.x = 1.0; - - const double target_speed = 2.0; - EXPECT_TRUE(planner.isAccelerating(target_speed, current_twist)); -} - -TEST(LongitudinalSpeedPlanner, isAccelerating_false) -{ - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - current_twist.linear.x = 3.0; - - const double target_speed = 2.0; - EXPECT_FALSE(planner.isAccelerating(target_speed, current_twist)); -} - -TEST(LongitudinalSpeedPlanner, isDecelerating_true) -{ - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - current_twist.linear.x = 3.0; - - const double target_speed = 2.0; - EXPECT_TRUE(planner.isDecelerating(target_speed, current_twist)); -} - -TEST(LongitudinalSpeedPlanner, isDecelerating_false) -{ - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - current_twist.linear.x = 1.0; - - const double target_speed = 2.0; - EXPECT_FALSE(planner.isDecelerating(target_speed, current_twist)); -} - -TEST(LongitudinalSpeedPlanner, getAccelerationDuration_acceleration) -{ - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - geometry_msgs::msg::Accel current_accel{}; - traffic_simulator_msgs::msg::DynamicConstraints constraints{}; - current_twist.linear.x = 1.0; - current_accel.linear.x = 1.0; - constraints.max_speed = 10.0; - constraints.max_acceleration = 2.0; - constraints.max_acceleration_rate = 1.0; - - const double epsilon = 1e-5; - - const double expected_duration = 4.0; - const double target_speed = 8.5f; - - const double result_duration = - planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel); - EXPECT_TRUE(std::abs(expected_duration - result_duration) < epsilon); -} - -TEST(LongitudinalSpeedPlanner, getAccelerationDuration_zero) -{ - // possible negative duration: - // longitudinal_speed_planning: 185, 199 - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - geometry_msgs::msg::Accel current_accel{}; - traffic_simulator_msgs::msg::DynamicConstraints constraints{}; - current_twist.linear.x = 1.0; - current_accel.linear.x = 1.0; - constraints.max_speed = 10.0; - constraints.max_acceleration = 1.0; - constraints.max_deceleration = 1.0; - constraints.max_acceleration_rate = 1.0; - constraints.max_deceleration_rate = 1.0; - - const double epsilon = 1e-5; - - double target_speed = current_twist.linear.x + epsilon; - double result_duration = - planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel); - EXPECT_TRUE(result_duration >= 0); - EXPECT_TRUE(result_duration < epsilon); - - constraints.max_speed = 100.0; - result_duration = - planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel); - EXPECT_TRUE(result_duration >= 0); - EXPECT_TRUE(result_duration < epsilon); - - current_twist.linear.x = 0.0; - target_speed = current_twist.linear.x + epsilon; - result_duration = - planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel); - EXPECT_TRUE(result_duration >= 0); - EXPECT_TRUE(result_duration < epsilon); - - target_speed = current_twist.linear.x + 0.0101; - result_duration = - planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel); - EXPECT_TRUE(result_duration >= 0); - EXPECT_TRUE(result_duration <= 0.0101); - target_speed = current_twist.linear.x + 0.0099; - result_duration = - planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel); - EXPECT_TRUE(result_duration >= 0); - EXPECT_TRUE(result_duration <= 0.0099); -} - -TEST(LongitudinalSpeedPlanner, planConstraintsFromJerkAndTimeConstraint_jerk) -{ - // possible sqrt of a negative number, results in a nan - // longitudinal_speed_planning: 59, 68 - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - geometry_msgs::msg::Accel current_accel{}; - traffic_simulator_msgs::msg::DynamicConstraints constraints{}; - current_twist.linear.x = 10.0; - current_accel.linear.x = 1.0; - constraints.max_speed = 10.0; - constraints.max_acceleration = 1.0; - constraints.max_deceleration = 1.0; - constraints.max_acceleration_rate = 1.0; - constraints.max_deceleration_rate = 1.0; - - double target_speed = 5.0; - double acceleration_duration = 1.0; - auto new_constraints = planner.planConstraintsFromJerkAndTimeConstraint( - target_speed, current_twist, current_accel, acceleration_duration, constraints); - - const double plausible_bound = 1e2; - EXPECT_CONSTRAINTS_BOUNDED(constraints, 0, plausible_bound); - - current_twist.linear.x = 1.0; - new_constraints = planner.planConstraintsFromJerkAndTimeConstraint( - target_speed, current_twist, current_accel, acceleration_duration, constraints); - EXPECT_CONSTRAINTS_BOUNDED(constraints, 0, plausible_bound); -} - -TEST(LongitudinalSpeedPlanner, planConstraintsFromJerkAndTimeConstraint_acceleration) -{ - // possible sqrt of a negative number, results in a nan - // longitudinal_speed_planning: 59, 68 - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - geometry_msgs::msg::Accel current_accel{}; - traffic_simulator_msgs::msg::DynamicConstraints constraints{}; - current_twist.linear.x = 10.0; - current_accel.linear.x = 1.0; - constraints.max_speed = 10.0; - constraints.max_acceleration = 1.0; - constraints.max_deceleration = 1.0; - constraints.max_acceleration_rate = 1.0; - constraints.max_deceleration_rate = 1.0; - - const double epsilon = 1e5; - double target_speed = current_twist.linear.x + epsilon; - double acceleration_duration = 1.0; - auto new_constraints = planner.planConstraintsFromJerkAndTimeConstraint( - target_speed, current_twist, current_accel, acceleration_duration, constraints); - - const double plausible_bound = 1e2; - EXPECT_CONSTRAINTS_BOUNDED(constraints, 0, plausible_bound); - - current_twist.linear.x = 0.0; - target_speed = current_twist.linear.x + epsilon; - new_constraints = planner.planConstraintsFromJerkAndTimeConstraint( - target_speed, current_twist, current_accel, acceleration_duration, constraints); - EXPECT_CONSTRAINTS_BOUNDED(constraints, 0, plausible_bound); -} - -TEST(LongitudinalSpeedPlanner, planConstraintsFromJerkAndTimeConstraint_deceleration) -{ - // possible sqrt of a negative number, results in a nan - // longitudinal_speed_planning: 59, 68 - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - geometry_msgs::msg::Accel current_accel{}; - traffic_simulator_msgs::msg::DynamicConstraints constraints{}; - current_twist.linear.x = 10.0; - current_accel.linear.x = 1.0; - constraints.max_speed = 10.0; - constraints.max_acceleration = 1.0; - constraints.max_deceleration = 1.0; - constraints.max_acceleration_rate = 1.0; - constraints.max_deceleration_rate = 1.0; - - const double epsilon = 1e5; - double target_speed = current_twist.linear.x - epsilon; - double acceleration_duration = 1.0; - auto new_constraints = planner.planConstraintsFromJerkAndTimeConstraint( - target_speed, current_twist, current_accel, acceleration_duration, constraints); - - const double plausible_bound = 1e2; - EXPECT_CONSTRAINTS_BOUNDED(constraints, 0, plausible_bound); - - current_twist.linear.x = 0.0; - target_speed = current_twist.linear.x - epsilon; - new_constraints = planner.planConstraintsFromJerkAndTimeConstraint( - target_speed, current_twist, current_accel, acceleration_duration, constraints); - EXPECT_CONSTRAINTS_BOUNDED(constraints, 0, plausible_bound); -} - -TEST(LongitudinalSpeedPlanner, getDynamicStates_targetSpeedOverLimit) -{ - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - geometry_msgs::msg::Accel current_accel{}; - traffic_simulator_msgs::msg::DynamicConstraints constraints{}; - current_twist.linear.x = 10.0; - current_accel.linear.x = 1.0; - constraints.max_speed = 20.0; - constraints.max_acceleration = 1.0; - constraints.max_deceleration = 1.0; - constraints.max_acceleration_rate = 1.0; - constraints.max_deceleration_rate = 1.0; - - double target_speed = 100.0; - auto [result0_twist, result0_accel, result0_jerk] = - planner.getDynamicStates(target_speed, constraints, current_twist, current_accel); - - target_speed = constraints.max_speed; - auto [result1_twist, result1_accel, result1_jerk] = - planner.getDynamicStates(target_speed, constraints, current_twist, current_accel); - - EXPECT_ACCEL_EQ(result0_accel, result1_accel); - EXPECT_TWIST_EQ(result0_twist, result1_twist); - EXPECT_DOUBLE_EQ(result0_jerk, result1_jerk); -} - -TEST(LongitudinalSpeedPlanner, getDynamicStates_maxJerk) -{ - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - geometry_msgs::msg::Accel current_accel{}; - traffic_simulator_msgs::msg::DynamicConstraints constraints{}; - current_twist.linear.x = 0.0; - current_accel.linear.x = 0.0; - constraints.max_speed = 20.0; - constraints.max_acceleration = 5.0; - constraints.max_deceleration = 5.0; - constraints.max_acceleration_rate = 1.0; - constraints.max_deceleration_rate = 1.0; - - double target_speed = constraints.max_speed; - - double result0_jerk = - std::get<2>(planner.getDynamicStates(target_speed, constraints, current_twist, current_accel)); - EXPECT_DOUBLE_EQ(result0_jerk, constraints.max_acceleration_rate); - - constraints.max_acceleration_rate = 2.0; - double result1_jerk = - std::get<2>(planner.getDynamicStates(target_speed, constraints, current_twist, current_accel)); - EXPECT_DOUBLE_EQ(result1_jerk, constraints.max_acceleration_rate); -} - -TEST(LongitudinalSpeedPlanner, getDynamicStates_shortAccel) -{ - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - geometry_msgs::msg::Accel current_accel{}; - traffic_simulator_msgs::msg::DynamicConstraints constraints{}; - current_twist.linear.x = 10.0; - current_accel.linear.x = 0.0; - constraints.max_speed = 20.0; - constraints.max_acceleration = 5.0; - constraints.max_deceleration = 5.0; - constraints.max_acceleration_rate = 5.0; - constraints.max_deceleration_rate = 5.0; - - const double epsilon = 1e-8; - double target_speed = current_twist.linear.x - epsilon; - - double result0_jerk = - std::get<2>(planner.getDynamicStates(target_speed, constraints, current_twist, current_accel)); - EXPECT_FALSE(result0_jerk == -constraints.max_deceleration_rate); - - constraints.max_deceleration_rate = 2.0; - double result1_jerk = - std::get<2>(planner.getDynamicStates(target_speed, constraints, current_twist, current_accel)); - EXPECT_FALSE(result1_jerk == -constraints.max_deceleration_rate); - - current_twist.linear.x = 0; - target_speed = current_twist.linear.x - epsilon; - double result2_jerk = - std::get<2>(planner.getDynamicStates(target_speed, constraints, current_twist, current_accel)); - EXPECT_FALSE(result2_jerk == -constraints.max_deceleration_rate); -} - -TEST(LongitudinalSpeedPlanner, isTargetSpeedReached_different) -{ - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - current_twist.linear.x = 10.0; - double target_speed = 15.0; - double tolerance = 3.0; - - EXPECT_FALSE(planner.isTargetSpeedReached(target_speed, current_twist, tolerance)); - - target_speed = 12.0; - EXPECT_TRUE(planner.isTargetSpeedReached(target_speed, current_twist, tolerance)); -} - -TEST(LongitudinalSpeedPlanner, isTargetSpeedReached_same) -{ - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - current_twist.linear.x = 10.0; - double target_speed = 10.0; - double tolerance = 1.0; - - EXPECT_TRUE(planner.isTargetSpeedReached(target_speed, current_twist, tolerance)); - - tolerance = 0.0; - EXPECT_TRUE(planner.isTargetSpeedReached(target_speed, current_twist, tolerance)); -} - -TEST(LongitudinalSpeedPlanner, getRunningDistance_shortTime) -{ - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - geometry_msgs::msg::Accel current_accel{}; - traffic_simulator_msgs::msg::DynamicConstraints constraints{}; - current_twist.linear.x = 10.0; - current_accel.linear.x = 0.0; - constraints.max_speed = 70.0; - constraints.max_acceleration = 18.0; - constraints.max_deceleration = 18.0; - constraints.max_acceleration_rate = 6.0; - constraints.max_deceleration_rate = 6.0; - - const double epsilon = 0.1; - double target_speed = current_twist.linear.x - epsilon; - double current_linear_jerk = 0.0; - double distance = planner.getRunningDistance( - target_speed, constraints, current_twist, current_accel, current_linear_jerk); - - EXPECT_TRUE(distance > 0); - double quad_time = constraints.max_deceleration / constraints.max_deceleration_rate; - double lin_time = (current_twist.linear.x - target_speed) / constraints.max_deceleration; - double distance_upper_bound = current_twist.linear.x * std::max(quad_time, lin_time); - EXPECT_TRUE(distance < distance_upper_bound); -} - -TEST(LongitudinalSpeedPlanner, getRunningDistance_longTime) -{ - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - geometry_msgs::msg::Accel current_accel{}; - traffic_simulator_msgs::msg::DynamicConstraints constraints{}; - current_twist.linear.x = 60.0; - current_accel.linear.x = 0.0; - constraints.max_speed = 70.0; - constraints.max_acceleration = 1.0; - constraints.max_deceleration = 1.0; - constraints.max_acceleration_rate = 5.0; - constraints.max_deceleration_rate = 5.0; - - double target_speed = 0.0; - double current_linear_jerk = 0.0; - double distance = planner.getRunningDistance( - target_speed, constraints, current_twist, current_accel, current_linear_jerk); - - EXPECT_TRUE(distance > 0); - double quad_time = constraints.max_deceleration / constraints.max_deceleration_rate; - double lin_time = (current_twist.linear.x - target_speed) / constraints.max_deceleration; - double distance_upper_bound = current_twist.linear.x * std::max(quad_time, lin_time); - EXPECT_TRUE(distance < distance_upper_bound); -} - -TEST(LongitudinalSpeedPlanner, getRunningDistance_zero) -{ - const double step_time = 0.001; - const std::string entity = "entity"; - traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner planner{ - step_time, entity}; - - geometry_msgs::msg::Twist current_twist{}; - geometry_msgs::msg::Accel current_accel{}; - traffic_simulator_msgs::msg::DynamicConstraints constraints{}; - current_twist.linear.x = 10.0; - current_accel.linear.x = 0.0; - constraints.max_speed = 20.0; - constraints.max_acceleration = 5.0; - constraints.max_deceleration = 5.0; - constraints.max_acceleration_rate = 5.0; - constraints.max_deceleration_rate = 5.0; - - double target_speed = current_twist.linear.x; - double current_linear_jerk = 1.0; - double distance = planner.getRunningDistance( - target_speed, constraints, current_twist, current_accel, current_linear_jerk); - - EXPECT_TRUE(distance == 0); -} diff --git a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp deleted file mode 100644 index 90d0ee34f6a..00000000000 --- a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include - -#include -#include -#include -#include - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -TEST(RoutePlanner, getRouteLanelets_empty) -{ - std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; - geographic_msgs::msg::GeoPoint origin; - origin.latitude = 35.61836750154; - origin.longitude = 139.78066608243; - auto hdmap_utils_ptr = std::make_shared(path, origin); - - traffic_simulator::RoutePlanner route_planner(hdmap_utils_ptr); - route_planner.setWaypoints({}); - - traffic_simulator::lanelet_pose::CanonicalizedLaneletPose entity_pose( - traffic_simulator::helper::constructLaneletPose(120659, 1, 0), hdmap_utils_ptr); - const auto route_lanelets = route_planner.getRouteLanelets(entity_pose, 100.0); - - const auto following_lanelets = hdmap_utils_ptr->getFollowingLanelets(120659, 100.0, true); - EXPECT_EQ(route_lanelets, following_lanelets); -} diff --git a/simulation/traffic_simulator/test/src/data_type/CMakeLists.txt b/simulation/traffic_simulator/test/src/data_type/CMakeLists.txt deleted file mode 100644 index 92829140b08..00000000000 --- a/simulation/traffic_simulator/test/src/data_type/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -ament_add_gtest(test_speed_change test_speed_change.cpp) -target_link_libraries(test_speed_change traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/data_type/test_speed_change.cpp b/simulation/traffic_simulator/test/src/data_type/test_speed_change.cpp deleted file mode 100644 index a77a6c9ee27..00000000000 --- a/simulation/traffic_simulator/test/src/data_type/test_speed_change.cpp +++ /dev/null @@ -1,12 +0,0 @@ -#include - -#include -#include -#include -#include - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/simulation/traffic_simulator/test/src/entity/CMakeLists.txt b/simulation/traffic_simulator/test/src/entity/CMakeLists.txt index ac7189815db..0e6b9eee47d 100644 --- a/simulation/traffic_simulator/test/src/entity/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/entity/CMakeLists.txt @@ -1,5 +1,2 @@ ament_add_gtest(test_vehicle_entity test_vehicle_entity.cpp) target_link_libraries(test_vehicle_entity traffic_simulator) - -ament_add_gtest(test_entity_base test_entity_base.cpp) -target_link_libraries(test_entity_base traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp deleted file mode 100644 index 45f1961f377..00000000000 --- a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp +++ /dev/null @@ -1,562 +0,0 @@ -#include - -#include -#include -#include -#include - -#include "../catalogs.hpp" -#include "../expect_eq_macros.hpp" - -class DummyEntity : public traffic_simulator::entity::EntityBase -{ -public: - explicit DummyEntity( - const std::string & name, const traffic_simulator::CanonicalizedEntityStatus & entity_status, - const std::shared_ptr & hdmap_utils_ptr) - : EntityBase(name, entity_status, hdmap_utils_ptr) - { - } - - traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; - auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter override - { - return behavior_parameter; - } - - void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter & params) override - { - behavior_parameter = params; - } - - void appendToJobList( - const std::function & func_on_update, - const std::function & func_on_cleanup, traffic_simulator::job::Type type, - bool exclusive, const traffic_simulator::job::Event event) - { - job_list_.append(func_on_update, func_on_cleanup, type, exclusive, event); - } - - auto getCurrentAction() const -> std::string override { return {}; } - - auto getDefaultDynamicConstraints() const - -> const traffic_simulator_msgs::msg::DynamicConstraints & override - { - static const auto default_dynamic_constraints = []() { - auto dynamic_constraints = traffic_simulator_msgs::msg::DynamicConstraints(); - dynamic_constraints.max_speed = 3.0; - dynamic_constraints.max_acceleration = 5.0; - dynamic_constraints.max_acceleration_rate = 7.0; - dynamic_constraints.max_deceleration = 11.0; - dynamic_constraints.max_deceleration_rate = 13.0; - return dynamic_constraints; - }(); - - return default_dynamic_constraints; - } - - auto getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & override - { - static traffic_simulator_msgs::msg::EntityType type; - type.type = traffic_simulator_msgs::msg::EntityType::MISC_OBJECT; - return type; - } - - auto getEntityTypename() const -> const std::string & override - { - static const auto str = std::string("dummy"); - return str; - } - - ~DummyEntity() override = default; - - auto getGoalPoses() -> std::vector override - { - return {}; - } - - std::optional getObstacle() override - { - return std::nullopt; - } - - auto getRouteLanelets(double) -> lanelet::Ids override - { - THROW_SEMANTIC_ERROR("getRouteLanelets function cannot not use in MiscObjectEntity"); - } - - auto fillLaneletPose(traffic_simulator::CanonicalizedEntityStatus &) -> void override {} - - auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override - { - return traffic_simulator_msgs::msg::WaypointsArray(); - } - - void setVelocityLimit(double) override {} - - void setAccelerationLimit(double) override {} - - void setAccelerationRateLimit(double) override {} - - void setDecelerationLimit(double) override {} - - void setDecelerationRateLimit(double) override {} - - void requestSpeedChange(double, bool) override {} - - void requestSpeedChange( - const traffic_simulator::speed_change::RelativeTargetSpeed &, bool) override - { - } - - void requestAssignRoute(const std::vector &) override - { - } - - void requestAssignRoute(const std::vector &) override {} - - void requestAcquirePosition(const traffic_simulator::CanonicalizedLaneletPose &) override {} - - void requestAcquirePosition(const geometry_msgs::msg::Pose &) override {} - - void requestSpeedChange( - const double, const traffic_simulator::speed_change::Transition, - const traffic_simulator::speed_change::Constraint, const bool) override - { - } -}; - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -TEST(EntityBase, asFieldOperatorApplication) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - - EXPECT_THROW(dummy.asFieldOperatorApplication(), std::runtime_error); -} - -TEST(EntityBase, startNpcLogic) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - - EXPECT_FALSE(dummy.isNpcLogicStarted()); - dummy.startNpcLogic(); - EXPECT_TRUE(dummy.isNpcLogicStarted()); -} - -TEST(EntityBase, activateOutOfRangeJob_speed) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - - double min_velocity = 0.0; - double max_velocity = 0.0; - double min_acceleration = -100.0; - double max_acceleration = 100.0; - double min_jerk = -100.0; - double max_jerk = 100.0; - double velocity = 1.0; - dummy.setLinearVelocity(velocity); - dummy.activateOutOfRangeJob( - min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); - double current_time = 0.0; - double step_time = 0.0; - EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); - EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), std::runtime_error); -} - -TEST(EntityBase, activateOutOfRangeJob_acceleration) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - - double min_velocity = -100.0; - double max_velocity = 100.0; - double min_acceleration = 0.0; - double max_acceleration = 0.0; - double min_jerk = -100.0; - double max_jerk = 100.0; - double acceleration = 1.0; - dummy.setLinearAcceleration(acceleration); - dummy.activateOutOfRangeJob( - min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); - double current_time = 0.0; - double step_time = 0.0; - EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); - EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), std::runtime_error); -} - -TEST(EntityBase, activateOutOfRangeJob_jerk) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - - double min_velocity = -100.0; - double max_velocity = 100.0; - double min_acceleration = -100.0; - double max_acceleration = 100.0; - double min_jerk = 0.0; - double max_jerk = 0.0; - double jerk = 1.0; - dummy.setLinearJerk(jerk); - dummy.activateOutOfRangeJob( - min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); - double current_time = 0.0; - double step_time = 0.0; - EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); - EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), std::runtime_error); -} - -TEST(EntityBase, onUpdate) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - - bool first_cleanup = false; - bool first_update = false; - bool second_cleanup = false; - bool second_update = false; - - auto first_update_func = [&first_update](const double) { return first_update = true; }; - auto first_cleanup_func = [&first_cleanup]() { first_cleanup = true; }; - auto second_update_func = [&second_update](const double) { return second_update = true; }; - auto second_cleanup_func = [&second_cleanup]() { second_cleanup = true; }; - - auto type_first = traffic_simulator::job::Type::LINEAR_VELOCITY; - auto type_second = traffic_simulator::job::Type::LINEAR_ACCELERATION; - auto first_event = traffic_simulator::job::Event::PRE_UPDATE; - auto second_event = traffic_simulator::job::Event::POST_UPDATE; - auto is_exclusive = true; - - dummy.appendToJobList( - first_update_func, first_cleanup_func, type_first, is_exclusive, first_event); - dummy.appendToJobList( - second_update_func, second_cleanup_func, type_second, is_exclusive, second_event); - - double current_time = 0.0; - double step_time = 0.0; - dummy.onUpdate(current_time, step_time); - - EXPECT_TRUE(first_cleanup); - EXPECT_TRUE(first_update); - EXPECT_FALSE(second_cleanup); - EXPECT_FALSE(second_update); -} - -TEST(EntityBase, onPostUpdate) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - - bool first_cleanup = false; - bool first_update = false; - bool second_cleanup = false; - bool second_update = false; - - auto first_update_func = [&first_update](const double) { return first_update = true; }; - auto first_cleanup_func = [&first_cleanup]() { first_cleanup = true; }; - auto second_update_func = [&second_update](const double) { return second_update = true; }; - auto second_cleanup_func = [&second_cleanup]() { second_cleanup = true; }; - - auto type_first = traffic_simulator::job::Type::LINEAR_VELOCITY; - auto type_second = traffic_simulator::job::Type::LINEAR_ACCELERATION; - auto first_event = traffic_simulator::job::Event::PRE_UPDATE; - auto second_event = traffic_simulator::job::Event::POST_UPDATE; - auto is_exclusive = true; - - dummy.appendToJobList( - first_update_func, first_cleanup_func, type_first, is_exclusive, first_event); - dummy.appendToJobList( - second_update_func, second_cleanup_func, type_second, is_exclusive, second_event); - - double current_time = 0.0; - double step_time = 0.0; - dummy.onPostUpdate(current_time, step_time); - - EXPECT_FALSE(first_cleanup); - EXPECT_FALSE(first_update); - EXPECT_TRUE(second_cleanup); - EXPECT_TRUE(second_update); -} - -TEST(EntityBase, resetDynamicConstraints) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - - auto default_constraints = dummy.getDefaultDynamicConstraints(); - dummy.resetDynamicConstraints(); - auto current_constraints = dummy.getDynamicConstraints(); - - EXPECT_EQ(default_constraints.max_speed, current_constraints.max_speed); - EXPECT_EQ(default_constraints.max_acceleration, current_constraints.max_acceleration); - EXPECT_EQ(default_constraints.max_deceleration, current_constraints.max_deceleration); - EXPECT_EQ(default_constraints.max_acceleration_rate, current_constraints.max_acceleration_rate); - EXPECT_EQ(default_constraints.max_deceleration_rate, current_constraints.max_deceleration_rate); -} - -TEST(EntityBase, setDynamicConstraints) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - traffic_simulator_msgs::msg::DynamicConstraints default_constraints{}; - - default_constraints.max_speed = 5.0; - default_constraints.max_acceleration = 7.0; - default_constraints.max_deceleration = 11.0; - default_constraints.max_acceleration_rate = 13.0; - default_constraints.max_deceleration_rate = 17.0; - dummy.setDynamicConstraints(default_constraints); - auto current_constraints = dummy.getDynamicConstraints(); - - EXPECT_EQ(default_constraints.max_speed, current_constraints.max_speed); - EXPECT_EQ(default_constraints.max_acceleration, current_constraints.max_acceleration); - EXPECT_EQ(default_constraints.max_deceleration, current_constraints.max_deceleration); - EXPECT_EQ(default_constraints.max_acceleration_rate, current_constraints.max_acceleration_rate); - EXPECT_EQ(default_constraints.max_deceleration_rate, current_constraints.max_deceleration_rate); -} - -TEST(EntityBase, requestFollowTrajectory) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - - std::shared_ptr ptr = nullptr; - EXPECT_THROW(dummy.requestFollowTrajectory(ptr), std::runtime_error); -} - -TEST(EntityBase, requestWalkStraight) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - - EXPECT_THROW(dummy.requestWalkStraight(), std::runtime_error); -} - -TEST(EntityBase, updateStandStillDuration_startedMoving) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - dummy.startNpcLogic(); - dummy.setLinearVelocity(3.0); - - EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); -} - -TEST(EntityBase, updateStandStillDuration_notStarted) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - - dummy.setLinearVelocity(3.0); - EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); - - dummy.setLinearVelocity(0.0); - EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); -} - -TEST(EntityBase, updateTraveledDistance_startedMoving) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - - double velocity = 3.0; - double step_time = 0.1; - dummy.startNpcLogic(); - dummy.setLinearVelocity(velocity); - - EXPECT_EQ(step_time * velocity, dummy.updateTraveledDistance(step_time)); -} - -TEST(EntityBase, updateTraveledDistance_notStarted) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - - double velocity = 3.0; - double step_time = 0.1; - dummy.setLinearVelocity(velocity); - EXPECT_EQ(0.0, dummy.updateTraveledDistance(step_time)); - - dummy.setLinearVelocity(0.0); - EXPECT_EQ(0.0, dummy.updateTraveledDistance(step_time)); -} - -TEST(EntityBase, stopAtCurrentPosition) -{ - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.lanelet_pose_valid = false; - - auto canonicalized_entity_status = - traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, nullptr); - auto dummy = DummyEntity(name, canonicalized_entity_status, nullptr); - - double velocity = 3.0; - dummy.setLinearVelocity(velocity); - auto curr_twist = dummy.getCurrentTwist(); - EXPECT_TRUE(curr_twist.linear.x == velocity); - - dummy.stopAtCurrentPosition(); - curr_twist = dummy.getCurrentTwist(); - EXPECT_TRUE(curr_twist.linear.x == 0.0); -} - -auto makeHdMapUtilsSharedPointer() -> std::shared_ptr -{ - std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; - geographic_msgs::msg::GeoPoint origin; - origin.latitude = 35.9037067912303; - origin.longitude = 139.9337945139059; - return std::make_shared(path, origin); -} - -auto makeCanonicalizedLaneletPose(std::shared_ptr ptr, lanelet::Id id = 120659) -{ - return traffic_simulator::lanelet_pose::CanonicalizedLaneletPose( - traffic_simulator::helper::constructLaneletPose(id, 0, 0), ptr); -} - -auto makeCanonicalizedEntityStatus(std::shared_ptr ptr, traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose) -{ - const double bounding_box_dims = 1.0; - const std::string name("test"); - auto entity_status = traffic_simulator::EntityStatus(); - entity_status.name = name; - entity_status.bounding_box = - traffic_simulator_msgs::build() - .center(geometry_msgs::build().x(0).y(0).z(0)) - .dimensions(geometry_msgs::build() - .x(bounding_box_dims) - .y(bounding_box_dims) - .z(bounding_box_dims)); - entity_status.lanelet_pose_valid = true; - entity_status.lanelet_pose = static_cast(pose); - entity_status.pose = ptr->toMapPose(entity_status.lanelet_pose).pose; - - return traffic_simulator::entity_status::CanonicalizedEntityStatus(entity_status, ptr); -} - -TEST(EntityBase, getDistanceToLeftLaneBound) -{ - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose); - - auto dummy = DummyEntity("test_name", status, hdmap_utils_ptr); - - auto left = dummy.getDistanceToLeftLaneBound(id); - - const double lane_width = 3.0; - const double entity_bounding_box_dims = 1.0; - EXPECT_NEAR(left, (lane_width - entity_bounding_box_dims) / 2, 0.1); -} diff --git a/simulation/traffic_simulator/test/src/job/test_job.cpp b/simulation/traffic_simulator/test/src/job/test_job.cpp index ae1a4dc1fc3..d7e6ac63989 100644 --- a/simulation/traffic_simulator/test/src/job/test_job.cpp +++ b/simulation/traffic_simulator/test/src/job/test_job.cpp @@ -13,9 +13,9 @@ TEST(Job, onUpdate) bool was_cleanup_func_called = false; auto update_func = [](const double) { return true; }; auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; - auto type = traffic_simulator::job::Type::UNKNOWN; - auto event = traffic_simulator::job::Event::POST_UPDATE; - auto is_exclusive = true; + const auto type = traffic_simulator::job::Type::UNKNOWN; + const auto event = traffic_simulator::job::Event::POST_UPDATE; + const bool is_exclusive = true; auto job = traffic_simulator::job::Job(update_func, cleanup_func, type, is_exclusive, event); const double step_time = 0.0; @@ -29,9 +29,9 @@ TEST(Job, get_type) bool was_cleanup_func_called = false; auto update_func = [](const double) { return true; }; auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; - auto type = traffic_simulator::job::Type::UNKNOWN; - auto event = traffic_simulator::job::Event::POST_UPDATE; - auto is_exclusive = true; + const auto type = traffic_simulator::job::Type::UNKNOWN; + const auto event = traffic_simulator::job::Event::POST_UPDATE; + const bool is_exclusive = true; auto job = traffic_simulator::job::Job(update_func, cleanup_func, type, is_exclusive, event); @@ -48,9 +48,9 @@ TEST(Job, get_exclusive) bool was_cleanup_func_called = false; auto update_func = [](const double) { return true; }; auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; - auto type = traffic_simulator::job::Type::UNKNOWN; - auto event = traffic_simulator::job::Event::POST_UPDATE; - auto is_exclusive = true; + const auto type = traffic_simulator::job::Type::UNKNOWN; + const auto event = traffic_simulator::job::Event::POST_UPDATE; + const bool is_exclusive = true; auto job = traffic_simulator::job::Job(update_func, cleanup_func, type, is_exclusive, event); @@ -67,9 +67,9 @@ TEST(Job, get_event) bool was_cleanup_func_called = false; auto update_func = [](const double) { return true; }; auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; - auto type = traffic_simulator::job::Type::UNKNOWN; - auto event = traffic_simulator::job::Event::POST_UPDATE; - auto is_exclusive = true; + const auto type = traffic_simulator::job::Type::UNKNOWN; + const auto event = traffic_simulator::job::Event::POST_UPDATE; + const bool is_exclusive = true; auto job = traffic_simulator::job::Job(update_func, cleanup_func, type, is_exclusive, event); diff --git a/simulation/traffic_simulator/test/src/job/test_job_list.cpp b/simulation/traffic_simulator/test/src/job/test_job_list.cpp index 6f396fcde57..84746407668 100644 --- a/simulation/traffic_simulator/test/src/job/test_job_list.cpp +++ b/simulation/traffic_simulator/test/src/job/test_job_list.cpp @@ -1,3 +1,17 @@ +// 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 #include @@ -13,9 +27,9 @@ TEST(JobList, append) bool was_cleanup_func_called = false; auto update_func = [](const double) { return true; }; auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; - auto type = traffic_simulator::job::Type::UNKNOWN; - auto event = traffic_simulator::job::Event::POST_UPDATE; - auto is_exclusive = true; + const auto type = traffic_simulator::job::Type::UNKNOWN; + const auto event = traffic_simulator::job::Event::POST_UPDATE; + const bool is_exclusive = true; auto job_list = traffic_simulator::job::JobList(); @@ -39,9 +53,9 @@ TEST(JobList, append_doubled) auto second_update_func = [&second_update](const double) { return second_update = true; }; auto second_cleanup_func = [&second_cleanup]() { second_cleanup = true; }; - auto type = traffic_simulator::job::Type::UNKNOWN; - auto event = traffic_simulator::job::Event::POST_UPDATE; - auto is_exclusive = true; + const auto type = traffic_simulator::job::Type::UNKNOWN; + const auto event = traffic_simulator::job::Event::POST_UPDATE; + const bool is_exclusive = true; auto job_list = traffic_simulator::job::JobList(); @@ -68,9 +82,9 @@ TEST(JobList, update) }; auto cleanup_func = [&cleanup_count]() { cleanup_count++; }; - auto type = traffic_simulator::job::Type::UNKNOWN; - auto event = traffic_simulator::job::Event::POST_UPDATE; - auto is_exclusive = true; + const auto type = traffic_simulator::job::Type::UNKNOWN; + const auto event = traffic_simulator::job::Event::POST_UPDATE; + const bool is_exclusive = true; auto job_list = traffic_simulator::job::JobList(); diff --git a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp index 370abaf5415..84faa4cfc60 100644 --- a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp +++ b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp @@ -1,79 +1,102 @@ +// 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 #include +TEST(SimulationClock, Initialize) +{ + const double realtime_factor = 1.0; + const double frame_rate = 10.0; + const bool use_sim_time = true; + auto simulation_clock = traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); + + EXPECT_FALSE(simulation_clock.started()); + simulation_clock.update(); + EXPECT_NO_THROW(simulation_clock.start()); + + EXPECT_TRUE(simulation_clock.started()); + simulation_clock.update(); + EXPECT_THROW(simulation_clock.start(), std::runtime_error); +} + TEST(SimulationClock, getCurrentRosTime) { - constexpr double realtime_factor = 2.0; - constexpr double frame_rate = 10.0; - traffic_simulator::SimulationClock simulation_clock(true, realtime_factor, frame_rate); + const double realtime_factor = 2.0; + const double frame_rate = 10.0; + const bool use_sim_time = true; + auto simulation_clock = traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); simulation_clock.start(); const auto initial_time = simulation_clock.getCurrentRosTime(); - for (int i = 0; i < 5; ++i) { + const int iterations = 5; + for (int i = 0; i < iterations; ++i) { simulation_clock.update(); } const auto current_time = simulation_clock.getCurrentRosTime(); - const double elapsed_time = (current_time - initial_time).seconds(); - const double expected_elapsed_time = 5.0 * realtime_factor / frame_rate; + const double result_elapsed_time = (current_time - initial_time).seconds(); + const double actual_elapsed_time = static_cast(iterations) * realtime_factor / frame_rate; + const double epsilon = 1e-6; - EXPECT_NEAR(elapsed_time, expected_elapsed_time, 1e-6); + EXPECT_NEAR(result_elapsed_time, actual_elapsed_time, epsilon); } TEST(SimulationClock, getCurrentScenarioTime) { - traffic_simulator::SimulationClock clock(true, 1.0, 30.0); + const double realtime_factor = 1.0; + const double frame_rate = 30.0; + const bool use_sim_time = true; + auto simulation_clock = traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); - clock.start(); + simulation_clock.start(); - EXPECT_DOUBLE_EQ(clock.getCurrentScenarioTime(), 0.0); + EXPECT_DOUBLE_EQ(simulation_clock.getCurrentScenarioTime(), 0.0); - for (int i = 0; i < 10; ++i) { - clock.update(); - const double expected_scenario_time = (i + 1) * clock.getStepTime(); - EXPECT_DOUBLE_EQ(clock.getCurrentScenarioTime(), expected_scenario_time); + const int iterations = 5; + for (int i = 0; i < iterations; ++i) { + simulation_clock.update(); } - for (int i = 0; i < 5; ++i) { - clock.update(); - const double expected_scenario_time = (10 + 1 + i) * clock.getStepTime(); - EXPECT_DOUBLE_EQ(clock.getCurrentScenarioTime(), expected_scenario_time); - } + const double result_elapsed_time = simulation_clock.getCurrentScenarioTime(); + const double actual_elapsed_time = static_cast(iterations) * realtime_factor / frame_rate; + const double epsilon = 1e-6; + + EXPECT_NEAR(actual_elapsed_time, result_elapsed_time, epsilon); } TEST(SimulationClock, Update) { - const bool use_sim_time = true; const double realtime_factor = 1.0; const double frame_rate = 10.0; - traffic_simulator::SimulationClock simulation_clock(use_sim_time, realtime_factor, frame_rate); + const bool use_sim_time = true; + auto simulation_clock = traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); simulation_clock.start(); + const double initial_simulation_time = simulation_clock.getCurrentSimulationTime(); - const int num_updates = 10; + const int iterations = 10; const double step_time = simulation_clock.getStepTime(); const double tolerance = 1e-6; - for (int i = 0; i < num_updates; ++i) { + for (int i = 0; i < iterations; ++i) { simulation_clock.update(); - const double expected_simulation_time = initial_simulation_time + (i + 1) * step_time; + const double expected_simulation_time = initial_simulation_time + static_cast(i + 1) * step_time; const double actual_simulation_time = simulation_clock.getCurrentSimulationTime(); EXPECT_NEAR(actual_simulation_time, expected_simulation_time, tolerance); } -} - -TEST(SimulationClock, Initialize) -{ - traffic_simulator::SimulationClock sim_clock(true, 1.0, 10.0); - - EXPECT_FALSE(sim_clock.started()); - sim_clock.update(); - EXPECT_NO_THROW(sim_clock.start()); - - EXPECT_TRUE(sim_clock.started()); - sim_clock.update(); - EXPECT_THROW(sim_clock.start(), std::runtime_error); } \ No newline at end of file diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp index f7e9d22b32c..113794f6eae 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp @@ -711,33 +711,33 @@ TEST(TrafficLight, Color_make_wrong) { using Color = traffic_simulator::TrafficLight::Color; Color color; - EXPECT_THROW(color = Color::make("karmazynowy"), std::runtime_error); - EXPECT_FALSE(boost::lexical_cast(color) == "karmazynowy"); + EXPECT_THROW(color = Color::make("wrong_color"), std::runtime_error); + EXPECT_FALSE(boost::lexical_cast(color) == "wrong_color"); } TEST(TrafficLight, Shape_make_wrong) { using Shape = traffic_simulator::TrafficLight::Shape; Shape shape; - EXPECT_THROW(shape = Shape::make("dwunastoscian foremny"), std::runtime_error); - EXPECT_FALSE(boost::lexical_cast(shape) == "dwunastoscian foremny"); + EXPECT_THROW(shape = Shape::make("wrong_shape"), std::runtime_error); + EXPECT_FALSE(boost::lexical_cast(shape) == "wrong_shape"); } TEST(TrafficLight, Status_make_wrong) { using Status = traffic_simulator::TrafficLight::Status; Status status; - EXPECT_THROW(status = Status::make("ekonomiczny"), std::runtime_error); - EXPECT_FALSE(boost::lexical_cast(status) == "ekonomiczny"); + EXPECT_THROW(status = Status::make("wrong_status"), std::runtime_error); + EXPECT_FALSE(boost::lexical_cast(status) == "wrong_status"); } TEST(TrafficLight, Bulb_make_wrong) { using Bulb = traffic_simulator::TrafficLight::Bulb; - EXPECT_THROW(Bulb("red flashing dwunastoscian foremny"), std::runtime_error); - EXPECT_THROW(Bulb("red ekonomiczny circle"), std::runtime_error); - EXPECT_THROW(Bulb("karmazynowy flashing circle"), std::runtime_error); + EXPECT_THROW(Bulb("red flashing wrong_shape"), std::runtime_error); + EXPECT_THROW(Bulb("red wrong_status circle"), std::runtime_error); + EXPECT_THROW(Bulb("wrong_color flashing circle"), std::runtime_error); - EXPECT_THROW(Bulb("karmazynowy ekonomiczny dwunastoscian foremny"), std::runtime_error); + EXPECT_THROW(Bulb("wrong_color wrong_status wrong_shape"), std::runtime_error); } From dc3fbe7f4e176cc9774548f0e6a7721c5d481ff2 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 27 May 2024 17:43:06 +0200 Subject: [PATCH 026/118] remove tests that exist in other branches and refactor wht is left --- .../simulation_clock/test_simulation_clock.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp index 84faa4cfc60..398a7b28773 100644 --- a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp +++ b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp @@ -21,7 +21,8 @@ TEST(SimulationClock, Initialize) const double realtime_factor = 1.0; const double frame_rate = 10.0; const bool use_sim_time = true; - auto simulation_clock = traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); + auto simulation_clock = + traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); EXPECT_FALSE(simulation_clock.started()); simulation_clock.update(); @@ -37,7 +38,8 @@ TEST(SimulationClock, getCurrentRosTime) const double realtime_factor = 2.0; const double frame_rate = 10.0; const bool use_sim_time = true; - auto simulation_clock = traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); + auto simulation_clock = + traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); simulation_clock.start(); const auto initial_time = simulation_clock.getCurrentRosTime(); @@ -60,7 +62,8 @@ TEST(SimulationClock, getCurrentScenarioTime) const double realtime_factor = 1.0; const double frame_rate = 30.0; const bool use_sim_time = true; - auto simulation_clock = traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); + auto simulation_clock = + traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); simulation_clock.start(); @@ -83,7 +86,8 @@ TEST(SimulationClock, Update) const double realtime_factor = 1.0; const double frame_rate = 10.0; const bool use_sim_time = true; - auto simulation_clock = traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); + auto simulation_clock = + traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); simulation_clock.start(); @@ -95,7 +99,8 @@ TEST(SimulationClock, Update) for (int i = 0; i < iterations; ++i) { simulation_clock.update(); - const double expected_simulation_time = initial_simulation_time + static_cast(i + 1) * step_time; + const double expected_simulation_time = + initial_simulation_time + static_cast(i + 1) * step_time; const double actual_simulation_time = simulation_clock.getCurrentSimulationTime(); EXPECT_NEAR(actual_simulation_time, expected_simulation_time, tolerance); } From f17189f4bbe9d34a4aadc72e6b8178b5946ba53c Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 5 Jun 2024 14:32:45 +0200 Subject: [PATCH 027/118] Prototype ParameterManager Signed-off-by: Mateusz Palczuk --- .../entity/entity_manager.hpp | 27 ++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 9a568d40ff6..985c9af71ef 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -52,6 +53,28 @@ #include #include +class ParameterManager +{ +public: + template + ParameterManager(NodeT && node) + : node_parameters_interface_(rclcpp::node_interfaces::get_node_parameters_interface(node)) + { + } + + template + auto getParameter(const std::string & name, const ParameterT & default_value = {}) + { + if (not node_parameters_interface_->has_parameter(name)) { + node_parameters_interface_->declare_parameter(name, rclcpp::ParameterValue(default_value)); + } + return node_parameters_interface_->get_parameter(name).get_value(); + } + +private: + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface_; +}; + /// @todo find some shared space for this function template static auto getParameter(const std::string & name, T value = {}) @@ -85,6 +108,7 @@ class EntityManager Configuration configuration; std::shared_ptr node_topics_interface; + ParameterManager parameter_manager_; tf2_ros::StaticTransformBroadcaster broadcaster_; tf2_ros::TransformBroadcaster base_link_broadcaster_; @@ -159,6 +183,7 @@ class EntityManager explicit EntityManager(NodeT && node, const Configuration & configuration) : configuration(configuration), node_topics_interface(rclcpp::node_interfaces::get_node_topics_interface(node)), + parameter_manager_(node), broadcaster_(node), base_link_broadcaster_(node), clock_ptr_(node->get_clock()), @@ -506,7 +531,7 @@ class EntityManager entity_status.lanelet_pose = *lanelet_pose; entity_status.lanelet_pose_valid = true; /// @note fix z, roll and pitch to fitting to the lanelet - if (getParameter("consider_pose_by_road_slope", false)) { + if (parameter_manager_.getParameter("consider_pose_by_road_slope", false)) { entity_status.pose = hdmap_utils_ptr_->toMapPose(*lanelet_pose).pose; } else { entity_status.pose = pose; From 5869e8afee97fe2c37a5f3548c54451edec110e2 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 5 Jun 2024 15:08:36 +0200 Subject: [PATCH 028/118] Move ParameterManager Signed-off-by: Mateusz Palczuk --- .../entity/entity_manager.hpp | 24 +--------- .../utils/parameter_manager.hpp | 47 +++++++++++++++++++ 2 files changed, 48 insertions(+), 23 deletions(-) create mode 100644 simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 985c9af71ef..d1ffee74a8a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include #include @@ -43,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -53,28 +53,6 @@ #include #include -class ParameterManager -{ -public: - template - ParameterManager(NodeT && node) - : node_parameters_interface_(rclcpp::node_interfaces::get_node_parameters_interface(node)) - { - } - - template - auto getParameter(const std::string & name, const ParameterT & default_value = {}) - { - if (not node_parameters_interface_->has_parameter(name)) { - node_parameters_interface_->declare_parameter(name, rclcpp::ParameterValue(default_value)); - } - return node_parameters_interface_->get_parameter(name).get_value(); - } - -private: - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface_; -}; - /// @todo find some shared space for this function template static auto getParameter(const std::string & name, T value = {}) diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp new file mode 100644 index 00000000000..a551c4cb8fe --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp @@ -0,0 +1,47 @@ +// Copyright 2024 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 TRAFFIC_SIMULATOR__UTILS__PARAMETER_MANAGER_HPP_ +#define TRAFFIC_SIMULATOR__UTILS__PARAMETER_MANAGER_HPP_ + +#include + +namespace traffic_simulator +{ + +class ParameterManager +{ +public: + template + ParameterManager(NodeT && node) + : node_parameters_interface_(rclcpp::node_interfaces::get_node_parameters_interface(node)) + { + } + + template + auto getParameter(const std::string & name, const ParameterT & default_value = {}) + { + if (not node_parameters_interface_->has_parameter(name)) { + node_parameters_interface_->declare_parameter(name, rclcpp::ParameterValue(default_value)); + } + return node_parameters_interface_->get_parameter(name).get_value(); + } + +private: + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface_; +}; + +} // namespace traffic_simulator + +#endif //TRAFFIC_SIMULATOR__UTILS__PARAMETER_MANAGER_HPP_ From bb3b21bfc9f13d85d7d5444836ed464905b18391 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 5 Jun 2024 15:24:13 +0200 Subject: [PATCH 029/118] Add ParameterManager to API Signed-off-by: Mateusz Palczuk --- .../include/traffic_simulator/api/api.hpp | 6 +++++- .../include/traffic_simulator/entity/entity_manager.hpp | 7 ++++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index d50990b863a..79898704b87 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -65,7 +65,9 @@ class API template , typename... Ts> explicit API(NodeT && node, const Configuration & configuration, Ts &&... xs) : configuration(configuration), - entity_manager_ptr_(std::make_shared(node, configuration)), + parameter_manager_(node), + entity_manager_ptr_( + std::make_shared(node, configuration, parameter_manager_)), traffic_controller_ptr_(std::make_shared( entity_manager_ptr_->getHdmapUtils(), [this]() { return API::getEntityNames(); }, [this](const auto & name) { return API::getMapPose(name); }, @@ -403,6 +405,8 @@ class API const Configuration configuration; + ParameterManager parameter_manager_; + const std::shared_ptr entity_manager_ptr_; const std::shared_ptr traffic_controller_ptr_; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index d1ffee74a8a..17dfcc42dda 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -145,7 +145,7 @@ class EntityManager auto makeV2ITrafficLightPublisher(Ts &&... xs) -> std::shared_ptr { if (const auto architecture_type = - getParameter("architecture_type", "awf/universe"); + parameter_manager_.getParameter("architecture_type", "awf/universe"); architecture_type.find("awf/universe") != std::string::npos) { return std::make_shared< TrafficLightPublisher>( @@ -158,10 +158,11 @@ class EntityManager } template > - explicit EntityManager(NodeT && node, const Configuration & configuration) + explicit EntityManager( + NodeT && node, const Configuration & configuration, const ParameterManager & parameter_manager) : configuration(configuration), node_topics_interface(rclcpp::node_interfaces::get_node_topics_interface(node)), - parameter_manager_(node), + parameter_manager_(parameter_manager), broadcaster_(node), base_link_broadcaster_(node), clock_ptr_(node->get_clock()), From 5575b4eef7cab5b06524b0d3024e857aeffee1ed Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 5 Jun 2024 15:27:46 +0200 Subject: [PATCH 030/118] Add getParameter forwarding in API Signed-off-by: Mateusz Palczuk --- .../include/traffic_simulator/api/api.hpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 79898704b87..78fd6e7cea3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -388,6 +388,13 @@ class API public: #undef FORWARD_TO_ENTITY_MANAGER + template + decltype(auto) getParameter(const std::string & name, const ParameterT & default_value = {}) + { + return parameter_manager_.getParameter( + std::forward(name), std::forward(default_value)); + } + auto canonicalize(const LaneletPose & maybe_non_canonicalized_lanelet_pose) const -> CanonicalizedLaneletPose; auto canonicalize(const EntityStatus & may_non_canonicalized_entity_status) const From 1f9dc1a9448bbeb8fff5af2f4d725fdd31e6bfc9 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 5 Jun 2024 15:58:00 +0200 Subject: [PATCH 031/118] Use const for getParameter Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/include/traffic_simulator/api/api.hpp | 4 ++-- .../include/traffic_simulator/entity/entity_manager.hpp | 2 +- .../include/traffic_simulator/utils/parameter_manager.hpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 78fd6e7cea3..7cc8fed8c3e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -389,7 +389,7 @@ class API #undef FORWARD_TO_ENTITY_MANAGER template - decltype(auto) getParameter(const std::string & name, const ParameterT & default_value = {}) + decltype(auto) getParameter(const std::string & name, const ParameterT & default_value = {}) const { return parameter_manager_.getParameter( std::forward(name), std::forward(default_value)); @@ -412,7 +412,7 @@ class API const Configuration configuration; - ParameterManager parameter_manager_; + const ParameterManager parameter_manager_; const std::shared_ptr entity_manager_ptr_; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 17dfcc42dda..6b496e30e32 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -86,7 +86,7 @@ class EntityManager Configuration configuration; std::shared_ptr node_topics_interface; - ParameterManager parameter_manager_; + const ParameterManager parameter_manager_; tf2_ros::StaticTransformBroadcaster broadcaster_; tf2_ros::TransformBroadcaster base_link_broadcaster_; diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp index a551c4cb8fe..86b04970ebe 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp @@ -30,7 +30,7 @@ class ParameterManager } template - auto getParameter(const std::string & name, const ParameterT & default_value = {}) + auto getParameter(const std::string & name, const ParameterT & default_value = {}) const { if (not node_parameters_interface_->has_parameter(name)) { node_parameters_interface_->declare_parameter(name, rclcpp::ParameterValue(default_value)); From 6f22fc0b324ba83dda4cd52333dd0542978b182c Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 5 Jun 2024 16:00:03 +0200 Subject: [PATCH 032/118] Apply API getParameter function where possible Signed-off-by: Mateusz Palczuk --- mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp | 2 +- .../include/openscenario_interpreter/simulator_core.hpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index a2b2980590c..b339e781252 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -101,7 +101,7 @@ void CppScenarioNode::spawnEgoEntity( api_.attachOccupancyGridSensor([this] { simulation_api_schema::OccupancyGridSensorConfiguration configuration; // clang-format off - configuration.set_architecture_type(getParameter("architecture_type", "awf/universe")); + configuration.set_architecture_type(api_.getParameter("architecture_type", "awf/universe")); configuration.set_entity("ego"); configuration.set_filter_by_range(true); configuration.set_height(200); diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 5e1f4ef6a76..760a54caba9 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -338,7 +338,7 @@ class SimulatorCore core->attachDetectionSensor([&]() { simulation_api_schema::DetectionSensorConfiguration configuration; // clang-format off - configuration.set_architecture_type(getParameter("architecture_type", "awf/universe")); + configuration.set_architecture_type(core->getParameter("architecture_type", "awf/universe")); configuration.set_entity(entity_ref); configuration.set_detect_all_objects_in_range(controller.properties.template get("isClairvoyant")); configuration.set_object_recognition_delay(controller.properties.template get("detectedObjectPublishingDelay")); @@ -355,7 +355,7 @@ class SimulatorCore core->attachOccupancyGridSensor([&]() { simulation_api_schema::OccupancyGridSensorConfiguration configuration; // clang-format off - configuration.set_architecture_type(getParameter("architecture_type", "awf/universe")); + configuration.set_architecture_type(core->getParameter("architecture_type", "awf/universe")); configuration.set_entity(entity_ref); configuration.set_filter_by_range(controller.properties.template get("isClairvoyant")); configuration.set_height(200); @@ -370,7 +370,7 @@ class SimulatorCore core->attachPseudoTrafficLightDetector([&]() { simulation_api_schema::PseudoTrafficLightDetectorConfiguration configuration; configuration.set_architecture_type( - getParameter("architecture_type", "awf/universe")); + core->getParameter("architecture_type", "awf/universe")); return configuration; }()); From 0e8a1a2a0de70c09e17de399d213b9866de022e4 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 5 Jun 2024 16:10:54 +0200 Subject: [PATCH 033/118] Use ParameterManager in EgoEntity Signed-off-by: Mateusz Palczuk --- .../include/traffic_simulator/api/api.hpp | 2 +- .../traffic_simulator/entity/ego_entity.hpp | 6 +++-- .../src/entity/ego_entity.cpp | 22 ++++++++++--------- 3 files changed, 17 insertions(+), 13 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 7cc8fed8c3e..911410c241b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -133,7 +133,7 @@ class API if (behavior == VehicleBehavior::autoware()) { return entity_manager_ptr_->entityExists(name) or entity_manager_ptr_->spawnEntity( - name, pose, parameters, configuration); + name, pose, parameters, configuration, parameter_manager_); } else { return entity_manager_ptr_->spawnEntity( name, pose, parameters, behavior); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index fdf0aa0d171..4091812d52e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -36,7 +37,7 @@ class EgoEntity : public VehicleEntity { const std::unique_ptr field_operator_application; - static auto makeFieldOperatorApplication(const Configuration &) + static auto makeFieldOperatorApplication(const Configuration &, const ParameterManager &) -> std::unique_ptr; bool is_controlled_by_simulator_{false}; @@ -50,7 +51,8 @@ class EgoEntity : public VehicleEntity explicit EgoEntity( const std::string & name, const CanonicalizedEntityStatus &, const std::shared_ptr &, - const traffic_simulator_msgs::msg::VehicleParameters &, const Configuration &); + const traffic_simulator_msgs::msg::VehicleParameters &, const Configuration &, + const ParameterManager &); explicit EgoEntity(EgoEntity &&) = delete; diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index e3a056c2dc8..75aafc9e437 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -46,22 +46,24 @@ static auto getParameter(const std::string & name, T value = {}) return value; } -auto EgoEntity::makeFieldOperatorApplication(const Configuration & configuration) +auto EgoEntity::makeFieldOperatorApplication( + const Configuration & configuration, const ParameterManager & parameter_manager) -> std::unique_ptr { - if (const auto architecture_type = getParameter("architecture_type", "awf/universe"); + if (const auto architecture_type = + parameter_manager.getParameter("architecture_type", "awf/universe"); architecture_type.find("awf/universe") != std::string::npos) { - std::string rviz_config = getParameter("rviz_config", ""); - return getParameter("launch_autoware", true) + std::string rviz_config = parameter_manager.getParameter("rviz_config", ""); + return parameter_manager.getParameter("launch_autoware", true) ? std::make_unique< concealer::FieldOperatorApplicationFor>( - getParameter("autoware_launch_package"), - getParameter("autoware_launch_file"), + parameter_manager.getParameter("autoware_launch_package"), + parameter_manager.getParameter("autoware_launch_file"), "map_path:=" + configuration.map_path.string(), "lanelet2_map_file:=" + configuration.getLanelet2MapFile(), "pointcloud_map_file:=" + configuration.getPointCloudMapFile(), - "sensor_model:=" + getParameter("sensor_model"), - "vehicle_model:=" + getParameter("vehicle_model"), + "sensor_model:=" + parameter_manager.getParameter("sensor_model"), + "vehicle_model:=" + parameter_manager.getParameter("vehicle_model"), "rviz_config:=" + ((rviz_config == "") ? configuration.rviz_config_path.string() : Configuration::Pathname(rviz_config).string()), @@ -80,9 +82,9 @@ EgoEntity::EgoEntity( const std::string & name, const CanonicalizedEntityStatus & entity_status, const std::shared_ptr & hdmap_utils_ptr, const traffic_simulator_msgs::msg::VehicleParameters & parameters, - const Configuration & configuration) + const Configuration & configuration, const ParameterManager & parameter_manager) : VehicleEntity(name, entity_status, hdmap_utils_ptr, parameters), - field_operator_application(makeFieldOperatorApplication(configuration)) + field_operator_application(makeFieldOperatorApplication(configuration, parameter_manager)) { } From 8c9e194037347df8fe94d861e947d5e07398fb97 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 5 Jun 2024 16:12:24 +0200 Subject: [PATCH 034/118] Remove getParameter from EgoEntity Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/src/entity/ego_entity.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 75aafc9e437..1f350298c95 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -34,18 +34,6 @@ namespace traffic_simulator { namespace entity { -/// @todo find some shared space for this function -template -static auto getParameter(const std::string & name, T value = {}) -{ - rclcpp::Node node{"get_parameter", "simulation"}; - - node.declare_parameter(name, value); - node.get_parameter(name, value); - - return value; -} - auto EgoEntity::makeFieldOperatorApplication( const Configuration & configuration, const ParameterManager & parameter_manager) -> std::unique_ptr From ebc5ded166e92a65f787f8f652cff578e9704ac0 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 5 Jun 2024 16:19:48 +0200 Subject: [PATCH 035/118] Remove getParameter from EntityManager Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/entity/entity_manager.hpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 6b496e30e32..06184896cbd 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -53,18 +53,6 @@ #include #include -/// @todo find some shared space for this function -template -static auto getParameter(const std::string & name, T value = {}) -{ - rclcpp::Node node{"get_parameter", "simulation"}; - - node.declare_parameter(name, value); - node.get_parameter(name, value); - - return value; -} - namespace traffic_simulator { namespace entity From ed4b5212d3d3e05da98a42a7d21b85b79dd370b9 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 6 Jun 2024 09:33:50 +0200 Subject: [PATCH 036/118] remove arithmetic tests --- common/math/arithmetic/CMakeLists.txt | 8 - common/math/arithmetic/test/CMakeLists.txt | 1 - .../test/src/floating_point/CMakeLists.txt | 1 - .../src/floating_point/test_comparison.cpp | 158 ------------------ 4 files changed, 168 deletions(-) delete mode 100644 common/math/arithmetic/test/CMakeLists.txt delete mode 100644 common/math/arithmetic/test/src/floating_point/CMakeLists.txt delete mode 100644 common/math/arithmetic/test/src/floating_point/test_comparison.cpp diff --git a/common/math/arithmetic/CMakeLists.txt b/common/math/arithmetic/CMakeLists.txt index fbc34158cf1..a7983644b76 100644 --- a/common/math/arithmetic/CMakeLists.txt +++ b/common/math/arithmetic/CMakeLists.txt @@ -18,12 +18,4 @@ install( DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ DESTINATION include/) -if(BUILD_TESTING) - include_directories(include) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - find_package(ament_cmake_gtest REQUIRED) - add_subdirectory(test) -endif() - ament_auto_package() diff --git a/common/math/arithmetic/test/CMakeLists.txt b/common/math/arithmetic/test/CMakeLists.txt deleted file mode 100644 index 8bc9826fbbc..00000000000 --- a/common/math/arithmetic/test/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -add_subdirectory(src/floating_point) diff --git a/common/math/arithmetic/test/src/floating_point/CMakeLists.txt b/common/math/arithmetic/test/src/floating_point/CMakeLists.txt deleted file mode 100644 index 18b78b072c6..00000000000 --- a/common/math/arithmetic/test/src/floating_point/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -ament_add_gtest(test_comparison test_comparison.cpp) \ No newline at end of file diff --git a/common/math/arithmetic/test/src/floating_point/test_comparison.cpp b/common/math/arithmetic/test/src/floating_point/test_comparison.cpp deleted file mode 100644 index 951b03a7687..00000000000 --- a/common/math/arithmetic/test/src/floating_point/test_comparison.cpp +++ /dev/null @@ -1,158 +0,0 @@ -#include - -#include - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -TEST(Comparison, isApproximatelyEqualTo_equal) -{ - const float a = 0.1; - const float b = 0.10000000000000001; - - EXPECT_TRUE(math::arithmetic::isApproximatelyEqualTo(a, b)); - - const double af = -1000.1; - const double bf = -1000.10000000000000001; - - EXPECT_TRUE(math::arithmetic::isApproximatelyEqualTo(af, bf)); -} - -TEST(Comparison, isApproximatelyEqualTo_notEqual) -{ - const float a = 0.1; - const float b = 0.11; - - EXPECT_FALSE(math::arithmetic::isApproximatelyEqualTo(a, b)); - - const double af = -1000.1; - const double bf = -1000.11; - - EXPECT_FALSE(math::arithmetic::isApproximatelyEqualTo(af, bf)); -} - -TEST(Comparison, isEssentiallyEqualTo_equal) -{ - const float a = 0.1; - const float b = 0.10000000000000001; - - EXPECT_TRUE(math::arithmetic::isApproximatelyEqualTo(a, b)); - - const double af = -1000.1; - const double bf = -1000.10000000000000001; - - EXPECT_TRUE(math::arithmetic::isApproximatelyEqualTo(af, bf)); -} - -TEST(Comparison, isEssentiallyEqualTo_notEqual) -{ - const float a = 0.1; - const float b = 0.1000001; - - EXPECT_FALSE(math::arithmetic::isEssentiallyEqualTo(a, b)); - - const double af = -1000.1; - const double bf = -1000.1000001; - - EXPECT_FALSE(math::arithmetic::isEssentiallyEqualTo(af, bf)); -} - -TEST(Comparison, isDefinitelyLessThan_lessTwo) -{ - const float a = 0.1; - const float b = 0.2; - - EXPECT_TRUE(math::arithmetic::isDefinitelyLessThan(a, b)); - - const double af = -1.2; - const double bf = -1.1; - - EXPECT_TRUE(math::arithmetic::isDefinitelyLessThan(af, bf)); -} - -TEST(Comparison, isDefinitelyLessThan_lessThree) -{ - const float a = 0.1; - const float b = 0.2; - const float c = 0.3; - - EXPECT_TRUE(math::arithmetic::isDefinitelyLessThan(a, b, c)); - - const double af = -1.3; - const double bf = -1.2; - const double cf = -1.1; - - EXPECT_TRUE(math::arithmetic::isDefinitelyLessThan(af, bf, cf)); -} - -TEST(Comparison, isDefinitelyLessThan_moreTwo) -{ - const float a = 0.2; - const float b = 0.1; - - EXPECT_FALSE(math::arithmetic::isDefinitelyLessThan(a, b)); - - const double af = -1.1; - const double bf = -1.2; - - EXPECT_FALSE(math::arithmetic::isDefinitelyLessThan(af, bf)); -} - -TEST(Comparison, isDefinitelyLessThan_moreThree) -{ - const float a = 0.3; - const float b = 0.2; - const float c = 0.1; - - EXPECT_FALSE(math::arithmetic::isDefinitelyLessThan(a, b, c)); - - const double af = -1.1; - const double bf = -1.2; - const double cf = -1.3; - - EXPECT_FALSE(math::arithmetic::isDefinitelyLessThan(af, bf, cf)); -} - -TEST(Comparison, isDefinitelyLessThan_partlyMoreThree) -{ - const float a = 0.1; - const float b = 0.2; - const float c = 0.1; - - EXPECT_FALSE(math::arithmetic::isDefinitelyLessThan(a, b, c)); - - const double af = -1.1; - const double bf = -1.2; - const double cf = -1.1; - - EXPECT_FALSE(math::arithmetic::isDefinitelyLessThan(af, bf, cf)); -} - -TEST(Comparison, isDefinitelyGreaterThan_greater) -{ - const float a = 0.2; - const float b = 0.1; - - EXPECT_TRUE(math::arithmetic::isDefinitelyGreaterThan(a, b)); - - const double af = -1.1; - const double bf = -1.2; - - EXPECT_TRUE(math::arithmetic::isDefinitelyGreaterThan(af, bf)); -} - -TEST(Comparison, isDefinitelyGreaterThan_notGreater) -{ - const float a = 0.1; - const float b = 0.2; - - EXPECT_FALSE(math::arithmetic::isDefinitelyGreaterThan(a, b)); - - const double af = -1.2; - const double bf = -1.1; - - EXPECT_FALSE(math::arithmetic::isDefinitelyGreaterThan(af, bf)); -} \ No newline at end of file From 07b8e6be3570f21955fcaaf5155a8d70ce4eb6f7 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 6 Jun 2024 09:35:58 +0200 Subject: [PATCH 037/118] remove simple sensor simulator tets --- .../simple_sensor_simulator/CMakeLists.txt | 2 - .../test/CMakeLists.txt | 6 - .../test/src/sensor_simulation/CMakeLists.txt | 2 - .../sensor_simulation/lidar/CMakeLists.txt | 5 - .../lidar/test_lidar_sensor.cpp | 24 - .../lidar/test_raycaster.cpp | 61 --- .../occupancy_grid/CMakeLists.txt | 8 - .../occupancy_grid/test_grid_traversal.cpp | 65 --- .../test_occupancy_grid_builder.cpp | 81 ---- .../test_occupancy_grid_sensor.cpp | 9 - .../primitives/CMakeLists.txt | 8 - .../sensor_simulation/primitives/test_box.cpp | 60 --- .../primitives/test_primitive.cpp | 456 ------------------ .../primitives/test_vertex.cpp | 95 ---- .../test_sensor_simulation.cpp | 55 --- .../vehicle_model/CMakeLists.txt | 11 - .../test_sim_model_ideal_steer_acc.cpp | 91 ---- .../test_sim_model_ideal_steer_acc_geared.cpp | 91 ---- .../test_sim_model_ideal_steer_vel.cpp | 87 ---- .../test_sim_model_interface.cpp | 9 - 20 files changed, 1226 deletions(-) delete mode 100644 simulation/simple_sensor_simulator/test/CMakeLists.txt delete mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/CMakeLists.txt delete mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt delete mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp delete mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.cpp delete mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt delete mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_grid_traversal.cpp delete mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp delete mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp delete mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt delete mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp delete mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.cpp delete mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp delete mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp delete mode 100644 simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/CMakeLists.txt delete mode 100644 simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc.cpp delete mode 100644 simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc_geared.cpp delete mode 100644 simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_vel.cpp delete mode 100644 simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_interface.cpp diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt index ee20b432ae4..8e784fecd3a 100644 --- a/simulation/simple_sensor_simulator/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/CMakeLists.txt @@ -86,8 +86,6 @@ EXPORT export_simple_sensor_simulator_component if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - find_package(ament_cmake_gtest REQUIRED) - add_subdirectory(test) endif() ament_auto_package() diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt deleted file mode 100644 index 5d184534ffa..00000000000 --- a/simulation/simple_sensor_simulator/test/CMakeLists.txt +++ /dev/null @@ -1,6 +0,0 @@ -add_subdirectory(src/sensor_simulation/primitives) -add_subdirectory(src/sensor_simulation/occupancy_grid) -add_subdirectory(src/sensor_simulation/lidar) -add_subdirectory(src/sensor_simulation) - -add_subdirectory(src/vehicle_simulation/vehicle_model) \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/CMakeLists.txt deleted file mode 100644 index 5508fb23d39..00000000000 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -ament_add_gtest(test_sensor_simulation test_sensor_simulation.cpp) -target_link_libraries(test_sensor_simulation simple_sensor_simulator_component) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt deleted file mode 100644 index 53bb19d7837..00000000000 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -ament_add_gtest(test_lidar_sensor test_lidar_sensor.cpp) -target_link_libraries(test_lidar_sensor simple_sensor_simulator_component) - -ament_add_gtest(test_raycaster test_raycaster.cpp) -target_link_libraries(test_raycaster simple_sensor_simulator_component) \ No newline at end of file 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 deleted file mode 100644 index d4fe5a7ceb6..00000000000 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp +++ /dev/null @@ -1,24 +0,0 @@ -#include - -#include - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -TEST(LidarSensor, update_noEgo) -{ - const double current_simulation_time{}; - const simulation_api_schema::LidarConfiguration configuration{}; - const rclcpp::Publisher::SharedPtr publisher_ptr{}; - - auto sensor = simple_sensor_simulator::LidarSensor( - current_simulation_time, configuration, publisher_ptr); - std::vector status{}; - const rclcpp::Time current_ros_time{}; - - EXPECT_THROW( - sensor.update(current_simulation_time + 1, status, current_ros_time), std::runtime_error); -} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.cpp deleted file mode 100644 index 97b6a6cd186..00000000000 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#include - -#include -#include -#include - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -TEST(Raycaster, addPrimitive_box) -{ - auto rc = simple_sensor_simulator::Raycaster(); - - const std::string type("name"); - geometry_msgs::msg::Pose pose; - pose.position.x = 9.0; - pose.position.y = 11.0; - pose.position.z = 17.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - - const float depth = 19.0; - const float width = 23.0; - const float height = 29.0; - simple_sensor_simulator::primitives::Box box(depth, width, height, pose); - - EXPECT_NO_THROW( - rc.addPrimitive(std::string("box"), box)); -} - -TEST(Raycaster, addPrimitive_twoIdenticalNames) -{ - auto rc = simple_sensor_simulator::Raycaster(); - - const std::string type("name"); - geometry_msgs::msg::Pose pose; - pose.position.x = 9.0; - pose.position.y = 11.0; - pose.position.z = 17.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - - const float depth = 19.0; - const float width = 23.0; - const float height = 29.0; - simple_sensor_simulator::primitives::Box box_0(depth, width, height, pose); - simple_sensor_simulator::primitives::Box box_1(depth, width, height, pose); - - EXPECT_NO_THROW( - rc.addPrimitive(std::string("box"), box_0)); - EXPECT_THROW( - rc.addPrimitive(std::string("box"), box_1), - std::runtime_error); -} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt deleted file mode 100644 index 95b15765cbb..00000000000 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -ament_add_gtest(test_grid_traversal test_grid_traversal.cpp) -target_link_libraries(test_grid_traversal simple_sensor_simulator_component) - -ament_add_gtest(test_occupancy_grid_builder test_occupancy_grid_builder.cpp) -target_link_libraries(test_occupancy_grid_builder simple_sensor_simulator_component) - -ament_add_gtest(test_occupancy_grid_sensor test_occupancy_grid_sensor.cpp) -target_link_libraries(test_occupancy_grid_sensor simple_sensor_simulator_component) \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_grid_traversal.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_grid_traversal.cpp deleted file mode 100644 index 24bb2908239..00000000000 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_grid_traversal.cpp +++ /dev/null @@ -1,65 +0,0 @@ -#include - -#include - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -TEST(GridTraversal, begin) -{ - double start_x = 2.2; - double start_y = 3.3; - double end_x = 5.5; - double end_y = 7.7; - auto traversal = simple_sensor_simulator::GridTraversal(start_x, start_y, end_x, end_y); - - auto it = traversal.begin(); - - EXPECT_TRUE((*it).first == static_cast(start_x)); - EXPECT_TRUE((*it).second == static_cast(start_y)); -} - -TEST(GridTraversal, operatorIncrement) -{ - double start_x = 2.2; - double start_y = 3.3; - double end_x = 5.5; - double end_y = 7.7; - auto traversal = simple_sensor_simulator::GridTraversal(start_x, start_y, end_x, end_y); - - auto it = traversal.begin(); - - ++it; - EXPECT_TRUE((*it).first == static_cast(start_x)); - EXPECT_TRUE((*it).second == static_cast(start_y) + 1); - ++it; - EXPECT_TRUE((*it).first == static_cast(start_x) + 1); - EXPECT_TRUE((*it).second == static_cast(start_y) + 1); -} - -TEST(GridTraversal, operatorNotEqual) -{ - double start_x = 0.0; - double start_y = 0.0; - double end_x = 10.5; - double end_y = 0.0; - auto traversal = simple_sensor_simulator::GridTraversal(start_x, start_y, end_x, end_y); - - auto it = traversal.begin(); - - int curr = 0; - for (int i = 0; i <= end_x; i++) { - EXPECT_TRUE(it != traversal.end()); - EXPECT_TRUE(curr == (*it).first); - EXPECT_TRUE(0 == (*it).second); - ++it; - curr++; - } - - EXPECT_FALSE(it != traversal.end()); - EXPECT_TRUE(std::ceil(end_x) == (*it).first); - EXPECT_TRUE(std::ceil(end_x) == curr); -} diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp deleted file mode 100644 index 9db93e12275..00000000000 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp +++ /dev/null @@ -1,81 +0,0 @@ -#include - -#include -#include - -auto getBox() -> simple_sensor_simulator::primitives::Box -{ - const std::string type("box"); - geometry_msgs::msg::Pose pose; - pose.position.x = 9.0; - pose.position.y = 11.0; - pose.position.z = 17.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - - const float depth = 19.0; - const float width = 23.0; - const float height = 29.0; - simple_sensor_simulator::primitives::Box prim(depth, width, height, pose); - return prim; -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -TEST(OccupancyGridBuilder, add_overLimit) -{ - const double resolution = 1000.0f; - const size_t height = 11; - const size_t width = 13; - const int8_t occupied_cost = 17; - const int8_t invisible_cost = 19; - auto builder = simple_sensor_simulator::OccupancyGridBuilder( - resolution, height, width, occupied_cost, invisible_cost); - auto count_max = std::numeric_limits::max(); - for (int i = 0; i < count_max; i++) { - EXPECT_NO_THROW(builder.add(getBox())); - } - EXPECT_THROW(builder.add(getBox()), std::runtime_error); -} - -TEST(OccupancyGridBuilder, reset) -{ - const double resolution = 1000.0f; - const size_t height = 11; - const size_t width = 13; - const int8_t occupied_cost = 17; - const int8_t invisible_cost = 19; - auto builder = simple_sensor_simulator::OccupancyGridBuilder( - resolution, height, width, occupied_cost, invisible_cost); - auto count_max = std::numeric_limits::max(); - for (int i = 0; i < count_max; i++) { - EXPECT_NO_THROW(builder.add(getBox())); - } - builder.reset(geometry_msgs::msg::Pose{}); - for (int i = 0; i < count_max; i++) { - EXPECT_NO_THROW(builder.add(getBox())); - } - EXPECT_THROW(builder.add(getBox()), std::runtime_error); -} - -TEST(OccupancyGridBuilder, build_empty) -{ - const double resolution = 1000.0f; - const size_t height = 11; - const size_t width = 13; - const int8_t occupied_cost = 17; - const int8_t invisible_cost = 19; - auto builder = simple_sensor_simulator::OccupancyGridBuilder( - resolution, height, width, occupied_cost, invisible_cost); - - builder.build(); - auto grid = builder.get(); - - EXPECT_TRUE(std::all_of(grid.begin(), grid.end(), [&](int elem) { return 0 == elem; })); -} diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp deleted file mode 100644 index 79366a900f2..00000000000 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include - -#include - -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/primitives/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt deleted file mode 100644 index ec4943146b1..00000000000 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -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) - -ament_add_gtest(test_box test_box.cpp) -target_link_libraries(test_box simple_sensor_simulator_component) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp deleted file mode 100644 index 9959173b775..00000000000 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#include - -#include -#include - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -TEST(Box, Box) -{ - const std::string type("name"); - geometry_msgs::msg::Pose pose; - pose.position.x = 9.0; - pose.position.y = 11.0; - pose.position.z = 17.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - - const float depth = 19.0; - const float width = 23.0; - const float height = 29.0; - simple_sensor_simulator::primitives::Box prim(depth, width, height, pose); - - const std::vector hull = prim.get2DConvexHull(); - auto p_min_x = std::min_element( - hull.begin(), hull.end(), - [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) { - return a.x < b.x; - }); - auto p_min_y = std::min_element( - hull.begin(), hull.end(), - [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) { - return a.y < b.y; - }); - auto p_max_x = std::min_element( - hull.begin(), hull.end(), - [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) { - return a.x > b.x; - }); - auto p_max_y = std::min_element( - hull.begin(), hull.end(), - [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) { - return a.y > b.y; - }); - - EXPECT_FALSE(p_min_x == hull.end()); - EXPECT_FALSE(p_min_y == hull.end()); - EXPECT_FALSE(p_max_x == hull.end()); - EXPECT_FALSE(p_max_y == hull.end()); - - EXPECT_TRUE(p_min_x->x == pose.position.x - depth / 2); - EXPECT_TRUE(p_min_y->y == pose.position.y - width / 2); - EXPECT_TRUE(p_max_x->x == pose.position.x + depth / 2); - EXPECT_TRUE(p_max_y->y == pose.position.y + width / 2); -} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.cpp deleted file mode 100644 index e17ec492460..00000000000 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.cpp +++ /dev/null @@ -1,456 +0,0 @@ -#include - -#include -#include - -class JaggedSquare : public simple_sensor_simulator::primitives::Primitive -{ -public: - explicit JaggedSquare( - float depth, float width, float height, const geometry_msgs::msg::Pose & pose) - : simple_sensor_simulator::primitives::Primitive("Box", pose), - depth(depth), - width(width), - height(height) - { - vertices_ = std::vector(8); - - vertices_[0].x = -0.5 * depth; - vertices_[0].y = -0.5 * width; - vertices_[0].z = 0.0 * height; - - vertices_[1].x = 0.0 * depth; - vertices_[1].y = -0.5 * width; - vertices_[1].z = 0.0 * height; - - vertices_[2].x = 0.5 * depth; - vertices_[2].y = -0.5 * width; - vertices_[2].z = 0.0 * height; - - vertices_[3].x = -0.5 * depth; - vertices_[3].y = 0.0 * width; - vertices_[3].z = 0.0 * height; - - vertices_[4].x = 0.0 * depth; - vertices_[4].y = 0.0 * width; - vertices_[4].z = 0.0 * height; - - vertices_[5].x = 0.5 * depth; - vertices_[5].y = 0.0 * width; - vertices_[5].z = 0.0 * height; - - vertices_[6].x = -0.5 * depth; - vertices_[6].y = 0.5 * width; - vertices_[6].z = 0.0 * height; - - vertices_[7].x = 0.0 * depth; - vertices_[7].y = 0.5 * width; - vertices_[7].z = 0.0 * height; - - triangles_ = std::vector(6); - - triangles_[0].v0 = 0; - triangles_[0].v1 = 1; - triangles_[0].v2 = 4; - - triangles_[1].v0 = 1; - triangles_[1].v1 = 4; - triangles_[1].v2 = 3; - - triangles_[2].v0 = 1; - triangles_[2].v1 = 2; - triangles_[2].v2 = 5; - - triangles_[3].v0 = 1; - triangles_[3].v1 = 5; - triangles_[3].v2 = 4; - - triangles_[4].v0 = 3; - triangles_[4].v1 = 4; - triangles_[4].v2 = 7; - - triangles_[5].v0 = 3; - triangles_[5].v1 = 7; - triangles_[5].v2 = 6; - } - std::vector getVertices(geometry_msgs::msg::Pose pose) - { - return transform(pose); - } - ~JaggedSquare() = default; - const float depth; - const float width; - const float height; -}; - -bool is2DSubset( - const std::vector & superset, - const std::vector & subset) -{ - for (const simple_sensor_simulator::Vertex & v_subset : subset) { - auto it = std::find_if( - superset.begin(), superset.end(), [&](const simple_sensor_simulator::Vertex & v_superset) { - return v_superset.x == v_subset.x && v_superset.y == v_subset.y; - }); - if (it == superset.end()) { - return false; - } - } - return true; -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -TEST(Primitive, get2DConvexHull_normal) -{ - const std::string type("name"); - geometry_msgs::msg::Pose pose; - pose.position.x = 9.0; - pose.position.y = 11.0; - pose.position.z = 17.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - - const float depth = 19.0; - const float width = 23.0; - const float height = 29.0; - JaggedSquare prim(depth, width, height, pose); - - std::vector hull = - simple_sensor_simulator::toVertex(prim.get2DConvexHull()); - std::vector hull_superset = prim.getVertex(); - hull_superset.erase(hull_superset.begin() + 4); - - std::vector hull_subset = prim.getVertex(); - hull_subset.erase(hull_subset.begin() + 4); - hull_subset.erase(hull_subset.begin() + 3); - hull_subset.erase(hull_subset.begin() + 1); - - EXPECT_TRUE(hull.size() >= 5); - EXPECT_TRUE(hull_superset.size() == 7); - EXPECT_TRUE(hull_subset.size() == 5); - EXPECT_TRUE(is2DSubset(hull_superset, hull)); - EXPECT_TRUE(is2DSubset(hull, hull_subset)); -} - -TEST(Primitive, get2DConvexHull_withTransform) -{ - const std::string type("name"); - geometry_msgs::msg::Pose pose; - pose.position.x = 9.0; - pose.position.y = 11.0; - pose.position.z = 17.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - - const float depth = 19.0; - const float width = 23.0; - const float height = 29.0; - JaggedSquare prim(depth, width, height, pose); - - geometry_msgs::msg::Pose sensor_pose{}; - sensor_pose.position.x = 11.0; - sensor_pose.position.y = 13.0; - sensor_pose.position.z = 17.0; - sensor_pose.orientation.x = 0.944; - sensor_pose.orientation.y = 0.187; - sensor_pose.orientation.z = 0.187; - sensor_pose.orientation.w = 0.199; - - std::vector hull = - simple_sensor_simulator::toVertex(prim.get2DConvexHull(sensor_pose)); - std::vector hull_superset = prim.getVertices(sensor_pose); - std::vector hull_subset = prim.getVertices(sensor_pose); - - hull_superset.erase(hull_superset.begin() + 4); - - hull_subset.erase(hull_subset.begin() + 4); - hull_subset.erase(hull_subset.begin() + 3); - hull_subset.erase(hull_subset.begin() + 1); - - EXPECT_TRUE(hull.size() >= 5); - EXPECT_TRUE(hull_superset.size() == 7); - EXPECT_TRUE(hull_subset.size() == 5); - EXPECT_TRUE(is2DSubset(hull_superset, hull)); - EXPECT_TRUE(is2DSubset(hull, hull_subset)); -} - -TEST(Primitive, getMin_noTransform) -{ - const std::string type("name"); - geometry_msgs::msg::Pose pose; - pose.position.x = 9.0; - pose.position.y = 11.0; - pose.position.z = 17.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - - const float depth = 19.0; - const float width = 23.0; - const float height = 29.0; - simple_sensor_simulator::primitives::Box prim(depth, width, height, pose); - - const auto axis_x = math::geometry::Axis::X; - const auto axis_y = math::geometry::Axis::Y; - const auto axis_z = math::geometry::Axis::Z; - - const std::optional min_x = prim.getMin(axis_x); - const std::optional min_y = prim.getMin(axis_y); - const std::optional min_z = prim.getMin(axis_z); - - EXPECT_TRUE(min_x.has_value()); - EXPECT_TRUE(min_y.has_value()); - EXPECT_TRUE(min_z.has_value()); - EXPECT_TRUE(min_x.value() == pose.position.x - depth / 2); - EXPECT_TRUE(min_y.value() == pose.position.y - width / 2); - EXPECT_TRUE(min_z.value() == pose.position.z - height / 2); -} - -TEST(Primitive, getMin_emptyPrimitive) -{ - const std::string type("name"); - geometry_msgs::msg::Pose pose; - pose.position.x = 11.0; - pose.position.y = 13.0; - pose.position.z = 17.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - simple_sensor_simulator::primitives::Primitive prim(type, pose); - - const auto axis_x = math::geometry::Axis::X; - const auto axis_y = math::geometry::Axis::Y; - const auto axis_z = math::geometry::Axis::Z; - - const std::optional min_x = prim.getMin(axis_x); - const std::optional min_y = prim.getMin(axis_y); - const std::optional min_z = prim.getMin(axis_z); - - EXPECT_FALSE(min_x.has_value()); - EXPECT_FALSE(min_y.has_value()); - EXPECT_FALSE(min_z.has_value()); -} - -TEST(Primitive, getMin_emptyPrimitiveWithPose) -{ - const std::string type("name"); - geometry_msgs::msg::Pose pose; - pose.position.x = 0.0; - pose.position.y = 0.0; - pose.position.z = 0.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - simple_sensor_simulator::primitives::Primitive prim(type, pose); - geometry_msgs::msg::Pose sensor_pose{}; - sensor_pose.position.x = 11.0; - sensor_pose.position.y = 13.0; - sensor_pose.position.z = 17.0; - sensor_pose.orientation.x = -0.5; - sensor_pose.orientation.y = 0.5; - sensor_pose.orientation.z = 0.5; - sensor_pose.orientation.w = 0.5; - - const auto axis_x = math::geometry::Axis::X; - const auto axis_y = math::geometry::Axis::Y; - const auto axis_z = math::geometry::Axis::Z; - - const std::optional min_x = prim.getMin(axis_x, sensor_pose); - const std::optional min_y = prim.getMin(axis_y, sensor_pose); - const std::optional min_z = prim.getMin(axis_z, sensor_pose); - - EXPECT_FALSE(min_x.has_value()); - EXPECT_FALSE(min_y.has_value()); - EXPECT_FALSE(min_z.has_value()); -} - -TEST(Primitive, getMin_withTransform) -{ - const std::string type("name"); - geometry_msgs::msg::Pose pose; - pose.position.x = 3.0; - pose.position.y = 5.0; - pose.position.z = 7.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - - const float depth = 19.0; - const float width = 23.0; - const float height = 29.0; - simple_sensor_simulator::primitives::Box prim(depth, width, height, pose); - - geometry_msgs::msg::Pose sensor_pose{}; - sensor_pose.position.x = -11.0; - sensor_pose.position.y = -13.0; - sensor_pose.position.z = -17.0; - sensor_pose.orientation.x = -0.5; - sensor_pose.orientation.y = 0.5; - sensor_pose.orientation.z = 0.5; - sensor_pose.orientation.w = 0.5; - - const auto axis_x = math::geometry::Axis::X; - const auto axis_y = math::geometry::Axis::Y; - const auto axis_z = math::geometry::Axis::Z; - - const std::optional min_x = prim.getMin(axis_x, sensor_pose); - const std::optional min_y = prim.getMin(axis_y, sensor_pose); - const std::optional min_z = prim.getMin(axis_z, sensor_pose); - - EXPECT_TRUE(min_x.has_value()); - EXPECT_TRUE(min_y.has_value()); - EXPECT_TRUE(min_z.has_value()); - - EXPECT_TRUE(min_x.value() == pose.position.x - sensor_pose.position.x - height / 2); - EXPECT_TRUE(min_y.value() == pose.position.y - sensor_pose.position.y - depth / 2); - EXPECT_TRUE(min_z.value() == pose.position.z - sensor_pose.position.z - width / 2); -} - -TEST(Primitive, getMax_noTransform) -{ - const std::string type("name"); - geometry_msgs::msg::Pose pose; - pose.position.x = 9.0; - pose.position.y = 11.0; - pose.position.z = 17.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - - const float depth = 19.0; - const float width = 23.0; - const float height = 29.0; - simple_sensor_simulator::primitives::Box prim(depth, width, height, pose); - - const auto axis_x = math::geometry::Axis::X; - const auto axis_y = math::geometry::Axis::Y; - const auto axis_z = math::geometry::Axis::Z; - - const std::optional max_x = prim.getMax(axis_x); - const std::optional max_y = prim.getMax(axis_y); - const std::optional max_z = prim.getMax(axis_z); - - EXPECT_TRUE(max_x.has_value()); - EXPECT_TRUE(max_y.has_value()); - EXPECT_TRUE(max_z.has_value()); - EXPECT_TRUE(max_x.value() == pose.position.x + depth / 2); - EXPECT_TRUE(max_y.value() == pose.position.y + width / 2); - EXPECT_TRUE(max_z.value() == pose.position.z + height / 2); -} - -TEST(Primitive, getMax_emptyPrimitive) -{ - const std::string type("name"); - geometry_msgs::msg::Pose pose; - pose.position.x = 11.0; - pose.position.y = 13.0; - pose.position.z = 17.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - simple_sensor_simulator::primitives::Primitive prim(type, pose); - - const auto axis_x = math::geometry::Axis::X; - const auto axis_y = math::geometry::Axis::Y; - const auto axis_z = math::geometry::Axis::Z; - - const std::optional max_x = prim.getMax(axis_x); - const std::optional max_y = prim.getMax(axis_y); - const std::optional max_z = prim.getMax(axis_z); - - EXPECT_FALSE(max_x.has_value()); - EXPECT_FALSE(max_y.has_value()); - EXPECT_FALSE(max_z.has_value()); -} - -TEST(Primitive, getMax_emptyPrimitiveWithPose) -{ - const std::string type("name"); - geometry_msgs::msg::Pose pose; - pose.position.x = 0.0; - pose.position.y = 0.0; - pose.position.z = 0.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - simple_sensor_simulator::primitives::Primitive prim(type, pose); - geometry_msgs::msg::Pose sensor_pose{}; - sensor_pose.position.x = 11.0; - sensor_pose.position.y = 13.0; - sensor_pose.position.z = 17.0; - sensor_pose.orientation.x = -0.5; - sensor_pose.orientation.y = 0.5; - sensor_pose.orientation.z = 0.5; - sensor_pose.orientation.w = 0.5; - - const auto axis_x = math::geometry::Axis::X; - const auto axis_y = math::geometry::Axis::Y; - const auto axis_z = math::geometry::Axis::Z; - - const std::optional max_x = prim.getMax(axis_x, sensor_pose); - const std::optional max_y = prim.getMax(axis_y, sensor_pose); - const std::optional max_z = prim.getMax(axis_z, sensor_pose); - - EXPECT_FALSE(max_x.has_value()); - EXPECT_FALSE(max_y.has_value()); - EXPECT_FALSE(max_z.has_value()); -} - -TEST(Primitive, getMax_withTransform) -{ - const std::string type("name"); - geometry_msgs::msg::Pose pose; - pose.position.x = 3.0; - pose.position.y = 5.0; - pose.position.z = 7.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - - const float depth = 19.0; - const float width = 23.0; - const float height = 29.0; - simple_sensor_simulator::primitives::Box prim(depth, width, height, pose); - - geometry_msgs::msg::Pose sensor_pose{}; - sensor_pose.position.x = -11.0; - sensor_pose.position.y = -13.0; - sensor_pose.position.z = -17.0; - sensor_pose.orientation.x = -0.5; - sensor_pose.orientation.y = 0.5; - sensor_pose.orientation.z = 0.5; - sensor_pose.orientation.w = 0.5; - - const auto axis_x = math::geometry::Axis::X; - const auto axis_y = math::geometry::Axis::Y; - const auto axis_z = math::geometry::Axis::Z; - - const std::optional max_x = prim.getMax(axis_x, sensor_pose); - const std::optional max_y = prim.getMax(axis_y, sensor_pose); - const std::optional max_z = prim.getMax(axis_z, sensor_pose); - - EXPECT_TRUE(max_x.has_value()); - EXPECT_TRUE(max_y.has_value()); - EXPECT_TRUE(max_z.has_value()); - - EXPECT_TRUE(max_x.value() == pose.position.x - sensor_pose.position.x + height / 2); - EXPECT_TRUE(max_y.value() == pose.position.y - sensor_pose.position.y + depth / 2); - EXPECT_TRUE(max_z.value() == pose.position.z - sensor_pose.position.z + width / 2); -} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp deleted file mode 100644 index 2a235899afb..00000000000 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp +++ /dev/null @@ -1,95 +0,0 @@ -#include - -#include - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -TEST(Vertex, toVertex_onePoint) -{ - geometry_msgs::msg::Point point{}; - point.x = -11.0; - point.y = -13.0; - point.z = -17.0; - - simple_sensor_simulator::Vertex v = simple_sensor_simulator::toVertex(point); - - EXPECT_EQ(point.x, v.x); - EXPECT_EQ(point.y, v.y); - EXPECT_EQ(point.z, v.z); -} - -TEST(Vertex, toVertex_manyPoints) -{ - std::vector points{}; - for (int i = 0; i < 10; i++) { - geometry_msgs::msg::Point point{}; - point.x = -11.0 * i; - point.y = -13.0 * i; - point.z = -17.0 * i; - points.push_back(point); - } - - std::vector vertices = simple_sensor_simulator::toVertex(points); - - for (int i = 0; i < 10; i++) { - EXPECT_EQ(points[i].x, vertices[i].x); - EXPECT_EQ(points[i].y, vertices[i].y); - EXPECT_EQ(points[i].z, vertices[i].z); - } -} - -TEST(Vertex, toVertex_empty) -{ - std::vector points{}; - - std::vector vertices = simple_sensor_simulator::toVertex(points); - - EXPECT_TRUE(0 == vertices.size()); -} - -TEST(Vertex, toPoint_oneVertex) -{ - simple_sensor_simulator::Vertex v{}; - v.x = -11.0; - v.y = -13.0; - v.z = -17.0; - - geometry_msgs::msg::Point point = simple_sensor_simulator::toPoint(v); - - EXPECT_EQ(point.x, v.x); - EXPECT_EQ(point.y, v.y); - EXPECT_EQ(point.z, v.z); -} - -TEST(Vertex, toPoints_manyVertices) -{ - std::vector vertices{}; - for (int i = 0; i < 10; i++) { - simple_sensor_simulator::Vertex v{}; - v.x = -11.0 * i; - v.y = -13.0 * i; - v.z = -17.0 * i; - vertices.push_back(v); - } - - std::vector points = simple_sensor_simulator::toPoints(vertices); - - for (int i = 0; i < 10; i++) { - EXPECT_EQ(points[i].x, vertices[i].x); - EXPECT_EQ(points[i].y, vertices[i].y); - EXPECT_EQ(points[i].z, vertices[i].z); - } -} - -TEST(Vertex, toPoints_empty) -{ - std::vector vertices{}; - - std::vector points = simple_sensor_simulator::toPoints(vertices); - - EXPECT_EQ(points.size(), 0); -} diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp deleted file mode 100644 index 00e5136961b..00000000000 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - testing::InitGoogleTest(&argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} - -TEST(SensorSimulation, attachLidarSensor_wrongArchitecture) -{ - const double current_simulation_time = 3.0; - const simulation_api_schema::LidarConfiguration configuration{}; - - rclcpp::NodeOptions options; - rclcpp::Node node{"name", options}; - - auto sim = simple_sensor_simulator::SensorSimulation(); - - EXPECT_THROW( - sim.attachLidarSensor(current_simulation_time, configuration, node), std::runtime_error); -} - -TEST(SensorSimulation, attachDetectionSensor_wrongArchitecture) -{ - const double current_simulation_time = 3.0; - const simulation_api_schema::DetectionSensorConfiguration configuration{}; - - rclcpp::NodeOptions options; - rclcpp::Node node{"name", options}; - - auto sim = simple_sensor_simulator::SensorSimulation(); - - EXPECT_THROW( - sim.attachDetectionSensor(current_simulation_time, configuration, node), std::runtime_error); -} - -TEST(SensorSimulation, attachOccupancyGridSensor_wrongArchitecture) -{ - const double current_simulation_time = 3.0; - const simulation_api_schema::OccupancyGridSensorConfiguration configuration{}; - - rclcpp::NodeOptions options; - rclcpp::Node node{"name", options}; - - auto sim = simple_sensor_simulator::SensorSimulation(); - - EXPECT_THROW( - sim.attachOccupancyGridSensor(current_simulation_time, configuration, node), - std::runtime_error); -} diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/CMakeLists.txt deleted file mode 100644 index 24a41741ce5..00000000000 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -ament_add_gtest(test_sim_model_interface test_sim_model_interface.cpp) -target_link_libraries(test_sim_model_interface simple_sensor_simulator_component) - -ament_add_gtest(test_sim_model_ideal_steer_vel test_sim_model_ideal_steer_vel.cpp) -target_link_libraries(test_sim_model_ideal_steer_vel simple_sensor_simulator_component) - -ament_add_gtest(test_sim_model_ideal_steer_acc test_sim_model_ideal_steer_acc.cpp) -target_link_libraries(test_sim_model_ideal_steer_acc simple_sensor_simulator_component) - -ament_add_gtest(test_sim_model_ideal_steer_acc_geared test_sim_model_ideal_steer_acc_geared.cpp) -target_link_libraries(test_sim_model_ideal_steer_acc_geared simple_sensor_simulator_component) \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc.cpp deleted file mode 100644 index 6cbc471c58f..00000000000 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include - -#include - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -TEST(SimModelIdealSteerAcc, update_nonZeroDT) -{ - const double wheelbase = 1.0; - auto model = SimModelIdealSteerAcc(wheelbase); - SimModelInterface * p_model = &model; - Eigen::VectorXd x_state(p_model->getDimX()); - Eigen::VectorXd u_input(p_model->getDimU()); - const double start_x = 3.0; - const double start_y = 5.0; - const double start_yaw = 0.0; - const double start_vx = 11.0; - const double start_ax_des = 13.0; - const double start_steer_des = 0.0; - x_state << start_x, start_y, start_yaw, start_vx; - u_input << start_ax_des, start_steer_des; - - p_model->setInput(u_input); - p_model->setState(x_state); - - const double dt = 1.0; - for (int i = 0; i < 10; i++) { - double current_vx = start_vx + i * dt * start_ax_des; - double current_x = start_x + i * dt * start_vx + 0.5 * i * i * dt * start_ax_des; - EXPECT_TRUE(std::abs(current_x - p_model->getX()) < 1e-5); - EXPECT_TRUE(std::abs(current_vx - p_model->getVx()) < 1e-5); - EXPECT_TRUE(start_y == p_model->getY()); - EXPECT_TRUE(start_yaw == p_model->getYaw()); - p_model->update(dt); - } -} - -TEST(SimModelIdealSteerAcc, update_zeroDT) -{ - const double wheelbase = 1.0; - auto model = SimModelIdealSteerAcc(wheelbase); - SimModelInterface * p_model = &model; - Eigen::VectorXd x_state(p_model->getDimX()); - Eigen::VectorXd u_input(p_model->getDimU()); - const double start_x = 3.0; - const double start_y = 5.0; - const double start_yaw = 7.0; - const double start_vx = 11.0; - const double start_ax_des = 13.0; - const double start_steer_des = 17.0; - x_state << start_x, start_y, start_yaw, start_vx; - u_input << start_ax_des, start_steer_des; - - p_model->setInput(u_input); - p_model->setState(x_state); - - const double dt = 0.0; - for (int i = 0; i < 10; i++) { - EXPECT_TRUE(start_x == p_model->getX()); - EXPECT_TRUE(start_y == p_model->getY()); - EXPECT_TRUE(start_yaw == p_model->getYaw()); - EXPECT_TRUE(start_vx == p_model->getVx()); - p_model->update(dt); - } -} - -TEST(SimModelIdealSteerAcc, calcModel_zeroWheelbase) -{ - const double wheelbase = 0.0; - auto model = SimModelIdealSteerAcc(wheelbase); - SimModelInterface * p_model = &model; - Eigen::VectorXd x_state(p_model->getDimX()); - Eigen::VectorXd u_input(p_model->getDimU()); - const double start_x = 3.0; - const double start_y = 5.0; - const double start_yaw = 7.0; - const double start_vx = 11.0; - const double start_ax_des = 13.0; - const double start_steer_des = 17.0; - x_state << start_x, start_y, start_yaw, start_vx; - u_input << start_ax_des, start_steer_des; - - auto out_x_state = p_model->calcModel(x_state, u_input); - p_model->setState(out_x_state); - - EXPECT_TRUE(p_model->getYaw() < std::numeric_limits::infinity()); -} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc_geared.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc_geared.cpp deleted file mode 100644 index 0db9635539c..00000000000 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_acc_geared.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include - -#include - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -TEST(SimModelIdealSteerAccGeared, update_nonZeroDT) -{ - const double wheelbase = 1.0; - auto model = SimModelIdealSteerAccGeared(wheelbase); - SimModelInterface * p_model = &model; - Eigen::VectorXd x_state(p_model->getDimX()); - Eigen::VectorXd u_input(p_model->getDimU()); - const double start_x = 3.0; - const double start_y = 5.0; - const double start_yaw = 0.0; - const double start_vx = 11.0; - const double start_ax_des = 13.0; - const double start_steer_des = 0.0; - x_state << start_x, start_y, start_yaw, start_vx; - u_input << start_ax_des, start_steer_des; - - p_model->setInput(u_input); - p_model->setState(x_state); - - const double dt = 1.0; - for (int i = 0; i < 10; i++) { - double current_vx = start_vx + i * dt * start_ax_des; - double current_x = start_x + i * dt * start_vx + 0.5 * i * i * dt * start_ax_des; - EXPECT_TRUE(std::abs(current_x - p_model->getX()) < 1e-5); - EXPECT_TRUE(std::abs(current_vx - p_model->getVx()) < 1e-5); - EXPECT_TRUE(start_y == p_model->getY()); - EXPECT_TRUE(start_yaw == p_model->getYaw()); - p_model->update(dt); - } -} - -TEST(SimModelIdealSteerAccGeared, update_zeroDT) -{ - const double wheelbase = 1.0; - auto model = SimModelIdealSteerAccGeared(wheelbase); - SimModelInterface * p_model = &model; - Eigen::VectorXd x_state(p_model->getDimX()); - Eigen::VectorXd u_input(p_model->getDimU()); - const double start_x = 3.0; - const double start_y = 5.0; - const double start_yaw = 7.0; - const double start_vx = 11.0; - const double start_ax_des = 13.0; - const double start_steer_des = 17.0; - x_state << start_x, start_y, start_yaw, start_vx; - u_input << start_ax_des, start_steer_des; - - p_model->setInput(u_input); - p_model->setState(x_state); - - const double dt = 0.0; - for (int i = 0; i < 10; i++) { - EXPECT_TRUE(start_x == p_model->getX()); - EXPECT_TRUE(start_y == p_model->getY()); - EXPECT_TRUE(start_yaw == p_model->getYaw()); - EXPECT_TRUE(start_vx == p_model->getVx()); - p_model->update(dt); - } -} - -TEST(SimModelIdealSteerAccGeared, calcModel_zeroWheelbase) -{ - const double wheelbase = 0.0; - auto model = SimModelIdealSteerAccGeared(wheelbase); - SimModelInterface * p_model = &model; - Eigen::VectorXd x_state(p_model->getDimX()); - Eigen::VectorXd u_input(p_model->getDimU()); - const double start_x = 3.0; - const double start_y = 5.0; - const double start_yaw = 7.0; - const double start_vx = 11.0; - const double start_ax_des = 13.0; - const double start_steer_des = 17.0; - x_state << start_x, start_y, start_yaw, start_vx; - u_input << start_ax_des, start_steer_des; - - auto out_x_state = p_model->calcModel(x_state, u_input); - p_model->setState(out_x_state); - - EXPECT_TRUE(p_model->getYaw() < std::numeric_limits::infinity()); -} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_vel.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_vel.cpp deleted file mode 100644 index f0089fddbe1..00000000000 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_ideal_steer_vel.cpp +++ /dev/null @@ -1,87 +0,0 @@ -#include - -#include - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -/* -0. dt: division by 0 -1. wheelbase: division by 0 -2. unintuitive choice of storing the parameters -3. unsure of how calcModel should work. initial position does not influence the result. -*/ - -TEST(SimModelIdealSteerVel, update_nonZeroDT) -{ - const double wheelbase = 1.0; - auto model = SimModelIdealSteerVel(wheelbase); - SimModelInterface * p_model = &model; - const double dt = 0.01; - Eigen::VectorXd x_state(p_model->getDimX()); - Eigen::VectorXd u_input(p_model->getDimU()); - const double start_vx = 3.0; - const double start_x = 5.0; - x_state << start_x, 0.0, 0.0; - u_input << start_vx, 0.0; - p_model->setInput(u_input); - p_model->setState(x_state); - - for (int i = 0; i < 10; i++) { - EXPECT_TRUE(std::abs(start_x + i * dt * start_vx - p_model->getX()) < 1e-5); - EXPECT_TRUE(0.0 == p_model->getY()); - EXPECT_TRUE(0.0 == p_model->getYaw()); - EXPECT_TRUE(start_vx == p_model->getVx()); - p_model->update(dt); - } - EXPECT_TRUE(p_model->getAx() < std::numeric_limits::infinity()); -} - -TEST(SimModelIdealSteerVel, update_zeroDT) -{ - const double wheelbase = 1.0; - auto model = SimModelIdealSteerVel(wheelbase); - SimModelInterface * p_model = &model; - const double dt = 0.0; - Eigen::VectorXd x_state(p_model->getDimX()); - Eigen::VectorXd u_input(p_model->getDimU()); - const double start_vx = 3.0; - const double start_x = 5.0; - x_state << start_x, 0.0, 0.0; - u_input << start_vx, 0.0; - p_model->setInput(u_input); - p_model->setState(x_state); - - for (int i = 0; i < 10; i++) { - EXPECT_TRUE(std::abs(start_x + i * dt * start_vx - p_model->getX()) < 1e-5); - EXPECT_TRUE(0.0 == p_model->getY()); - EXPECT_TRUE(0.0 == p_model->getYaw()); - EXPECT_TRUE(start_vx == p_model->getVx()); - p_model->update(dt); - } - EXPECT_TRUE(p_model->getAx() < std::numeric_limits::infinity()); -} - -TEST(SimModelIdealSteerVel, calcModel_zeroWheelbase) -{ - const double wheelbase = 0.0; - auto model = SimModelIdealSteerVel(wheelbase); - SimModelInterface * p_model = &model; - Eigen::VectorXd x_state(p_model->getDimX()); - Eigen::VectorXd u_input(p_model->getDimU()); - const double start_x = 3.0; - const double start_y = 5.0; - const double start_yaw = 7.0; - const double start_vx = 11.0; - const double start_steer = 13.0; - x_state << start_x, start_y, start_yaw; - u_input << start_vx, start_steer; - - auto out_x_state = p_model->calcModel(x_state, u_input); - p_model->setState(out_x_state); - - EXPECT_TRUE(p_model->getYaw() < std::numeric_limits::infinity()); -} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_interface.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_interface.cpp deleted file mode 100644 index 034f4113904..00000000000 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/vehicle_model/test_sim_model_interface.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include - -#include - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file From 067b01c5d97afd774347d4fbc37c525c57fdfa12 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 6 Jun 2024 09:42:52 +0200 Subject: [PATCH 038/118] remove conversions tests --- .../simulation_interface/CMakeLists.txt | 2 + .../simulation_interface/conversions.hpp | 9 +- .../simulation_interface/test/CMakeLists.txt | 2 - .../test/src/test_conversions.cpp | 1604 ----------------- .../test/test_conversions.cpp | 534 ++++++ .../include/traffic_simulator/job/job.hpp | 2 +- 6 files changed, 543 insertions(+), 1610 deletions(-) delete mode 100644 simulation/simulation_interface/test/CMakeLists.txt delete mode 100644 simulation/simulation_interface/test/src/test_conversions.cpp create mode 100644 simulation/simulation_interface/test/test_conversions.cpp diff --git a/simulation/simulation_interface/CMakeLists.txt b/simulation/simulation_interface/CMakeLists.txt index c00c98db57f..ac11baa6019 100644 --- a/simulation/simulation_interface/CMakeLists.txt +++ b/simulation/simulation_interface/CMakeLists.txt @@ -72,6 +72,8 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_conversion test/test_conversions.cpp) + target_link_libraries(test_conversion simulation_interface) add_subdirectory(test) endif() diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index bd66bb1e583..4ec8320616b 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -192,7 +192,8 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr { + auto convert_color = [](auto color) constexpr + { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -207,7 +208,8 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr { + auto convert_shape = [](auto shape) constexpr + { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -235,7 +237,8 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr { + auto convert_status = [](auto status) constexpr + { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; diff --git a/simulation/simulation_interface/test/CMakeLists.txt b/simulation/simulation_interface/test/CMakeLists.txt deleted file mode 100644 index faf6a8f76c7..00000000000 --- a/simulation/simulation_interface/test/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -ament_add_gtest(test_conversion src/test_conversions.cpp) -target_link_libraries(test_conversion simulation_interface) diff --git a/simulation/simulation_interface/test/src/test_conversions.cpp b/simulation/simulation_interface/test/src/test_conversions.cpp deleted file mode 100644 index cf64821f647..00000000000 --- a/simulation/simulation_interface/test/src/test_conversions.cpp +++ /dev/null @@ -1,1604 +0,0 @@ -// 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 -#include - -#include "../include/expect_equal_macros.hpp" - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -TEST(Conversion, toProto_Point) -{ - geometry_msgs::msg::Point src_msg; - geometry_msgs::Point dst_proto; - src_msg.y = 3.0; - src_msg.z = 5.0; - src_msg.z = 7.0; - simulation_interface::toProto(src_msg, dst_proto); - EXPECT_POINT_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_Point) -{ - geometry_msgs::msg::Point init_msg; - geometry_msgs::Point src_proto; - geometry_msgs::msg::Point dst_msg; - init_msg.y = 3.0; - init_msg.z = 5.0; - init_msg.z = 7.0; - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - EXPECT_POINT_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_Quaternion) -{ - geometry_msgs::msg::Quaternion src_msg; - geometry_msgs::Quaternion dst_proto; - src_msg.x = 3.0; - src_msg.y = 5.0; - src_msg.z = 7.0; - src_msg.w = 11.0; - simulation_interface::toProto(src_msg, dst_proto); - EXPECT_QUATERNION_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_Quaternion) -{ - geometry_msgs::msg::Quaternion init_msg; - geometry_msgs::Quaternion src_proto; - geometry_msgs::msg::Quaternion dst_msg; - init_msg.x = 3.0; - init_msg.y = 5.0; - init_msg.z = 7.0; - init_msg.w = 11.0; - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - EXPECT_QUATERNION_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_Pose) -{ - geometry_msgs::msg::Pose src_msg; - geometry_msgs::Pose dst_proto; - src_msg.position.x = 3.0; - src_msg.position.y = 5.0; - src_msg.position.z = 7.0; - src_msg.orientation.x = 11.0; - src_msg.orientation.y = 13.0; - src_msg.orientation.z = 17.0; - src_msg.orientation.w = 19.0; - simulation_interface::toProto(src_msg, dst_proto); - EXPECT_POSE_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_Pose) -{ - geometry_msgs::msg::Pose init_msg; - geometry_msgs::Pose src_proto; - geometry_msgs::msg::Pose dst_msg; - init_msg.position.x = 3.0; - init_msg.position.y = 5.0; - init_msg.position.z = 7.0; - init_msg.orientation.x = 11.0; - init_msg.orientation.y = 13.0; - init_msg.orientation.z = 17.0; - init_msg.orientation.w = 19.0; - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - EXPECT_POSE_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_Vector3) -{ - geometry_msgs::msg::Vector3 src_msg; - geometry_msgs::Vector3 dst_proto; - src_msg.x = 3.0; - src_msg.y = 5.0; - src_msg.z = 7.0; - simulation_interface::toProto(src_msg, dst_proto); - EXPECT_VECTOR3_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_Vector3) -{ - geometry_msgs::msg::Vector3 init_msg; - geometry_msgs::Vector3 src_proto; - geometry_msgs::msg::Vector3 dst_msg; - init_msg.x = 3.0; - init_msg.y = 5.0; - init_msg.z = 7.0; - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - EXPECT_VECTOR3_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_Twist) -{ - geometry_msgs::msg::Twist src_msg; - geometry_msgs::Twist dst_proto; - src_msg.linear.x = 3.0; - src_msg.linear.y = 5.0; - src_msg.linear.z = 7.0; - simulation_interface::toProto(src_msg, dst_proto); - EXPECT_TWIST_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_Twist) -{ - geometry_msgs::msg::Twist init_msg; - geometry_msgs::Twist src_proto; - geometry_msgs::msg::Twist dst_msg; - init_msg.linear.x = 3.0; - init_msg.linear.y = 5.0; - init_msg.linear.z = 7.0; - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - EXPECT_TWIST_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_Accel) -{ - geometry_msgs::msg::Accel src_msg; - geometry_msgs::Accel dst_proto; - src_msg.linear.x = 3.0; - src_msg.linear.y = 5.0; - src_msg.linear.z = 7.0; - simulation_interface::toProto(src_msg, dst_proto); - EXPECT_ACCEL_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_Accel) -{ - geometry_msgs::msg::Accel init_msg; - geometry_msgs::Accel src_proto; - geometry_msgs::msg::Accel dst_msg; - init_msg.linear.x = 3.0; - init_msg.linear.y = 5.0; - init_msg.linear.z = 7.0; - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - EXPECT_ACCEL_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_Performance) -{ - traffic_simulator_msgs::msg::Performance src_msg; - traffic_simulator_msgs::Performance dst_proto; - src_msg.max_speed = 3.0; - src_msg.max_deceleration = 5.0; - src_msg.max_acceleration = 7.0; - src_msg.max_deceleration_rate = 11.0; - src_msg.max_acceleration_rate = 13.0; - simulation_interface::toProto(src_msg, dst_proto); - EXPECT_PERFORMANCE_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_Performance) -{ - traffic_simulator_msgs::msg::Performance init_msg; - traffic_simulator_msgs::Performance src_proto; - traffic_simulator_msgs::msg::Performance dst_msg; - init_msg.max_speed = 3.0; - init_msg.max_deceleration = 5.0; - init_msg.max_acceleration = 7.0; - init_msg.max_deceleration_rate = 11.0; - init_msg.max_acceleration_rate = 13.0; - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - EXPECT_PERFORMANCE_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_Axle) -{ - traffic_simulator_msgs::msg::Axle src_msg; - traffic_simulator_msgs::Axle dst_proto; - src_msg.max_steering = 3.0; - src_msg.position_x = 5.0; - src_msg.position_z = 7.0; - src_msg.track_width = 11.0; - src_msg.wheel_diameter = 13.0; - simulation_interface::toProto(src_msg, dst_proto); - EXPECT_AXLE_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_Axle) -{ - traffic_simulator_msgs::msg::Axle init_msg; - traffic_simulator_msgs::Axle src_proto; - traffic_simulator_msgs::msg::Axle dst_msg; - init_msg.max_steering = 3.0; - init_msg.position_x = 5.0; - init_msg.position_z = 7.0; - init_msg.track_width = 11.0; - init_msg.wheel_diameter = 13.0; - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - EXPECT_AXLE_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_Axles) -{ - traffic_simulator_msgs::msg::Axles src_msg; - traffic_simulator_msgs::Axles dst_proto; - src_msg.front_axle.max_steering = 3.0; - src_msg.front_axle.position_x = 5.0; - src_msg.front_axle.position_z = 7.0; - src_msg.front_axle.track_width = 11.0; - src_msg.front_axle.wheel_diameter = 13.0; - src_msg.rear_axle.max_steering = 17.0; - src_msg.rear_axle.position_x = 19.0; - src_msg.rear_axle.position_z = 23.0; - src_msg.rear_axle.track_width = 29.0; - src_msg.rear_axle.wheel_diameter = 31.0; - simulation_interface::toProto(src_msg, dst_proto); - EXPECT_AXLES_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_Axles) -{ - traffic_simulator_msgs::msg::Axles init_msg; - traffic_simulator_msgs::Axles src_proto; - traffic_simulator_msgs::msg::Axles dst_msg; - init_msg.front_axle.max_steering = 3.0; - init_msg.front_axle.position_x = 5.0; - init_msg.front_axle.position_z = 7.0; - init_msg.front_axle.track_width = 11.0; - init_msg.front_axle.wheel_diameter = 13.0; - init_msg.rear_axle.max_steering = 17.0; - init_msg.rear_axle.position_x = 19.0; - init_msg.rear_axle.position_z = 23.0; - init_msg.rear_axle.track_width = 29.0; - init_msg.rear_axle.wheel_diameter = 31.0; - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - EXPECT_AXLES_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_BoundingBox) -{ - traffic_simulator_msgs::msg::BoundingBox src_msg; - traffic_simulator_msgs::BoundingBox dst_proto; - src_msg.center.x = 3.0; - src_msg.center.y = 5.0; - src_msg.center.z = 7.0; - src_msg.dimensions.x = 11.0; - src_msg.dimensions.y = 13.0; - src_msg.dimensions.z = 17.0; - simulation_interface::toProto(src_msg, dst_proto); - EXPECT_BOUNDING_BOX_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_BoundingBox) -{ - traffic_simulator_msgs::msg::BoundingBox init_msg; - traffic_simulator_msgs::BoundingBox src_proto; - traffic_simulator_msgs::msg::BoundingBox dst_msg; - init_msg.center.x = 3.0; - init_msg.center.y = 5.0; - init_msg.center.z = 7.0; - init_msg.dimensions.x = 11.0; - init_msg.dimensions.y = 13.0; - init_msg.dimensions.z = 17.0; - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - EXPECT_BOUNDING_BOX_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_VehicleParameters) -{ - traffic_simulator_msgs::msg::VehicleParameters src_msg; - traffic_simulator_msgs::VehicleParameters dst_proto; - src_msg.name = "test"; - traffic_simulator_msgs::msg::BoundingBox box_msg; - box_msg.center.x = 3.0; - box_msg.center.y = 5.0; - box_msg.center.z = 7.0; - box_msg.dimensions.x = 11.0; - box_msg.dimensions.y = 13.0; - box_msg.dimensions.z = 17.0; - src_msg.bounding_box = box_msg; - traffic_simulator_msgs::msg::Performance performance_msg; - performance_msg.max_speed = 19.0; - performance_msg.max_deceleration = 23.0; - src_msg.performance = performance_msg; - traffic_simulator_msgs::msg::Axle axle_msg; - axle_msg.max_steering = 29.0; - axle_msg.position_x = 31.0; - axle_msg.position_z = 37.0; - axle_msg.track_width = 41.0; - axle_msg.wheel_diameter = 43.0; - src_msg.axles.front_axle = axle_msg; - src_msg.axles.rear_axle = axle_msg; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_VEHICLE_PARAMETERS_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_VehicleParameters) -{ - traffic_simulator_msgs::msg::VehicleParameters init_msg; - traffic_simulator_msgs::VehicleParameters src_proto; - traffic_simulator_msgs::msg::VehicleParameters dst_msg; - init_msg.name = "test"; - traffic_simulator_msgs::msg::BoundingBox box_msg; - box_msg.center.x = 3.0; - box_msg.center.y = 5.0; - box_msg.center.z = 7.0; - box_msg.dimensions.x = 11.0; - box_msg.dimensions.y = 13.0; - box_msg.dimensions.z = 17.0; - init_msg.bounding_box = box_msg; - traffic_simulator_msgs::msg::Performance performance_msg; - performance_msg.max_speed = 19.0; - performance_msg.max_deceleration = 23.0; - init_msg.performance = performance_msg; - traffic_simulator_msgs::msg::Axle axle_msg; - axle_msg.max_steering = 29.0; - axle_msg.position_x = 31.0; - axle_msg.position_z = 37.0; - axle_msg.track_width = 41.0; - axle_msg.wheel_diameter = 43.0; - init_msg.axles.front_axle = axle_msg; - init_msg.axles.rear_axle = axle_msg; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_VEHICLE_PARAMETERS_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_PedestrianParameters) -{ - traffic_simulator_msgs::msg::PedestrianParameters src_msg; - traffic_simulator_msgs::PedestrianParameters dst_proto; - src_msg.name = "test"; - traffic_simulator_msgs::msg::BoundingBox box_msg; - box_msg.center.x = 3.0; - box_msg.center.y = 5.0; - box_msg.center.z = 7.0; - box_msg.dimensions.x = 11.0; - box_msg.dimensions.y = 13.0; - box_msg.dimensions.z = 17.0; - src_msg.bounding_box = box_msg; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_PEDESTRIAN_PARAMETERS_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_PedestrianParameters) -{ - traffic_simulator_msgs::msg::PedestrianParameters init_msg; - traffic_simulator_msgs::PedestrianParameters src_proto; - traffic_simulator_msgs::msg::PedestrianParameters dst_msg; - init_msg.name = "test"; - traffic_simulator_msgs::msg::BoundingBox box_msg; - box_msg.center.x = 3.0; - box_msg.center.y = 5.0; - box_msg.center.z = 7.0; - box_msg.dimensions.x = 11.0; - box_msg.dimensions.y = 13.0; - box_msg.dimensions.z = 17.0; - init_msg.bounding_box = box_msg; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_PEDESTRIAN_PARAMETERS_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_MiscObjectParameters) -{ - traffic_simulator_msgs::msg::MiscObjectParameters src_msg; - traffic_simulator_msgs::MiscObjectParameters dst_proto; - traffic_simulator_msgs::msg::BoundingBox box_msg; - box_msg.center.x = 3.0; - box_msg.center.y = 5.0; - box_msg.center.z = 7.0; - box_msg.dimensions.x = 11.0; - box_msg.dimensions.y = 13.0; - box_msg.dimensions.z = 17.0; - src_msg.bounding_box = box_msg; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_MISC_OBJECT_PARAMETERS_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_MiscObjectParameters) -{ - traffic_simulator_msgs::msg::MiscObjectParameters init_msg; - traffic_simulator_msgs::MiscObjectParameters src_proto; - traffic_simulator_msgs::msg::MiscObjectParameters dst_msg; - traffic_simulator_msgs::msg::BoundingBox box_msg; - box_msg.center.x = 3.0; - box_msg.center.y = 5.0; - box_msg.center.z = 7.0; - box_msg.dimensions.x = 11.0; - box_msg.dimensions.y = 13.0; - box_msg.dimensions.z = 17.0; - init_msg.bounding_box = box_msg; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_MISC_OBJECT_PARAMETERS_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_ActionStatus) -{ - traffic_simulator_msgs::msg::ActionStatus src_msg; - traffic_simulator_msgs::ActionStatus dst_proto; - - src_msg.current_action = "test"; - src_msg.twist.linear.x = 3.0; - src_msg.twist.linear.y = 5.0; - src_msg.twist.linear.z = 7.0; - src_msg.twist.angular.x = 11.0; - src_msg.twist.angular.y = 13.0; - src_msg.twist.angular.z = 17.0; - - src_msg.accel.linear.x = 19.0; - src_msg.accel.linear.y = 23.0; - src_msg.accel.linear.z = 29.0; - src_msg.accel.angular.x = 31.0; - src_msg.accel.angular.y = 37.0; - src_msg.accel.angular.z = 41.0; - - simulation_interface::toProto(src_msg, dst_proto); - - EXPECT_ACTION_STATUS_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_ActionStatus) -{ - traffic_simulator_msgs::msg::ActionStatus init_msg; - traffic_simulator_msgs::ActionStatus src_proto; - traffic_simulator_msgs::msg::ActionStatus dst_msg; - - init_msg.current_action = "test"; - init_msg.twist.linear.x = 3.0; - init_msg.twist.linear.y = 5.0; - init_msg.twist.linear.z = 7.0; - init_msg.twist.angular.x = 11.0; - init_msg.twist.angular.y = 13.0; - init_msg.twist.angular.z = 17.0; - - init_msg.accel.linear.x = 19.0; - init_msg.accel.linear.y = 23.0; - init_msg.accel.linear.z = 29.0; - init_msg.accel.angular.x = 31.0; - init_msg.accel.angular.y = 37.0; - init_msg.accel.angular.z = 41.0; - - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - - EXPECT_ACTION_STATUS_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_trafficSimulatorMsgsEntityStatus) -{ - traffic_simulator_msgs::msg::EntityStatus src_msg; - traffic_simulator_msgs::EntityStatus dst_proto; - - src_msg.name = "test"; - src_msg.time = 3.0; - - traffic_simulator_msgs::msg::BoundingBox box_msg; - box_msg.center.x = 5.0; - box_msg.center.y = 7.0; - box_msg.center.z = 11.0; - box_msg.dimensions.x = 13.0; - box_msg.dimensions.y = 17.0; - box_msg.dimensions.z = 19.0; - src_msg.bounding_box = box_msg; - - traffic_simulator_msgs::msg::ActionStatus action_msg; - action_msg.current_action = "test"; - action_msg.twist.linear.x = 23.0; - action_msg.twist.linear.y = 29.0; - action_msg.twist.linear.z = 31.0; - action_msg.twist.angular.x = 37.0; - action_msg.twist.angular.y = 41.0; - action_msg.twist.angular.z = 43.0; - action_msg.accel.linear.x = 47.0; - action_msg.accel.linear.y = 53.0; - action_msg.accel.linear.z = 59.0; - action_msg.accel.angular.x = 61.0; - action_msg.accel.angular.y = 67.0; - action_msg.accel.angular.z = 71.0; - src_msg.action_status = action_msg; - - geometry_msgs::msg::Pose pose_msg; - pose_msg.position.x = 73.0; - pose_msg.position.y = 79.0; - pose_msg.position.z = 83.0; - pose_msg.orientation.x = 89.0; - pose_msg.orientation.y = 97.0; - pose_msg.orientation.z = 101.0; - pose_msg.orientation.w = 103.0; - src_msg.pose = pose_msg; - - traffic_simulator_msgs::msg::LaneletPose lanelet_pose_msg; - lanelet_pose_msg.lanelet_id = 107; - lanelet_pose_msg.s = 109.0; - lanelet_pose_msg.offset = 113.0; - lanelet_pose_msg.rpy.x = 127.0; - lanelet_pose_msg.rpy.y = 131.0; - lanelet_pose_msg.rpy.z = 137.0; - src_msg.lanelet_pose = lanelet_pose_msg; - src_msg.lanelet_pose_valid = true; - - simulation_interface::toProto(src_msg, dst_proto); - - EXPECT_ENTITY_STATUS_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_trafficSimulatorMsgsEntityStatus) -{ - traffic_simulator_msgs::msg::EntityStatus init_msg; - traffic_simulator_msgs::EntityStatus src_proto; - traffic_simulator_msgs::msg::EntityStatus dst_msg; - - init_msg.name = "test"; - init_msg.time = 3.0; - - traffic_simulator_msgs::msg::BoundingBox box_msg; - box_msg.center.x = 5.0; - box_msg.center.y = 7.0; - box_msg.center.z = 11.0; - box_msg.dimensions.x = 13.0; - box_msg.dimensions.y = 17.0; - box_msg.dimensions.z = 19.0; - init_msg.bounding_box = box_msg; - - traffic_simulator_msgs::msg::ActionStatus action_msg; - action_msg.current_action = "test"; - action_msg.twist.linear.x = 23.0; - action_msg.twist.linear.y = 29.0; - action_msg.twist.linear.z = 31.0; - action_msg.twist.angular.x = 37.0; - action_msg.twist.angular.y = 41.0; - action_msg.twist.angular.z = 43.0; - action_msg.accel.linear.x = 47.0; - action_msg.accel.linear.y = 53.0; - action_msg.accel.linear.z = 59.0; - action_msg.accel.angular.x = 61.0; - action_msg.accel.angular.y = 67.0; - action_msg.accel.angular.z = 71.0; - init_msg.action_status = action_msg; - - geometry_msgs::msg::Pose pose_msg; - pose_msg.position.x = 73.0; - pose_msg.position.y = 79.0; - pose_msg.position.z = 83.0; - pose_msg.orientation.x = 89.0; - pose_msg.orientation.y = 97.0; - pose_msg.orientation.z = 101.0; - pose_msg.orientation.w = 103.0; - init_msg.pose = pose_msg; - - traffic_simulator_msgs::msg::LaneletPose lanelet_pose_msg; - lanelet_pose_msg.lanelet_id = 107; - lanelet_pose_msg.s = 109.0; - lanelet_pose_msg.offset = 113.0; - lanelet_pose_msg.rpy.x = 127.0; - lanelet_pose_msg.rpy.y = 131.0; - lanelet_pose_msg.rpy.z = 137.0; - init_msg.lanelet_pose = lanelet_pose_msg; - init_msg.lanelet_pose_valid = true; - - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - - EXPECT_ENTITY_STATUS_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_simulationApiSchemaEntityStatus) -{ - traffic_simulator_msgs::msg::EntityStatus src_msg; - simulation_api_schema::EntityStatus dst_proto; - - src_msg.name = "test"; - src_msg.time = 3.0; - - traffic_simulator_msgs::msg::ActionStatus action_msg; - action_msg.current_action = "test"; - action_msg.twist.linear.x = 5.0; - action_msg.twist.linear.y = 7.0; - action_msg.twist.linear.z = 11.0; - action_msg.twist.angular.x = 13.0; - action_msg.twist.angular.y = 17.0; - action_msg.twist.angular.z = 19.0; - action_msg.accel.linear.x = 23.0; - action_msg.accel.linear.y = 29.0; - action_msg.accel.linear.z = 31.0; - action_msg.accel.angular.x = 37.0; - action_msg.accel.angular.y = 41.0; - action_msg.accel.angular.z = 43.0; - src_msg.action_status = action_msg; - - geometry_msgs::msg::Pose pose_msg; - pose_msg.position.x = 47.0; - pose_msg.position.y = 53.0; - pose_msg.position.z = 59.0; - pose_msg.orientation.x = 61.0; - pose_msg.orientation.y = 67.0; - pose_msg.orientation.z = 71.0; - pose_msg.orientation.w = 73.0; - src_msg.pose = pose_msg; - - simulation_interface::toProto(src_msg, dst_proto); - - EXPECT_SENT_ENTITY_STATUS_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_simulationApiSchemaEntityStatus) -{ - traffic_simulator_msgs::msg::EntityStatus init_msg; - simulation_api_schema::EntityStatus src_proto; - traffic_simulator_msgs::msg::EntityStatus dst_msg; - - init_msg.name = "test"; - init_msg.time = 3.0; - - traffic_simulator_msgs::msg::ActionStatus action_msg; - action_msg.current_action = "test"; - action_msg.twist.linear.x = 5.0; - action_msg.twist.linear.y = 7.0; - action_msg.twist.linear.z = 11.0; - action_msg.twist.angular.x = 13.0; - action_msg.twist.angular.y = 17.0; - action_msg.twist.angular.z = 19.0; - action_msg.accel.linear.x = 23.0; - action_msg.accel.linear.y = 29.0; - action_msg.accel.linear.z = 31.0; - action_msg.accel.angular.x = 37.0; - action_msg.accel.angular.y = 41.0; - action_msg.accel.angular.z = 43.0; - init_msg.action_status = action_msg; - - geometry_msgs::msg::Pose pose_msg; - pose_msg.position.x = 47.0; - pose_msg.position.y = 53.0; - pose_msg.position.z = 59.0; - pose_msg.orientation.x = 61.0; - pose_msg.orientation.y = 67.0; - pose_msg.orientation.z = 71.0; - pose_msg.orientation.w = 73.0; - init_msg.pose = pose_msg; - - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - - EXPECT_SENT_ENTITY_STATUS_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_Time) -{ - builtin_interfaces::msg::Time src_msg; - builtin_interfaces::Time dst_proto; - - src_msg.nanosec = 3; - src_msg.sec = 5; - - simulation_interface::toProto(src_msg, dst_proto); - - EXPECT_TIME_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_Time) -{ - builtin_interfaces::msg::Time init_msg; - builtin_interfaces::Time src_proto; - builtin_interfaces::msg::Time dst_msg; - - init_msg.nanosec = 3; - init_msg.sec = 5; - - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - - EXPECT_TIME_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_Duration) -{ - builtin_interfaces::msg::Duration src_msg; - builtin_interfaces::Duration dst_proto; - - src_msg.nanosec = 3; - src_msg.sec = 5; - - simulation_interface::toProto(src_msg, dst_proto); - - EXPECT_DURATION_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_Duration) -{ - builtin_interfaces::msg::Duration init_msg; - builtin_interfaces::Duration src_proto; - builtin_interfaces::msg::Duration dst_msg; - - init_msg.nanosec = 3; - init_msg.sec = 5; - - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - - EXPECT_DURATION_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_Header) -{ - std_msgs::msg::Header src_msg; - std_msgs::Header dst_proto; - - src_msg.frame_id = "test"; - src_msg.stamp.nanosec = 3; - src_msg.stamp.sec = 5; - - simulation_interface::toProto(src_msg, dst_proto); - - EXPECT_HEADER_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_Header) -{ - std_msgs::msg::Header init_msg; - std_msgs::Header src_proto; - std_msgs::msg::Header dst_msg; - - init_msg.frame_id = "test"; - init_msg.stamp.nanosec = 3; - init_msg.stamp.sec = 5; - - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - - EXPECT_HEADER_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_Clock) -{ - rosgraph_msgs::msg::Clock src_msg; - rosgraph_msgs::Clock dst_proto; - - src_msg.clock.nanosec = 3; - src_msg.clock.sec = 5; - - simulation_interface::toProto(src_msg, dst_proto); - - EXPECT_CLOCK_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_Clock) -{ - rosgraph_msgs::msg::Clock init_msg; - rosgraph_msgs::Clock src_proto; - rosgraph_msgs::msg::Clock dst_msg; - - init_msg.clock.nanosec = 3; - init_msg.clock.sec = 5; - - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - - EXPECT_CLOCK_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_AckermannControlCommand) -{ - autoware_auto_control_msgs::msg::AckermannControlCommand src_msg; - autoware_auto_control_msgs::AckermannControlCommand dst_proto; - - src_msg.longitudinal.acceleration = 3.0; - src_msg.lateral.steering_tire_angle = 5.0; - src_msg.lateral.steering_tire_rotation_rate = 7.0; - src_msg.longitudinal.speed = 11.0; - - simulation_interface::toProto(src_msg, dst_proto); - - EXPECT_CONTROL_COMMAND_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_AckermannControlCommand) -{ - autoware_auto_control_msgs::msg::AckermannControlCommand init_msg; - autoware_auto_control_msgs::AckermannControlCommand src_proto; - autoware_auto_control_msgs::msg::AckermannControlCommand dst_msg; - - init_msg.longitudinal.acceleration = 3.0; - init_msg.lateral.steering_tire_angle = 5.0; - init_msg.lateral.steering_tire_rotation_rate = 7.0; - init_msg.longitudinal.speed = 11.0; - - simulation_interface::toProto(init_msg, src_proto); - simulation_interface::toMsg(src_proto, dst_msg); - - EXPECT_CONTROL_COMMAND_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_EntityTypeEgo) -{ - traffic_simulator_msgs::msg::EntityType src_msg; - traffic_simulator_msgs::EntityType dst_proto; - src_msg.type = traffic_simulator_msgs::msg::EntityType::EGO; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.type(), traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_EGO); -} - -TEST(Conversion, toMsg_EntityTypeEgo) -{ - traffic_simulator_msgs::msg::EntityType init_msg; - traffic_simulator_msgs::EntityType src_proto; - traffic_simulator_msgs::msg::EntityType dst_msg; - init_msg.type = traffic_simulator_msgs::msg::EntityType::EGO; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.type, traffic_simulator_msgs::msg::EntityType::EGO); -} - -TEST(Conversion, toProto_EntityTypeVehicle) -{ - traffic_simulator_msgs::msg::EntityType src_msg; - traffic_simulator_msgs::EntityType dst_proto; - src_msg.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.type(), traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_VEHICLE); -} - -TEST(Conversion, toMsg_EntityTypeVehicle) -{ - traffic_simulator_msgs::msg::EntityType init_msg; - traffic_simulator_msgs::EntityType src_proto; - traffic_simulator_msgs::msg::EntityType dst_msg; - init_msg.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.type, traffic_simulator_msgs::msg::EntityType::VEHICLE); -} - -TEST(Conversion, toProto_EntityTypePedestrian) -{ - traffic_simulator_msgs::msg::EntityType src_msg; - traffic_simulator_msgs::EntityType dst_proto; - src_msg.type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.type(), traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_PEDESTRIAN); -} - -TEST(Conversion, toMsg_EntityTypePedestrian) -{ - traffic_simulator_msgs::msg::EntityType init_msg; - traffic_simulator_msgs::EntityType src_proto; - traffic_simulator_msgs::msg::EntityType dst_msg; - init_msg.type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.type, traffic_simulator_msgs::msg::EntityType::PEDESTRIAN); -} - -TEST(Conversion, toProto_EntityTypeMiscObject) -{ - traffic_simulator_msgs::msg::EntityType src_msg; - traffic_simulator_msgs::EntityType dst_proto; - src_msg.type = traffic_simulator_msgs::msg::EntityType::MISC_OBJECT; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.type(), traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_MISC_OBJECT); -} - -TEST(Conversion, toMsg_EntityTypeMiscObject) -{ - traffic_simulator_msgs::msg::EntityType init_msg; - traffic_simulator_msgs::EntityType src_proto; - traffic_simulator_msgs::msg::EntityType dst_msg; - init_msg.type = traffic_simulator_msgs::msg::EntityType::MISC_OBJECT; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.type, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT); -} - -TEST(Conversion, toProto_EntitySubtypeUnknown) -{ - traffic_simulator_msgs::msg::EntitySubtype src_msg; - traffic_simulator_msgs::EntitySubtype dst_proto; - src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ( - dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_UNKNOWN); -} - -TEST(Conversion, toMsg_EntitySubtypeUnknown) -{ - traffic_simulator_msgs::msg::EntitySubtype init_msg; - traffic_simulator_msgs::EntitySubtype src_proto; - traffic_simulator_msgs::msg::EntitySubtype dst_msg; - init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN); -} - -TEST(Conversion, toProto_EntitySubtypeCar) -{ - traffic_simulator_msgs::msg::EntitySubtype src_msg; - traffic_simulator_msgs::EntitySubtype dst_proto; - src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR); -} - -TEST(Conversion, toMsg_EntitySubtypeCar) -{ - traffic_simulator_msgs::msg::EntitySubtype init_msg; - traffic_simulator_msgs::EntitySubtype src_proto; - traffic_simulator_msgs::msg::EntitySubtype dst_msg; - init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::CAR); -} - -TEST(Conversion, toProto_EntitySubtypeTruck) -{ - traffic_simulator_msgs::msg::EntitySubtype src_msg; - traffic_simulator_msgs::EntitySubtype dst_proto; - src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::TRUCK; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ( - dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRUCK); -} - -TEST(Conversion, toMsg_EntitySubtypeTruck) -{ - traffic_simulator_msgs::msg::EntitySubtype init_msg; - traffic_simulator_msgs::EntitySubtype src_proto; - traffic_simulator_msgs::msg::EntitySubtype dst_msg; - init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::TRUCK; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::TRUCK); -} - -TEST(Conversion, toProto_EntitySubtypeBus) -{ - traffic_simulator_msgs::msg::EntitySubtype src_msg; - traffic_simulator_msgs::EntitySubtype dst_proto; - src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::BUS; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BUS); -} - -TEST(Conversion, toMsg_EntitySubtypeBus) -{ - traffic_simulator_msgs::msg::EntitySubtype init_msg; - traffic_simulator_msgs::EntitySubtype src_proto; - traffic_simulator_msgs::msg::EntitySubtype dst_msg; - init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::BUS; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::BUS); -} - -TEST(Conversion, toProto_EntitySubtypeTrailer) -{ - traffic_simulator_msgs::msg::EntitySubtype src_msg; - traffic_simulator_msgs::EntitySubtype dst_proto; - src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::TRAILER; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ( - dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRAILER); -} - -TEST(Conversion, toMsg_EntitySubtypeTrailer) -{ - traffic_simulator_msgs::msg::EntitySubtype init_msg; - traffic_simulator_msgs::EntitySubtype src_proto; - traffic_simulator_msgs::msg::EntitySubtype dst_msg; - init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::TRAILER; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::TRAILER); -} - -TEST(Conversion, toProto_EntitySubtypeMotorcycle) -{ - traffic_simulator_msgs::msg::EntitySubtype src_msg; - traffic_simulator_msgs::EntitySubtype dst_proto; - src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::MOTORCYCLE; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ( - dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_MOTORCYCLE); -} - -TEST(Conversion, toMsg_EntitySubtypeMotorcycle) -{ - traffic_simulator_msgs::msg::EntitySubtype init_msg; - traffic_simulator_msgs::EntitySubtype src_proto; - traffic_simulator_msgs::msg::EntitySubtype dst_msg; - init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::MOTORCYCLE; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::MOTORCYCLE); -} - -TEST(Conversion, toProto_EntitySubtypeBicycle) -{ - traffic_simulator_msgs::msg::EntitySubtype src_msg; - traffic_simulator_msgs::EntitySubtype dst_proto; - src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::BICYCLE; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ( - dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BICYCLE); -} - -TEST(Conversion, toMsg_EntitySubtypeBicycle) -{ - traffic_simulator_msgs::msg::EntitySubtype init_msg; - traffic_simulator_msgs::EntitySubtype src_proto; - traffic_simulator_msgs::msg::EntitySubtype dst_msg; - init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::BICYCLE; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::BICYCLE); -} - -TEST(Conversion, toProto_EntitySubtypePedestrian) -{ - traffic_simulator_msgs::msg::EntitySubtype src_msg; - traffic_simulator_msgs::EntitySubtype dst_proto; - src_msg.value = traffic_simulator_msgs::msg::EntitySubtype::PEDESTRIAN; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ( - dst_proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_PEDESTRIAN); -} - -TEST(Conversion, toMsg_EntitySubtypePedestrian) -{ - traffic_simulator_msgs::msg::EntitySubtype init_msg; - traffic_simulator_msgs::EntitySubtype src_proto; - traffic_simulator_msgs::msg::EntitySubtype dst_msg; - init_msg.value = traffic_simulator_msgs::msg::EntitySubtype::PEDESTRIAN; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.value, traffic_simulator_msgs::msg::EntitySubtype::PEDESTRIAN); -} - -TEST(Conversion, toProto_LaneletPose) -{ - traffic_simulator_msgs::msg::LaneletPose src_msg; - traffic_simulator_msgs::LaneletPose dst_proto; - src_msg.lanelet_id = 3; - src_msg.s = 5.0; - src_msg.offset = 7.0; - src_msg.rpy.x = 11.0; - src_msg.rpy.y = 13.0; - src_msg.rpy.z = 17.0; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_LANELET_POSE_EQ(src_msg, dst_proto); -} - -TEST(Conversion, toMsg_LaneletPose) -{ - traffic_simulator_msgs::msg::LaneletPose init_msg; - traffic_simulator_msgs::LaneletPose src_proto; - traffic_simulator_msgs::msg::LaneletPose dst_msg; - init_msg.lanelet_id = 3; - init_msg.s = 5.0; - init_msg.offset = 7.0; - init_msg.rpy.x = 11.0; - init_msg.rpy.y = 13.0; - init_msg.rpy.z = 17.0; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_LANELET_POSE_EQ(dst_msg, src_proto); -} - -TEST(Conversion, toProto_GearCommandNone) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::NONE; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::NONE); -} - -TEST(Conversion, toMsg_GearCommandNone) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::NONE; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::NONE); -} - -TEST(Conversion, toProto_GearCommandNeutral) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::NEUTRAL; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::NEUTRAL); -} - -TEST(Conversion, toMsg_GearCommandNeutral) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::NEUTRAL; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::NEUTRAL); -} - -TEST(Conversion, toProto_GearCommandDrive) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE); -} - -TEST(Conversion, toMsg_GearCommandDrive) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE); -} - -TEST(Conversion, toProto_GearCommandDrive2) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_2; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_2); -} - -TEST(Conversion, toMsg_GearCommandDrive2) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_2; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_2); -} - -TEST(Conversion, toProto_GearCommandDrive3) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_3; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_3); -} - -TEST(Conversion, toMsg_GearCommandDrive3) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_3; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_3); -} - -TEST(Conversion, toProto_GearCommandDrive4) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_4; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_4); -} - -TEST(Conversion, toMsg_GearCommandDrive4) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_4; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_4); -} - -TEST(Conversion, toProto_GearCommandDrive5) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_5; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_5); -} - -TEST(Conversion, toMsg_GearCommandDrive5) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_5; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_5); -} - -TEST(Conversion, toProto_GearCommandDrive6) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_6; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_6); -} - -TEST(Conversion, toMsg_GearCommandDrive6) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_6; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_6); -} - -TEST(Conversion, toProto_GearCommandDrive7) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_7; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_7); -} - -TEST(Conversion, toMsg_GearCommandDrive7) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_7; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_7); -} - -TEST(Conversion, toProto_GearCommandDrive8) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_8; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_8); -} - -TEST(Conversion, toMsg_GearCommandDrive8) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_8; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_8); -} - -TEST(Conversion, toProto_GearCommandDrive9) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_9; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_9); -} - -TEST(Conversion, toMsg_GearCommandDrive9) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_9; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_9); -} - -TEST(Conversion, toProto_GearCommandDrive10) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_10; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_10); -} - -TEST(Conversion, toMsg_GearCommandDrive10) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_10; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_10); -} - -TEST(Conversion, toProto_GearCommandDrive11) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_11; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_11); -} - -TEST(Conversion, toMsg_GearCommandDrive11) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_11; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_11); -} - -TEST(Conversion, toProto_GearCommandDrive12) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_12; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_12); -} - -TEST(Conversion, toMsg_GearCommandDrive12) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_12; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_12); -} - -TEST(Conversion, toProto_GearCommandDrive13) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_13; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_13); -} - -TEST(Conversion, toMsg_GearCommandDrive13) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_13; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_13); -} - -TEST(Conversion, toProto_GearCommandDrive14) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_14; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_14); -} - -TEST(Conversion, toMsg_GearCommandDrive14) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_14; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_14); -} - -TEST(Conversion, toProto_GearCommandDrive15) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_15; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_15); -} - -TEST(Conversion, toMsg_GearCommandDrive15) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_15; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_15); -} - -TEST(Conversion, toProto_GearCommandDrive16) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_16; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_16); -} - -TEST(Conversion, toMsg_GearCommandDrive16) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_16; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_16); -} - -TEST(Conversion, toProto_GearCommandDrive17) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_17; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_17); -} - -TEST(Conversion, toMsg_GearCommandDrive17) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_17; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_17); -} - -TEST(Conversion, toProto_GearCommandDrive18) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_18; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::DRIVE_18); -} - -TEST(Conversion, toMsg_GearCommandDrive18) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_18; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_18); -} - -TEST(Conversion, toProto_GearCommandReverse) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::REVERSE); -} - -TEST(Conversion, toMsg_GearCommandReverse) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE); -} - -TEST(Conversion, toProto_GearCommandReverse2) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE_2; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::REVERSE_2); -} - -TEST(Conversion, toMsg_GearCommandReverse2) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE_2; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE_2); -} - -TEST(Conversion, toProto_GearCommandPark) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::PARK; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::PARK); -} - -TEST(Conversion, toMsg_GearCommandPark) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::PARK; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::PARK); -} - -TEST(Conversion, toProto_GearCommandLow) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::LOW; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::LOW); -} - -TEST(Conversion, toMsg_GearCommandLow) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::LOW; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::LOW); -} - -TEST(Conversion, toProto_GearCommandLow2) -{ - autoware_auto_vehicle_msgs::msg::GearCommand src_msg; - autoware_auto_vehicle_msgs::GearCommand dst_proto; - src_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::LOW_2; - EXPECT_NO_THROW(simulation_interface::toProto(src_msg, dst_proto)); - EXPECT_EQ(dst_proto.command(), autoware_auto_vehicle_msgs::GearCommand_Constants::LOW_2); -} - -TEST(Conversion, toMsg_GearCommandLow2) -{ - autoware_auto_vehicle_msgs::msg::GearCommand init_msg; - autoware_auto_vehicle_msgs::GearCommand src_proto; - autoware_auto_vehicle_msgs::msg::GearCommand dst_msg; - init_msg.command = autoware_auto_vehicle_msgs::msg::GearCommand::LOW_2; - EXPECT_NO_THROW(simulation_interface::toProto(init_msg, src_proto)); - EXPECT_NO_THROW(simulation_interface::toMsg(src_proto, dst_msg)); - EXPECT_EQ(dst_msg.command, autoware_auto_vehicle_msgs::msg::GearCommand::LOW_2); -} \ No newline at end of file diff --git a/simulation/simulation_interface/test/test_conversions.cpp b/simulation/simulation_interface/test/test_conversions.cpp new file mode 100644 index 00000000000..0a7bf9bccb7 --- /dev/null +++ b/simulation/simulation_interface/test/test_conversions.cpp @@ -0,0 +1,534 @@ +// 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 +#include + +#include "expect_equal_macros.hpp" +/** + * @brief Test cases + */ + +TEST(Conversion, Point) +{ + geometry_msgs::Point proto; + geometry_msgs::msg::Point p; + p.x = 1.0; + p.y = 2; + p.z = 3.1; + simulation_interface::toProto(p, proto); + EXPECT_POINT_EQ(p, proto); + p = geometry_msgs::msg::Point(); + EXPECT_DOUBLE_EQ(p.x, 0); + simulation_interface::toMsg(proto, p); + EXPECT_POINT_EQ(p, proto); +} + +TEST(Conversion, Quaternion) +{ + geometry_msgs::Quaternion proto; + geometry_msgs::msg::Quaternion q; + q.x = 1.0; + q.y = 2; + q.z = 3.1; + q.w = -10; + simulation_interface::toProto(q, proto); + EXPECT_QUATERNION_EQ(q, proto); + q = geometry_msgs::msg::Quaternion(); + EXPECT_DOUBLE_EQ(q.x, 0); + simulation_interface::toMsg(proto, q); + EXPECT_QUATERNION_EQ(q, proto); +} + +TEST(Conversion, Pose) +{ + geometry_msgs::Pose proto; + geometry_msgs::msg::Pose p; + p.position.x = 1.0; + p.position.y = 2; + p.position.z = 3.1; + p.orientation.x = 0; + p.orientation.y = 0; + p.orientation.z = 0; + p.orientation.w = 1; + simulation_interface::toProto(p, proto); + EXPECT_POSE_EQ(p, proto); + p = geometry_msgs::msg::Pose(); + EXPECT_DOUBLE_EQ(p.position.x, 0); + simulation_interface::toMsg(proto, p); + EXPECT_POSE_EQ(p, proto); +} + +TEST(Conversion, Vector) +{ + geometry_msgs::Vector3 proto; + geometry_msgs::msg::Vector3 vec; + vec.x = 1; + vec.y = 2; + vec.z = 3.0; + simulation_interface::toProto(vec, proto); + EXPECT_VECTOR3_EQ(vec, proto); + vec = geometry_msgs::msg::Vector3(); + EXPECT_DOUBLE_EQ(vec.x, 0); + simulation_interface::toMsg(proto, vec); + EXPECT_VECTOR3_EQ(vec, proto); +} + +TEST(Conversion, Twist) +{ + geometry_msgs::Twist proto; + geometry_msgs::msg::Twist twist; + twist.linear.x = 1; + twist.linear.y = 2; + twist.linear.z = 3.0; + simulation_interface::toProto(twist, proto); + EXPECT_TWIST_EQ(twist, proto); + twist = geometry_msgs::msg::Twist(); + EXPECT_DOUBLE_EQ(twist.linear.x, 0); + simulation_interface::toMsg(proto, twist); + EXPECT_TWIST_EQ(twist, proto); +} + +TEST(Conversion, Accel) +{ + geometry_msgs::Accel proto; + geometry_msgs::msg::Accel accel; + accel.linear.x = 1; + accel.linear.y = 2; + accel.linear.z = 3.0; + simulation_interface::toProto(accel, proto); + EXPECT_ACCEL_EQ(accel, proto); + accel = geometry_msgs::msg::Accel(); + EXPECT_DOUBLE_EQ(accel.linear.x, 0); + simulation_interface::toMsg(proto, accel); + EXPECT_ACCEL_EQ(accel, proto); +} + +TEST(Conversion, Performance) +{ + traffic_simulator_msgs::Performance proto; + traffic_simulator_msgs::msg::Performance performance; + performance.max_speed = 10; + performance.max_deceleration = 3; + simulation_interface::toProto(performance, proto); + EXPECT_PERFORMANCE_EQ(performance, proto); + EXPECT_DOUBLE_EQ(performance.max_acceleration, proto.max_acceleration()); + EXPECT_DOUBLE_EQ(performance.max_deceleration, proto.max_deceleration()); + EXPECT_DOUBLE_EQ(performance.max_speed, proto.max_speed()); + performance = traffic_simulator_msgs::msg::Performance(); + EXPECT_DOUBLE_EQ(performance.max_speed, 30.0); + simulation_interface::toMsg(proto, performance); + EXPECT_PERFORMANCE_EQ(performance, proto); +} + +TEST(Conversion, Axle) +{ + traffic_simulator_msgs::Axle proto; + traffic_simulator_msgs::msg::Axle axle; + axle.max_steering = 30; + axle.position_x = 3; + axle.position_z = 14; + axle.track_width = -10; + axle.wheel_diameter = 53; + simulation_interface::toProto(axle, proto); + EXPECT_AXLE_EQ(axle, proto); + axle = traffic_simulator_msgs::msg::Axle(); + EXPECT_DOUBLE_EQ(axle.max_steering, 0); + simulation_interface::toMsg(proto, axle); + EXPECT_AXLE_EQ(axle, proto); +} + +TEST(Conversion, Axles) +{ + traffic_simulator_msgs::Axles proto; + traffic_simulator_msgs::msg::Axles axles; + axles.front_axle.max_steering = 3; + axles.front_axle.position_x = 35; + axles.front_axle.position_z = 234; + axles.front_axle.track_width = 1; + axles.front_axle.wheel_diameter = 123; + axles.rear_axle.max_steering = 13; + axles.rear_axle.position_x = 3; + axles.rear_axle.position_z = 23; + axles.rear_axle.track_width = 14; + axles.rear_axle.wheel_diameter = 122; + simulation_interface::toProto(axles, proto); + EXPECT_AXLES_EQ(axles, proto); + axles = traffic_simulator_msgs::msg::Axles(); + EXPECT_DOUBLE_EQ(axles.front_axle.max_steering, 0); + simulation_interface::toMsg(proto, axles); + EXPECT_AXLES_EQ(axles, proto); +} + +TEST(Conversion, BoundingBox) +{ + traffic_simulator_msgs::BoundingBox proto; + traffic_simulator_msgs::msg::BoundingBox box; + box.center.x = 1.0; + box.center.y = 1.23; + box.center.z = 43.0; + box.dimensions.x = 12.3; + box.dimensions.y = 3.9; + box.dimensions.z = 4.0; + simulation_interface::toProto(box, proto); + EXPECT_BOUNDING_BOX_EQ(box, proto); + box = traffic_simulator_msgs::msg::BoundingBox(); + EXPECT_DOUBLE_EQ(box.center.x, 0); + simulation_interface::toMsg(proto, box); + EXPECT_BOUNDING_BOX_EQ(box, proto); +} + +TEST(Conversion, VehicleParameters) +{ + traffic_simulator_msgs::VehicleParameters proto; + traffic_simulator_msgs::msg::VehicleParameters p; + p.name = "foo"; + traffic_simulator_msgs::msg::BoundingBox box; + box.center.x = 1.0; + box.center.y = 1.23; + box.center.z = 43.0; + box.dimensions.x = 12.3; + box.dimensions.y = 3.9; + box.dimensions.z = 4.0; + p.bounding_box = box; + traffic_simulator_msgs::msg::Performance performance; + performance.max_speed = 10; + performance.max_deceleration = 3; + p.performance = performance; + traffic_simulator_msgs::msg::Axle axle; + axle.max_steering = 30; + axle.position_x = 3; + axle.position_z = 14; + axle.track_width = -10; + axle.wheel_diameter = 53; + p.axles.front_axle = axle; + p.axles.rear_axle = axle; + EXPECT_NO_THROW(simulation_interface::toProto(p, proto)); + EXPECT_VEHICLE_PARAMETERS_EQ(p, proto); + p = traffic_simulator_msgs::msg::VehicleParameters(); + EXPECT_DOUBLE_EQ(p.bounding_box.dimensions.x, 0); + EXPECT_NO_THROW(simulation_interface::toMsg(proto, p)); + EXPECT_VEHICLE_PARAMETERS_EQ(p, proto); +} + +TEST(Conversion, PedestrianParameters) +{ + traffic_simulator_msgs::PedestrianParameters proto; + traffic_simulator_msgs::msg::PedestrianParameters p; + p.name = "foo"; + traffic_simulator_msgs::msg::BoundingBox box; + box.center.x = 1.0; + box.center.y = 1.23; + box.center.z = 43.0; + box.dimensions.x = 12.3; + box.dimensions.y = 3.9; + box.dimensions.z = 4.0; + p.bounding_box = box; + EXPECT_NO_THROW(simulation_interface::toProto(p, proto)); + EXPECT_PEDESTRIAN_PARAMETERS_EQ(p, proto); + p = traffic_simulator_msgs::msg::PedestrianParameters(); + EXPECT_DOUBLE_EQ(p.bounding_box.dimensions.x, 0); + EXPECT_NO_THROW(simulation_interface::toMsg(proto, p)); + EXPECT_PEDESTRIAN_PARAMETERS_EQ(p, proto); +} + +TEST(Conversion, MiscObjectParameters) +{ + traffic_simulator_msgs::MiscObjectParameters proto; + traffic_simulator_msgs::msg::MiscObjectParameters p; + traffic_simulator_msgs::msg::BoundingBox box; + box.center.x = 1.0; + box.center.y = 1.23; + box.center.z = 43.0; + box.dimensions.x = 12.3; + box.dimensions.y = 3.9; + box.dimensions.z = 4.0; + p.bounding_box = box; + EXPECT_NO_THROW(simulation_interface::toProto(p, proto)); + EXPECT_MISC_OBJECT_PARAMETERS_EQ(p, proto); + EXPECT_NO_THROW(simulation_interface::toMsg(proto, p)); + EXPECT_MISC_OBJECT_PARAMETERS_EQ(p, proto); +} + +TEST(Conversion, ActionStatus) +{ + traffic_simulator_msgs::ActionStatus proto; + traffic_simulator_msgs::msg::ActionStatus action; + action.current_action = "test"; + action.twist.linear.x = 1.0; + action.twist.linear.y = 2.0; + action.twist.linear.z = 3.0; + action.twist.angular.x = -20; + action.twist.angular.y = -4.2; + action.twist.angular.z = 9; + action.accel.linear.x = 3.0; + action.accel.linear.y = 908; + action.accel.linear.z = 987.0; + action.accel.angular.x = 0.3; + action.accel.angular.y = 0.5; + action.accel.angular.z = 98; + simulation_interface::toProto(action, proto); + EXPECT_ACTION_STATUS_EQ(action, proto); + action = traffic_simulator_msgs::msg::ActionStatus(); + EXPECT_DOUBLE_EQ(action.twist.linear.x, 0); + simulation_interface::toMsg(proto, action); + EXPECT_ACTION_STATUS_EQ(action, proto); +} + +TEST(Conversion, EntityStatus) +{ + traffic_simulator_msgs::EntityStatus proto; + traffic_simulator_msgs::msg::EntityStatus status; + status.name = "test"; + status.time = 3.0; + traffic_simulator_msgs::msg::BoundingBox box; + box.center.x = 1.0; + box.center.y = 1.23; + box.center.z = 43.0; + box.dimensions.x = 12.3; + box.dimensions.y = 3.9; + box.dimensions.z = 4.0; + status.bounding_box = box; + traffic_simulator_msgs::msg::ActionStatus action; + action.current_action = "test"; + action.twist.linear.x = 1.0; + action.twist.linear.y = 2.0; + action.twist.linear.z = 3.0; + action.twist.angular.x = -20; + action.twist.angular.y = -4.2; + action.twist.angular.z = 9; + action.accel.linear.x = 3.0; + action.accel.linear.y = 908; + action.accel.linear.z = 987.0; + action.accel.angular.x = 0.3; + action.accel.angular.y = 0.5; + action.accel.angular.z = 98; + status.action_status = action; + geometry_msgs::msg::Pose pose; + pose.position.x = 4.0; + pose.position.y = 1.2; + pose.position.z = 5.1; + pose.orientation.x = 0.3; + pose.orientation.y = 8.3; + pose.orientation.z = 9.3; + pose.orientation.w = 10.2; + status.pose = pose; + traffic_simulator_msgs::msg::LaneletPose lanelet_pose; + lanelet_pose.lanelet_id = 23; + lanelet_pose.s = 1.0; + lanelet_pose.offset = 3.5; + lanelet_pose.rpy.x = 3.4; + lanelet_pose.rpy.y = 5.1; + lanelet_pose.rpy.z = 1.3; + status.lanelet_pose = lanelet_pose; + status.lanelet_pose_valid = false; + simulation_interface::toProto(status, proto); + EXPECT_ENTITY_STATUS_EQ(status, proto); + status = traffic_simulator_msgs::msg::EntityStatus(); + EXPECT_TRUE(status.lanelet_pose_valid); + EXPECT_FALSE(proto.lanelet_pose_valid()); + simulation_interface::toMsg(proto, status); + EXPECT_ENTITY_STATUS_EQ(status, proto); + EXPECT_FALSE(status.lanelet_pose_valid); +} + +TEST(Conversion, SentEntityStatus) +{ + simulation_api_schema::EntityStatus proto; + traffic_simulator_msgs::msg::EntityStatus status; + status.name = "test"; + status.time = 3.0; + traffic_simulator_msgs::msg::ActionStatus action; + action.current_action = "test"; + action.twist.linear.x = 1.0; + action.twist.linear.y = 2.0; + action.twist.linear.z = 3.0; + action.twist.angular.x = -20; + action.twist.angular.y = -4.2; + action.twist.angular.z = 9; + action.accel.linear.x = 3.0; + action.accel.linear.y = 908; + action.accel.linear.z = 987.0; + action.accel.angular.x = 0.3; + action.accel.angular.y = 0.5; + action.accel.angular.z = 98; + status.action_status = action; + geometry_msgs::msg::Pose pose; + pose.position.x = 4.0; + pose.position.y = 1.2; + pose.position.z = 5.1; + pose.orientation.x = 0.3; + pose.orientation.y = 8.3; + pose.orientation.z = 9.3; + pose.orientation.w = 10.2; + status.pose = pose; + simulation_interface::toProto(status, proto); + EXPECT_SENT_ENTITY_STATUS_EQ(status, proto); + status = traffic_simulator_msgs::msg::EntityStatus(); + simulation_interface::toMsg(proto, status); + EXPECT_SENT_ENTITY_STATUS_EQ(status, proto); +} + +TEST(Conversion, Time) +{ + builtin_interfaces::Time proto; + builtin_interfaces::msg::Time msg; + msg.nanosec = 1; + msg.sec = 2; + simulation_interface::toProto(msg, proto); + EXPECT_TIME_EQ(msg, proto); + msg.nanosec = 0; + msg.sec = 0; + simulation_interface::toMsg(proto, msg); + EXPECT_TIME_EQ(msg, proto); +} + +TEST(Conversion, Duration) +{ + builtin_interfaces::Duration proto; + builtin_interfaces::msg::Duration msg; + msg.nanosec = 1; + msg.sec = 2; + simulation_interface::toProto(msg, proto); + EXPECT_DURATION_EQ(msg, proto); + msg.nanosec = 0; + msg.sec = 0; + simulation_interface::toMsg(proto, msg); + EXPECT_DURATION_EQ(msg, proto); +} + +TEST(Conversion, Header) +{ + std_msgs::Header proto; + std_msgs::msg::Header msg; + msg.frame_id = "base_link"; + msg.stamp.nanosec = 4; + msg.stamp.sec = 1; + simulation_interface::toProto(msg, proto); + EXPECT_HEADER_EQ(msg, proto); + msg.frame_id = ""; + msg.stamp.nanosec = 0; + msg.stamp.sec = 0; + simulation_interface::toMsg(proto, msg); + EXPECT_HEADER_EQ(msg, proto); +} + +TEST(Conversion, Clock) +{ + rosgraph_msgs::msg::Clock msg; + rosgraph_msgs::Clock proto; + msg.clock.nanosec = 12; + msg.clock.sec = 11; + simulation_interface::toProto(msg, proto); + EXPECT_CLOCK_EQ(msg, proto); + msg = rosgraph_msgs::msg::Clock(); + EXPECT_EQ(msg.clock.nanosec, static_cast(0)); + simulation_interface::toMsg(proto, msg); + EXPECT_CLOCK_EQ(msg, proto); +} + +TEST(Conversion, AckermannControlCommand) +{ + autoware_auto_control_msgs::AckermannControlCommand proto; + autoware_auto_control_msgs::msg::AckermannControlCommand msg; + msg.longitudinal.acceleration = 3; + msg.lateral.steering_tire_angle = 1.4; + msg.lateral.steering_tire_rotation_rate = 13.4; + msg.longitudinal.speed = 11.3; + simulation_interface::toProto(msg, proto); + EXPECT_CONTROL_COMMAND_EQ(msg, proto); + msg.longitudinal.acceleration = 0; + msg.lateral.steering_tire_angle = 0; + msg.lateral.steering_tire_rotation_rate = 0; + msg.longitudinal.speed = 0; + simulation_interface::toMsg(proto, msg); + EXPECT_CONTROL_COMMAND_EQ(msg, proto); +} + +TEST(Conversion, EntityType) +{ + traffic_simulator_msgs::EntityType proto; + traffic_simulator_msgs::msg::EntityType msg; + msg.type = msg.VEHICLE; + EXPECT_NO_THROW(simulation_interface::toProto(msg, proto)); + EXPECT_EQ(proto.type(), traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_VEHICLE); + msg.type = msg.MISC_OBJECT; + EXPECT_NO_THROW(simulation_interface::toProto(msg, proto)); + EXPECT_EQ(proto.type(), traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_MISC_OBJECT); + proto.set_type(traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_VEHICLE); + msg.type = msg.EGO; + EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); + EXPECT_EQ(msg.type, traffic_simulator_msgs::msg::EntityType::VEHICLE); + msg.type = msg.VEHICLE; + proto.set_type(traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_EGO); + EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); + EXPECT_EQ(msg.type, traffic_simulator_msgs::msg::EntityType::EGO); + msg.type = msg.VEHICLE; + proto.set_type(traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_PEDESTRIAN); + EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); + EXPECT_EQ(msg.type, traffic_simulator_msgs::msg::EntityType::PEDESTRIAN); + proto.set_type(traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_MISC_OBJECT); + EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); + EXPECT_EQ(msg.type, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT); +} + +TEST(Conversion, EntitySubtype) +{ + traffic_simulator_msgs::EntitySubtype proto; + traffic_simulator_msgs::msg::EntitySubtype msg; + msg.value = msg.CAR; + EXPECT_NO_THROW(simulation_interface::toProto(msg, proto)); + EXPECT_EQ(proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR); + msg.value = msg.UNKNOWN; + EXPECT_NO_THROW(simulation_interface::toProto(msg, proto)); + EXPECT_EQ(proto.value(), traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_UNKNOWN); + proto.set_value(traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR); + msg.value = msg.UNKNOWN; + EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); + EXPECT_EQ(msg.value, traffic_simulator_msgs::msg::EntitySubtype::CAR); + msg.value = msg.CAR; + proto.set_value(traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR); + EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); + EXPECT_EQ(msg.value, traffic_simulator_msgs::msg::EntitySubtype::CAR); + msg.value = msg.CAR; + proto.set_value(traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_PEDESTRIAN); + EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); + EXPECT_EQ(msg.value, traffic_simulator_msgs::msg::EntitySubtype::PEDESTRIAN); + proto.set_value(traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_UNKNOWN); + EXPECT_NO_THROW(simulation_interface::toMsg(proto, msg)); + EXPECT_EQ(msg.value, traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN); +} + +TEST(Conversion, LaneletPose) +{ + traffic_simulator_msgs::msg::LaneletPose pose; + traffic_simulator_msgs::LaneletPose proto; + pose.lanelet_id = 23; + pose.s = 1.0; + pose.offset = 3.5; + pose.rpy.x = 3.4; + pose.rpy.y = 5.1; + pose.rpy.z = 1.3; + EXPECT_NO_THROW(simulation_interface::toProto(pose, proto)); + EXPECT_LANELET_POSE_EQ(pose, proto); + pose = traffic_simulator_msgs::msg::LaneletPose(); + EXPECT_NO_THROW(simulation_interface::toMsg(proto, pose)); + EXPECT_LANELET_POSE_EQ(pose, proto); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/simulation/traffic_simulator/include/traffic_simulator/job/job.hpp b/simulation/traffic_simulator/include/traffic_simulator/job/job.hpp index e92fc9b2c7e..7731d376f91 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/job/job.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/job/job.hpp @@ -23,7 +23,7 @@ namespace traffic_simulator namespace job { enum class Type { - UNKNOWN = 0, + UNKOWN = 0, LINEAR_VELOCITY = 1, LINEAR_ACCELERATION = 2, STAND_STILL_DURATION = 3, From 7951725ad4eced5f9192b573cf18961afb43b20d Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 6 Jun 2024 09:44:41 +0200 Subject: [PATCH 039/118] final cleanup --- simulation/simulation_interface/CMakeLists.txt | 1 - .../test/{include => }/expect_equal_macros.hpp | 0 2 files changed, 1 deletion(-) rename simulation/simulation_interface/test/{include => }/expect_equal_macros.hpp (100%) diff --git a/simulation/simulation_interface/CMakeLists.txt b/simulation/simulation_interface/CMakeLists.txt index ac11baa6019..6e2cc1fb292 100644 --- a/simulation/simulation_interface/CMakeLists.txt +++ b/simulation/simulation_interface/CMakeLists.txt @@ -74,7 +74,6 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_conversion test/test_conversions.cpp) target_link_libraries(test_conversion simulation_interface) - add_subdirectory(test) endif() ament_auto_package() diff --git a/simulation/simulation_interface/test/include/expect_equal_macros.hpp b/simulation/simulation_interface/test/expect_equal_macros.hpp similarity index 100% rename from simulation/simulation_interface/test/include/expect_equal_macros.hpp rename to simulation/simulation_interface/test/expect_equal_macros.hpp From a02b847ac38a6273715823ea899f009741637042 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 6 Jun 2024 11:17:51 +0200 Subject: [PATCH 040/118] prepare notes for descriptions --- .../test/src/behavior/CMakeLists.txt | 3 + .../test/src/behavior/test_behavior.cpp | 17 +- .../test/src/behavior/test_route_planner.cpp | 177 ++++++++++++ .../test/src/entity/CMakeLists.txt | 3 + .../src/entity/test_misc_object_entity.cpp | 271 ++++++++++++++++++ .../test/src/entity_helper_functions.hpp | 133 +++++++++ .../test/src/job/test_job.cpp | 36 ++- .../test/src/job/test_job_list.cpp | 17 +- .../test_simulation_clock.cpp | 14 +- .../src/traffic_lights/test_traffic_light.cpp | 67 +++-- 10 files changed, 707 insertions(+), 31 deletions(-) create mode 100644 simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp create mode 100644 simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp create mode 100644 simulation/traffic_simulator/test/src/entity_helper_functions.hpp diff --git a/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt b/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt index e06238b665b..605307aeedf 100644 --- a/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt @@ -1,2 +1,5 @@ ament_add_gtest(test_behavior test_behavior.cpp) target_link_libraries(test_behavior traffic_simulator) + +ament_add_gtest(test_route_planner test_route_planner.cpp) +target_link_libraries(test_route_planner traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp b/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp index dc05788ad93..3f783954530 100644 --- a/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp +++ b/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp @@ -22,6 +22,9 @@ int main(int argc, char ** argv) return RUN_ALL_TESTS(); } +/** + * @note + */ TEST(Behavior, getRequestString_none) { const auto req = traffic_simulator::behavior::Request::NONE; @@ -30,6 +33,9 @@ TEST(Behavior, getRequestString_none) EXPECT_TRUE("none" == str); } +/** + * @note + */ TEST(Behavior, getRequestString_lane_change) { const auto req = traffic_simulator::behavior::Request::LANE_CHANGE; @@ -38,6 +44,9 @@ TEST(Behavior, getRequestString_lane_change) EXPECT_TRUE("lane_change" == str); } +/** + * @note + */ TEST(Behavior, getRequestString_follow_lane) { const auto req = traffic_simulator::behavior::Request::FOLLOW_LANE; @@ -46,6 +55,9 @@ TEST(Behavior, getRequestString_follow_lane) EXPECT_TRUE("follow_lane" == str); } +/** + * @note + */ TEST(Behavior, getRequestString_follow_polyline_trajectory) { const auto req = traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY; @@ -54,10 +66,13 @@ TEST(Behavior, getRequestString_follow_polyline_trajectory) EXPECT_TRUE("follow_polyline_trajectory" == str); } +/** + * @note + */ TEST(Behavior, getRequestString_walk_straight) { const auto req = traffic_simulator::behavior::Request::WALK_STRAIGHT; std::string str; EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); EXPECT_TRUE("walk_straight" == str); -} \ No newline at end of file +} diff --git a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp new file mode 100644 index 00000000000..7c0dadb4a14 --- /dev/null +++ b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp @@ -0,0 +1,177 @@ +// 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 + +#include + +#include "../catalogs.hpp" +#include "../entity_helper_functions.hpp" +#include "../expect_eq_macros.hpp" + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +/** + * @note Test functionality used by other units. + * Test accessor getGoalPoses. + */ +TEST(RoutePlanner, getGoalPoses) +{ + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + traffic_simulator::RoutePlanner planner(hdmap_utils_ptr); + lanelet::Id id_0 = 120659; + lanelet::Id id_1 = 120660; + lanelet::Id id_2 = 34468; + auto pose_0 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); + auto pose_1 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_1); + auto pose_2 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_2); + + std::vector in_poses; + in_poses.push_back(pose_0); + in_poses.push_back(pose_1); + in_poses.push_back(pose_2); + + planner.setWaypoints(in_poses); + + auto out_poses = planner.getGoalPoses(); + + EXPECT_EQ(in_poses.size(), out_poses.size()); + for (auto it_in = in_poses.begin(), it_out = out_poses.end(); + it_in != in_poses.end() && it_out != out_poses.end(); ++it_in, ++it_out) { + EXPECT_LANELET_POSE_EQ( + static_cast(*it_in), + static_cast(*it_out)); + } +} + +/** + * @note + */ +TEST(RoutePlanner, getGoalPosesInWorldFrame) +{ + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + traffic_simulator::RoutePlanner planner(hdmap_utils_ptr); + lanelet::Id id_0 = 120659; + lanelet::Id id_1 = 120660; + lanelet::Id id_2 = 34468; + auto pose_0 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); + auto pose_1 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_1); + auto pose_2 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_2); + + std::vector in_poses; + in_poses.push_back(pose_0); + in_poses.push_back(pose_1); + in_poses.push_back(pose_2); + + planner.setWaypoints(in_poses); + + auto out_poses = planner.getGoalPosesInWorldFrame(); + + EXPECT_EQ(in_poses.size(), out_poses.size()); + for (size_t i = 0; i < in_poses.size(); i++) { + EXPECT_POSE_EQ(static_cast(in_poses[i]), out_poses[i]); + } +} + +/** + * @note + */ +TEST(RoutePlanner, getRouteLanelets_empty) +{ + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + traffic_simulator::RoutePlanner planner(hdmap_utils_ptr); + + lanelet::Id id_0 = 120659; + auto pose_0 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); + + const double distance = 100; + lanelet::Ids following_ids({120659, 120660, 34468, 34465, 34462}); + + planner.setWaypoints({}); + auto route = planner.getRouteLanelets(pose_0, distance); + + EXPECT_EQ(route.size(), following_ids.size()); + for (size_t i = 0; i < route.size(); i++) { + EXPECT_EQ(following_ids[i], route[i]); + } +} + +/** + * @note + */ +TEST(RoutePlanner, getRouteLanelets_horizon) +{ + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + traffic_simulator::RoutePlanner planner(hdmap_utils_ptr); + + lanelet::Id id_start = 120659; + lanelet::Id id_target = 34579; + auto pose_start = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_start); + auto pose_target = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_target); + + const double distance = 1000; + + planner.setWaypoints({pose_target}); + auto route = planner.getRouteLanelets(pose_start, distance); + + EXPECT_TRUE(std::find(route.begin(), route.end(), id_target) != route.end()); +} + +/** + * @note + */ +TEST(RoutePlanner, getRouteLanelets_noHorizon) +{ + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + traffic_simulator::RoutePlanner planner(hdmap_utils_ptr); + + lanelet::Id id_start = 120659; + lanelet::Id id_target = 34579; + auto pose_start = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_start); + auto pose_target = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_target); + + const double distance = 100; + + planner.setWaypoints({pose_target}); + auto route = planner.getRouteLanelets(pose_start, distance); + + EXPECT_FALSE(std::find(route.begin(), route.end(), id_target) != route.end()); +} + +/** + * @note + */ +TEST(RoutePlanner, cancelGoal) +{ + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + traffic_simulator::RoutePlanner planner(hdmap_utils_ptr); + + lanelet::Id id_start = 120659; + lanelet::Id id_target = 34579; + auto pose_start = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_start); + auto pose_target = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_target); + + const double distance = 100; + + planner.setWaypoints({pose_target}); + auto route_0 = planner.getRouteLanelets(pose_start, distance); + planner.cancelRoute(); + auto route_1 = planner.getRouteLanelets(pose_start, distance); + + EXPECT_NE(route_0, route_1); +} diff --git a/simulation/traffic_simulator/test/src/entity/CMakeLists.txt b/simulation/traffic_simulator/test/src/entity/CMakeLists.txt index 0e6b9eee47d..0dcb3f4ca29 100644 --- a/simulation/traffic_simulator/test/src/entity/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/entity/CMakeLists.txt @@ -1,2 +1,5 @@ ament_add_gtest(test_vehicle_entity test_vehicle_entity.cpp) target_link_libraries(test_vehicle_entity traffic_simulator) + +ament_add_gtest(test_misc_object_entity test_misc_object_entity.cpp) +target_link_libraries(test_misc_object_entity traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp new file mode 100644 index 00000000000..da5cbd2496f --- /dev/null +++ b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp @@ -0,0 +1,271 @@ +// 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 + +#include +#include +#include +#include + +#include "../catalogs.hpp" +#include "../entity_helper_functions.hpp" +#include "../expect_eq_macros.hpp" + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +/** + * @note + */ +TEST(MiscObjectEntity, requestSpeedChange_absolute) +{ + lanelet::Id id = 120659; + const double initial_speed = 0.0; + std::string entity_name("blob"); + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = + makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); + traffic_simulator_msgs::msg::MiscObjectParameters params{}; + traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); + + const double target_speed = 10.0; + const bool continuous = false; + EXPECT_THROW(blob.requestSpeedChange(target_speed, continuous), common::Error); +} + +/** + * @note + */ +TEST(MiscObjectEntity, requestSpeedChange_relative) +{ + lanelet::Id id = 120659; + const double initial_speed = 0.0; + std::string entity_name("blob"); + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = + makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); + traffic_simulator_msgs::msg::MiscObjectParameters params{}; + traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + blob.setOtherStatus(others); + + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + bool continuous = true; + + EXPECT_THROW(blob.requestSpeedChange(relative_taget_speed, continuous), common::Error); +} + +/** + * @note + */ +TEST(MiscObjectEntity, requestSpeedChange_absoluteTransition) +{ + lanelet::Id id = 120659; + const double initial_speed = 0.0; + std::string entity_name("blob"); + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = + makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); + traffic_simulator_msgs::msg::MiscObjectParameters params{}; + traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); + + const double target_speed = 10.0; + auto transition = traffic_simulator::speed_change::Transition::AUTO; + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::NONE, 0.0); + const bool continuous = false; + + EXPECT_THROW( + blob.requestSpeedChange(target_speed, transition, constraint, continuous), common::Error); +} + +/** + * @note + */ +TEST(MiscObjectEntity, requestAcquirePosition_pose) +{ + lanelet::Id id = 120659; + const double initial_speed = 0.0; + std::string entity_name("blob"); + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = + makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); + traffic_simulator_msgs::msg::MiscObjectParameters params{}; + traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); + + geometry_msgs::msg::Pose target_pose; + target_pose.position.x = 3759.34; + target_pose.position.y = 73791.38; + + EXPECT_THROW(blob.requestAcquirePosition(target_pose), common::Error); +} + +/** + * @note + */ +TEST(MiscObjectEntity, requestAssignRoute_laneletPose) +{ + lanelet::Id id_0 = 120659; + lanelet::Id id_1 = 120660; + const double initial_speed = 0.0; + std::string entity_name("blob"); + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); + auto bbox = makeBoundingBox(); + auto status = + makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); + traffic_simulator_msgs::msg::MiscObjectParameters params{}; + traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); + + auto target_pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_1); + + std::vector target_poses; + target_poses.push_back(target_pose); + + EXPECT_THROW(blob.requestAssignRoute(target_poses), common::Error); +} + +/** + * @note + */ +TEST(MiscObjectEntity, requestAssignRoute_pose) +{ + lanelet::Id id = 120659; + const double initial_speed = 0.0; + std::string entity_name("blob"); + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = + makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); + traffic_simulator_msgs::msg::MiscObjectParameters params{}; + traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); + + geometry_msgs::msg::Pose target_pose; + target_pose.position.x = 3759.34; + target_pose.position.y = 73791.38; + + std::vector target_poses; + target_poses.push_back(target_pose); + + EXPECT_THROW(blob.requestAssignRoute(target_poses), common::Error); +} + +/** + * @note + */ +TEST(MiscObjectEntity, requestAcquirePosition_laneletPose) +{ + lanelet::Id id_0 = 120659; + lanelet::Id id_1 = 120660; + const double initial_speed = 0.0; + std::string entity_name("blob"); + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); + auto bbox = makeBoundingBox(); + auto status = + makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); + traffic_simulator_msgs::msg::MiscObjectParameters params{}; + traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); + + auto target_pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_1); + + EXPECT_THROW(blob.requestAcquirePosition(target_pose), common::Error); +} + +/** + * @note + */ +TEST(MiscObjectEntity, getCurrentAction_npcStarted) +{ + lanelet::Id id = 120659; + const double initial_speed = 0.0; + std::string entity_name("blob"); + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto non_canonicalized_status = + makeEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); + non_canonicalized_status.action_status.current_action = "purposelessly_existing"; + traffic_simulator::entity_status::CanonicalizedEntityStatus status( + non_canonicalized_status, hdmap_utils_ptr); + traffic_simulator_msgs::msg::MiscObjectParameters params{}; + traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); + + blob.startNpcLogic(); + EXPECT_TRUE(blob.isNpcLogicStarted()); + EXPECT_TRUE(blob.getCurrentAction() == "purposelessly_existing"); +} + +/** + * @note + */ +TEST(MiscObjectEntity, getCurrentAction_npcNotStarted) +{ + lanelet::Id id = 120659; + const double initial_speed = 0.0; + std::string entity_name("blob"); + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto non_canonicalized_status = + makeEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); + non_canonicalized_status.action_status.current_action = "purposelessly_existing"; + traffic_simulator::entity_status::CanonicalizedEntityStatus status( + non_canonicalized_status, hdmap_utils_ptr); + traffic_simulator_msgs::msg::MiscObjectParameters params{}; + traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); + + EXPECT_FALSE(blob.isNpcLogicStarted()); + EXPECT_TRUE(blob.getCurrentAction() == "waiting"); +} + +/** + * @note + */ +TEST(MiscObjectEntity, getRouteLanelets) +{ + lanelet::Id id = 120659; + const double initial_speed = 0.0; + std::string entity_name("blob"); + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = + makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); + traffic_simulator_msgs::msg::MiscObjectParameters params{}; + traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); + + const double horizon = 100.0; + EXPECT_THROW(blob.getRouteLanelets(horizon), common::Error); +} diff --git a/simulation/traffic_simulator/test/src/entity_helper_functions.hpp b/simulation/traffic_simulator/test/src/entity_helper_functions.hpp new file mode 100644 index 00000000000..de50a1dbb12 --- /dev/null +++ b/simulation/traffic_simulator/test/src/entity_helper_functions.hpp @@ -0,0 +1,133 @@ +// 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 TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ +#define TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ + +#include +#include +#include +#include + +#include "catalogs.hpp" +#include "expect_eq_macros.hpp" + +auto makeHdMapUtilsSharedPointer() -> std::shared_ptr +{ + std::string path = + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; + geographic_msgs::msg::GeoPoint origin; + origin.latitude = 35.9037067912303; + origin.longitude = 139.9337945139059; + return std::make_shared(path, origin); +} + +auto makeCanonicalizedLaneletPose( + std::shared_ptr hdmap_utils, const lanelet::Id id = 120659, + const double s = 0.0, const double offset = 0.0) + -> traffic_simulator::lanelet_pose::CanonicalizedLaneletPose +{ + return traffic_simulator::lanelet_pose::CanonicalizedLaneletPose( + traffic_simulator::helper::constructLaneletPose(id, s, offset), hdmap_utils); +} + +auto makeBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs::msg::BoundingBox +{ + traffic_simulator_msgs::msg::BoundingBox bbox; + bbox.center.x = 1.0; + bbox.center.y = center_y; + bbox.dimensions.x = 4.0; + bbox.dimensions.y = 2.0; + bbox.dimensions.z = 1.5; + return bbox; +} + +auto makeEntityStatus( + std::shared_ptr hdmap_utils, + traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, + traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, + const std::string name = "dummy_entity") -> traffic_simulator::EntityStatus +{ + traffic_simulator::EntityStatus entity_status; + entity_status.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + entity_status.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; + entity_status.time = 0.0; + entity_status.name = name; + entity_status.bounding_box = bbox; + geometry_msgs::msg::Twist twist; + entity_status.action_status = + traffic_simulator::helper::constructActionStatus(speed, 0.0, 0.0, 0.0); + entity_status.lanelet_pose_valid = true; + entity_status.lanelet_pose = static_cast(pose); + entity_status.pose = hdmap_utils->toMapPose(entity_status.lanelet_pose).pose; + return entity_status; +} + +auto makeEntityStatus( + std::shared_ptr /* hdmap_utils */, geometry_msgs::msg::Pose pose, + traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, + const std::string name = "dummy_entity") -> traffic_simulator::EntityStatus +{ + traffic_simulator::EntityStatus entity_status; + entity_status.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + entity_status.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; + entity_status.time = 0.0; + entity_status.name = name; + entity_status.bounding_box = bbox; + geometry_msgs::msg::Twist twist; + entity_status.action_status = + traffic_simulator::helper::constructActionStatus(speed, 0.0, 0.0, 0.0); + entity_status.lanelet_pose_valid = false; + entity_status.pose = pose; + return entity_status; +} + +auto makeCanonicalizedEntityStatus( + std::shared_ptr hdmap_utils, + traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, + traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, + const std::string name = "dummy_entity") + -> traffic_simulator::entity_status::CanonicalizedEntityStatus +{ + return traffic_simulator::entity_status::CanonicalizedEntityStatus( + makeEntityStatus(hdmap_utils, pose, bbox, speed, name), hdmap_utils); +} + +auto makeCanonicalizedEntityStatus( + std::shared_ptr hdmap_utils, geometry_msgs::msg::Pose pose, + traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, + const std::string name = "dummy_entity") + -> traffic_simulator::entity_status::CanonicalizedEntityStatus +{ + return traffic_simulator::entity_status::CanonicalizedEntityStatus( + makeEntityStatus(hdmap_utils, pose, bbox, speed, name), hdmap_utils); +} + +auto makePoint(const double x, const double y, const double z = 0.0) -> geometry_msgs::msg::Point +{ + geometry_msgs::msg::Point point; + point.x = x; + point.y = y; + point.z = z; + return point; +} + +auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion +{ + geometry_msgs::msg::Vector3 v; + v.z = yaw; + return quaternion_operation::convertEulerAngleToQuaternion(v); +} + +#endif // TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ diff --git a/simulation/traffic_simulator/test/src/job/test_job.cpp b/simulation/traffic_simulator/test/src/job/test_job.cpp index d7e6ac63989..2470974aa54 100644 --- a/simulation/traffic_simulator/test/src/job/test_job.cpp +++ b/simulation/traffic_simulator/test/src/job/test_job.cpp @@ -1,3 +1,17 @@ +// 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 #include @@ -8,12 +22,15 @@ int main(int argc, char ** argv) return RUN_ALL_TESTS(); } +/** + * @note + */ TEST(Job, onUpdate) { bool was_cleanup_func_called = false; auto update_func = [](const double) { return true; }; auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; - const auto type = traffic_simulator::job::Type::UNKNOWN; + const auto type = traffic_simulator::job::Type::UNKOWN; const auto event = traffic_simulator::job::Event::POST_UPDATE; const bool is_exclusive = true; @@ -24,12 +41,15 @@ TEST(Job, onUpdate) EXPECT_TRUE(was_cleanup_func_called); } +/** + * @note + */ TEST(Job, get_type) { bool was_cleanup_func_called = false; auto update_func = [](const double) { return true; }; auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; - const auto type = traffic_simulator::job::Type::UNKNOWN; + const auto type = traffic_simulator::job::Type::UNKOWN; const auto event = traffic_simulator::job::Event::POST_UPDATE; const bool is_exclusive = true; @@ -43,12 +63,15 @@ TEST(Job, get_type) EXPECT_TRUE(job.type == type); } +/** + * @note + */ TEST(Job, get_exclusive) { bool was_cleanup_func_called = false; auto update_func = [](const double) { return true; }; auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; - const auto type = traffic_simulator::job::Type::UNKNOWN; + const auto type = traffic_simulator::job::Type::UNKOWN; const auto event = traffic_simulator::job::Event::POST_UPDATE; const bool is_exclusive = true; @@ -62,12 +85,15 @@ TEST(Job, get_exclusive) EXPECT_TRUE(job.exclusive == is_exclusive); } +/** + * @note + */ TEST(Job, get_event) { bool was_cleanup_func_called = false; auto update_func = [](const double) { return true; }; auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; - const auto type = traffic_simulator::job::Type::UNKNOWN; + const auto type = traffic_simulator::job::Type::UNKOWN; const auto event = traffic_simulator::job::Event::POST_UPDATE; const bool is_exclusive = true; @@ -79,4 +105,4 @@ TEST(Job, get_event) job.onUpdate(step_time); EXPECT_TRUE(job.event == event); -} \ No newline at end of file +} diff --git a/simulation/traffic_simulator/test/src/job/test_job_list.cpp b/simulation/traffic_simulator/test/src/job/test_job_list.cpp index 84746407668..8764d5822fd 100644 --- a/simulation/traffic_simulator/test/src/job/test_job_list.cpp +++ b/simulation/traffic_simulator/test/src/job/test_job_list.cpp @@ -22,12 +22,15 @@ int main(int argc, char ** argv) return RUN_ALL_TESTS(); } +/** + * @note + */ TEST(JobList, append) { bool was_cleanup_func_called = false; auto update_func = [](const double) { return true; }; auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; - const auto type = traffic_simulator::job::Type::UNKNOWN; + const auto type = traffic_simulator::job::Type::UNKOWN; const auto event = traffic_simulator::job::Event::POST_UPDATE; const bool is_exclusive = true; @@ -41,6 +44,9 @@ TEST(JobList, append) EXPECT_TRUE(was_cleanup_func_called); } +/** + * @note + */ TEST(JobList, append_doubled) { bool first_cleanup = false; @@ -53,7 +59,7 @@ TEST(JobList, append_doubled) auto second_update_func = [&second_update](const double) { return second_update = true; }; auto second_cleanup_func = [&second_cleanup]() { second_cleanup = true; }; - const auto type = traffic_simulator::job::Type::UNKNOWN; + const auto type = traffic_simulator::job::Type::UNKOWN; const auto event = traffic_simulator::job::Event::POST_UPDATE; const bool is_exclusive = true; @@ -71,6 +77,9 @@ TEST(JobList, append_doubled) EXPECT_TRUE(second_update); } +/** + * @note + */ TEST(JobList, update) { int update_count = 0; @@ -82,7 +91,7 @@ TEST(JobList, update) }; auto cleanup_func = [&cleanup_count]() { cleanup_count++; }; - const auto type = traffic_simulator::job::Type::UNKNOWN; + const auto type = traffic_simulator::job::Type::UNKOWN; const auto event = traffic_simulator::job::Event::POST_UPDATE; const bool is_exclusive = true; @@ -109,4 +118,4 @@ TEST(JobList, update) EXPECT_EQ(2, update_count); EXPECT_EQ(1, cleanup_count); -} \ No newline at end of file +} diff --git a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp index 398a7b28773..81228867af9 100644 --- a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp +++ b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp @@ -16,6 +16,9 @@ #include +/** + * @note + */ TEST(SimulationClock, Initialize) { const double realtime_factor = 1.0; @@ -33,6 +36,9 @@ TEST(SimulationClock, Initialize) EXPECT_THROW(simulation_clock.start(), std::runtime_error); } +/** + * @note + */ TEST(SimulationClock, getCurrentRosTime) { const double realtime_factor = 2.0; @@ -57,6 +63,9 @@ TEST(SimulationClock, getCurrentRosTime) EXPECT_NEAR(result_elapsed_time, actual_elapsed_time, epsilon); } +/** + * @note + */ TEST(SimulationClock, getCurrentScenarioTime) { const double realtime_factor = 1.0; @@ -81,6 +90,9 @@ TEST(SimulationClock, getCurrentScenarioTime) EXPECT_NEAR(actual_elapsed_time, result_elapsed_time, epsilon); } +/** + * @note + */ TEST(SimulationClock, Update) { const double realtime_factor = 1.0; @@ -104,4 +116,4 @@ TEST(SimulationClock, Update) const double actual_simulation_time = simulation_clock.getCurrentSimulationTime(); EXPECT_NEAR(actual_simulation_time, expected_simulation_time, tolerance); } -} \ No newline at end of file +} diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp index 113794f6eae..954328da72b 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp @@ -241,26 +241,26 @@ TEST(TrafficLight, Bulb) using Bulb = TrafficLight::Bulb; // clang-format off - EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); - EXPECT_TRUE(Bulb(Color::yellow, Status::solid_on, Shape::circle ).hash() == 0b0000'0001'0000'0000'0000'0000'0000'0000); - EXPECT_TRUE(Bulb(Color::red, Status::solid_on, Shape::circle ).hash() == 0b0000'0010'0000'0000'0000'0000'0000'0000); - EXPECT_TRUE(Bulb(Color::white, Status::solid_on, Shape::circle ).hash() == 0b0000'0011'0000'0000'0000'0000'0000'0000); - - EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); - EXPECT_TRUE(Bulb(Color::green, Status::solid_off, Shape::circle ).hash() == 0b0000'0000'0000'0001'0000'0000'0000'0000); - EXPECT_TRUE(Bulb(Color::green, Status::flashing, Shape::circle ).hash() == 0b0000'0000'0000'0010'0000'0000'0000'0000); - EXPECT_TRUE(Bulb(Color::green, Status::unknown, Shape::circle ).hash() == 0b0000'0000'0000'0011'0000'0000'0000'0000); - - EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); - EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::cross ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0001); - EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::left ).hash() == 0b0000'0000'0000'0000'0000'1000'0000'0010); - EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::down ).hash() == 0b0000'0000'0000'0000'0000'0100'0000'0010); - EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::up ).hash() == 0b0000'0000'0000'0000'0000'0010'0000'0010); - EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::right ).hash() == 0b0000'0000'0000'0000'0000'0001'0000'0010); - EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::lower_left ).hash() == 0b0000'0000'0000'0000'0000'1100'0000'0010); - EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::upper_left ).hash() == 0b0000'0000'0000'0000'0000'1010'0000'0010); - EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::lower_right).hash() == 0b0000'0000'0000'0000'0000'0101'0000'0010); - EXPECT_TRUE(Bulb(Color::green, Status::solid_on, Shape::upper_right).hash() == 0b0000'0000'0000'0000'0000'0011'0000'0010); + static_assert(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); + static_assert(Bulb(Color::yellow, Status::solid_on, Shape::circle ).hash() == 0b0000'0001'0000'0000'0000'0000'0000'0000); + static_assert(Bulb(Color::red, Status::solid_on, Shape::circle ).hash() == 0b0000'0010'0000'0000'0000'0000'0000'0000); + static_assert(Bulb(Color::white, Status::solid_on, Shape::circle ).hash() == 0b0000'0011'0000'0000'0000'0000'0000'0000); + + static_assert(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); + static_assert(Bulb(Color::green, Status::solid_off, Shape::circle ).hash() == 0b0000'0000'0000'0001'0000'0000'0000'0000); + static_assert(Bulb(Color::green, Status::flashing, Shape::circle ).hash() == 0b0000'0000'0000'0010'0000'0000'0000'0000); + static_assert(Bulb(Color::green, Status::unknown, Shape::circle ).hash() == 0b0000'0000'0000'0011'0000'0000'0000'0000); + + static_assert(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); + static_assert(Bulb(Color::green, Status::solid_on, Shape::cross ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0001); + static_assert(Bulb(Color::green, Status::solid_on, Shape::left ).hash() == 0b0000'0000'0000'0000'0000'1000'0000'0010); + static_assert(Bulb(Color::green, Status::solid_on, Shape::down ).hash() == 0b0000'0000'0000'0000'0000'0100'0000'0010); + static_assert(Bulb(Color::green, Status::solid_on, Shape::up ).hash() == 0b0000'0000'0000'0000'0000'0010'0000'0010); + static_assert(Bulb(Color::green, Status::solid_on, Shape::right ).hash() == 0b0000'0000'0000'0000'0000'0001'0000'0010); + static_assert(Bulb(Color::green, Status::solid_on, Shape::lower_left ).hash() == 0b0000'0000'0000'0000'0000'1100'0000'0010); + static_assert(Bulb(Color::green, Status::solid_on, Shape::upper_left ).hash() == 0b0000'0000'0000'0000'0000'1010'0000'0010); + static_assert(Bulb(Color::green, Status::solid_on, Shape::lower_right).hash() == 0b0000'0000'0000'0000'0000'0101'0000'0010); + static_assert(Bulb(Color::green, Status::solid_on, Shape::upper_right).hash() == 0b0000'0000'0000'0000'0000'0011'0000'0010); // clang-format on { @@ -373,6 +373,9 @@ TEST(TrafficLight, TrafficLight) } } +/** + * @note + */ TEST(TrafficLight, Color_make) { using Color = traffic_simulator::TrafficLight::Color; @@ -422,6 +425,9 @@ TEST(TrafficLight, Color_make) } } +/** + * @note + */ TEST(TrafficLight, Shape_make) { using Shape = traffic_simulator::TrafficLight::Shape; @@ -537,6 +543,9 @@ TEST(TrafficLight, Shape_make) } } +/** + * @note + */ TEST(TrafficLight, Status_make) { using Status = traffic_simulator::TrafficLight::Status; @@ -578,6 +587,9 @@ TEST(TrafficLight, Status_make) } } +/** + * @note + */ TEST(TrafficLight, Bulb_make) { using TrafficLight = traffic_simulator::TrafficLight; @@ -649,6 +661,9 @@ TEST(TrafficLight, Bulb_make) } } +/** + * @note + */ TEST(TrafficLight, Bulb_trafficLightMessageConversion) { using TrafficLight = traffic_simulator::TrafficLight; @@ -707,6 +722,9 @@ TEST(TrafficLight, Bulb_trafficLightMessageConversion) } } +/** + * @note + */ TEST(TrafficLight, Color_make_wrong) { using Color = traffic_simulator::TrafficLight::Color; @@ -715,6 +733,9 @@ TEST(TrafficLight, Color_make_wrong) EXPECT_FALSE(boost::lexical_cast(color) == "wrong_color"); } +/** + * @note + */ TEST(TrafficLight, Shape_make_wrong) { using Shape = traffic_simulator::TrafficLight::Shape; @@ -723,6 +744,9 @@ TEST(TrafficLight, Shape_make_wrong) EXPECT_FALSE(boost::lexical_cast(shape) == "wrong_shape"); } +/** + * @note + */ TEST(TrafficLight, Status_make_wrong) { using Status = traffic_simulator::TrafficLight::Status; @@ -731,6 +755,9 @@ TEST(TrafficLight, Status_make_wrong) EXPECT_FALSE(boost::lexical_cast(status) == "wrong_status"); } +/** + * @note + */ TEST(TrafficLight, Bulb_make_wrong) { using Bulb = traffic_simulator::TrafficLight::Bulb; From df258bd2f9abd1524e711537113228f6a50f68c4 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 6 Jun 2024 11:34:03 +0200 Subject: [PATCH 041/118] Make ParameterManager explicit Signed-off-by: Mateusz Palczuk --- .../include/traffic_simulator/utils/parameter_manager.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp index 86b04970ebe..5cae116fb42 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp @@ -24,11 +24,15 @@ class ParameterManager { public: template - ParameterManager(NodeT && node) + explicit ParameterManager(NodeT && node) : node_parameters_interface_(rclcpp::node_interfaces::get_node_parameters_interface(node)) { } + ParameterManager() = delete; + + ParameterManager(const ParameterManager &) = default; + template auto getParameter(const std::string & name, const ParameterT & default_value = {}) const { From fe789e725902fbfad241bf9e6c1a176e85d6d7c4 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 6 Jun 2024 13:40:32 +0200 Subject: [PATCH 042/118] add tes descriptions --- .../test/src/behavior/test_behavior.cpp | 80 +++++------ .../test/src/behavior/test_route_planner.cpp | 56 +++----- .../src/entity/test_misc_object_entity.cpp | 129 +++++++++--------- .../test/src/job/test_job.cpp | 71 +--------- .../test/src/job/test_job_list.cpp | 10 +- .../test_simulation_clock.cpp | 14 +- .../src/traffic_lights/test_traffic_light.cpp | 20 +-- 7 files changed, 151 insertions(+), 229 deletions(-) diff --git a/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp b/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp index 3f783954530..434e7fcbbac 100644 --- a/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp +++ b/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp @@ -23,56 +23,42 @@ int main(int argc, char ** argv) } /** - * @note + * @note Test basic functionality. Test whether correct string is retrieved. */ -TEST(Behavior, getRequestString_none) +TEST(Behavior, getRequestString) { - const auto req = traffic_simulator::behavior::Request::NONE; - std::string str; - EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); - EXPECT_TRUE("none" == str); -} + { + const auto req = traffic_simulator::behavior::Request::NONE; + std::string str; + EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); + EXPECT_TRUE("none" == str); + } -/** - * @note - */ -TEST(Behavior, getRequestString_lane_change) -{ - const auto req = traffic_simulator::behavior::Request::LANE_CHANGE; - std::string str; - EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); - EXPECT_TRUE("lane_change" == str); -} + { + const auto req = traffic_simulator::behavior::Request::LANE_CHANGE; + std::string str; + EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); + EXPECT_TRUE("lane_change" == str); + } -/** - * @note - */ -TEST(Behavior, getRequestString_follow_lane) -{ - const auto req = traffic_simulator::behavior::Request::FOLLOW_LANE; - std::string str; - EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); - EXPECT_TRUE("follow_lane" == str); -} + { + const auto req = traffic_simulator::behavior::Request::FOLLOW_LANE; + std::string str; + EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); + EXPECT_TRUE("follow_lane" == str); + } -/** - * @note - */ -TEST(Behavior, getRequestString_follow_polyline_trajectory) -{ - const auto req = traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY; - std::string str; - EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); - EXPECT_TRUE("follow_polyline_trajectory" == str); -} + { + const auto req = traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY; + std::string str; + EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); + EXPECT_TRUE("follow_polyline_trajectory" == str); + } -/** - * @note - */ -TEST(Behavior, getRequestString_walk_straight) -{ - const auto req = traffic_simulator::behavior::Request::WALK_STRAIGHT; - std::string str; - EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); - EXPECT_TRUE("walk_straight" == str); -} + { + const auto req = traffic_simulator::behavior::Request::WALK_STRAIGHT; + std::string str; + EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); + EXPECT_TRUE("walk_straight" == str); + } +} \ No newline at end of file diff --git a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp index 7c0dadb4a14..650a0ffb463 100644 --- a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp +++ b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp @@ -60,7 +60,8 @@ TEST(RoutePlanner, getGoalPoses) } /** - * @note + * @note Test functionality used by other units. + * Test performing transformations by getGoalPosesInWorldFrame accessor. */ TEST(RoutePlanner, getGoalPosesInWorldFrame) { @@ -89,30 +90,8 @@ TEST(RoutePlanner, getGoalPosesInWorldFrame) } /** - * @note - */ -TEST(RoutePlanner, getRouteLanelets_empty) -{ - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - traffic_simulator::RoutePlanner planner(hdmap_utils_ptr); - - lanelet::Id id_0 = 120659; - auto pose_0 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); - - const double distance = 100; - lanelet::Ids following_ids({120659, 120660, 34468, 34465, 34462}); - - planner.setWaypoints({}); - auto route = planner.getRouteLanelets(pose_0, distance); - - EXPECT_EQ(route.size(), following_ids.size()); - for (size_t i = 0; i < route.size(); i++) { - EXPECT_EQ(following_ids[i], route[i]); - } -} - -/** - * @note + * @note Test functionality used by other units. + * Test routing correctness with an entity pose and target pose spaced apart for more than horizon. */ TEST(RoutePlanner, getRouteLanelets_horizon) { @@ -133,7 +112,8 @@ TEST(RoutePlanner, getRouteLanelets_horizon) } /** - * @note + * @note Test functionality used by other units. + * Test routing correctness with an entity pose and target pose spaced apart for less than horizon. */ TEST(RoutePlanner, getRouteLanelets_noHorizon) { @@ -154,24 +134,26 @@ TEST(RoutePlanner, getRouteLanelets_noHorizon) } /** - * @note + * @note Test functionality used by other units. + * Test routing correctness with an entity pose and empty waypoints vector + * - the goal is to test function behavior when empty vector is passed. */ -TEST(RoutePlanner, cancelGoal) +TEST(RoutePlanner, getRouteLanelets_empty) { auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); traffic_simulator::RoutePlanner planner(hdmap_utils_ptr); - lanelet::Id id_start = 120659; - lanelet::Id id_target = 34579; - auto pose_start = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_start); - auto pose_target = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_target); + lanelet::Id id_0 = 120659; + auto pose_0 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); const double distance = 100; + lanelet::Ids following_ids({120659, 120660, 34468, 34465, 34462}); - planner.setWaypoints({pose_target}); - auto route_0 = planner.getRouteLanelets(pose_start, distance); - planner.cancelRoute(); - auto route_1 = planner.getRouteLanelets(pose_start, distance); + planner.setWaypoints({}); + auto route = planner.getRouteLanelets(pose_0, distance); - EXPECT_NE(route_0, route_1); + EXPECT_EQ(route.size(), following_ids.size()); + for (size_t i = 0; i < route.size(); i++) { + EXPECT_EQ(following_ids[i], route[i]); + } } diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp index da5cbd2496f..e9ae2ac3a7b 100644 --- a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp @@ -30,7 +30,54 @@ int main(int argc, char ** argv) } /** - * @note + * @note Test basic functionality. Test current action obtaining when NPC logic is not started. + */ +TEST(MiscObjectEntity, getCurrentAction_npcNotStarted) +{ + lanelet::Id id = 120659; + const double initial_speed = 0.0; + std::string entity_name("blob"); + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto non_canonicalized_status = + makeEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); + non_canonicalized_status.action_status.current_action = "purposelessly_existing"; + traffic_simulator::entity_status::CanonicalizedEntityStatus status( + non_canonicalized_status, hdmap_utils_ptr); + traffic_simulator_msgs::msg::MiscObjectParameters params{}; + traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); + + EXPECT_FALSE(blob.isNpcLogicStarted()); + EXPECT_TRUE(blob.getCurrentAction() == "waiting"); +} + +/** + * @note Test basic functionality. Test current action obtaining when NPC logic is started. + */ +TEST(MiscObjectEntity, getCurrentAction_npcStarted) +{ + lanelet::Id id = 120659; + const double initial_speed = 0.0; + std::string entity_name("blob"); + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto non_canonicalized_status = + makeEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); + non_canonicalized_status.action_status.current_action = "purposelessly_existing"; + traffic_simulator::entity_status::CanonicalizedEntityStatus status( + non_canonicalized_status, hdmap_utils_ptr); + traffic_simulator_msgs::msg::MiscObjectParameters params{}; + traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); + + blob.startNpcLogic(); + EXPECT_TRUE(blob.isNpcLogicStarted()); + EXPECT_TRUE(blob.getCurrentAction() == "purposelessly_existing"); +} + +/** + * @note Test function behavior when absolute speed change is requested - the goal is to test throwing error. */ TEST(MiscObjectEntity, requestSpeedChange_absolute) { @@ -51,7 +98,7 @@ TEST(MiscObjectEntity, requestSpeedChange_absolute) } /** - * @note + * @note Test function behavior when relative speed change is requested - the goal is to test throwing error. */ TEST(MiscObjectEntity, requestSpeedChange_relative) { @@ -82,7 +129,8 @@ TEST(MiscObjectEntity, requestSpeedChange_relative) } /** - * @note + * @note Test function behavior when relative speed change with transition type is requested + * - the goal is to test throwing error. */ TEST(MiscObjectEntity, requestSpeedChange_absoluteTransition) { @@ -108,30 +156,8 @@ TEST(MiscObjectEntity, requestSpeedChange_absoluteTransition) } /** - * @note - */ -TEST(MiscObjectEntity, requestAcquirePosition_pose) -{ - lanelet::Id id = 120659; - const double initial_speed = 0.0; - std::string entity_name("blob"); - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = - makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); - traffic_simulator_msgs::msg::MiscObjectParameters params{}; - traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); - - geometry_msgs::msg::Pose target_pose; - target_pose.position.x = 3759.34; - target_pose.position.y = 73791.38; - - EXPECT_THROW(blob.requestAcquirePosition(target_pose), common::Error); -} - -/** - * @note + * @note Test function behavior when route assigning is requested with lanelet pose + * - the goal is to test throwing error. */ TEST(MiscObjectEntity, requestAssignRoute_laneletPose) { @@ -156,7 +182,8 @@ TEST(MiscObjectEntity, requestAssignRoute_laneletPose) } /** - * @note + * @note Test function behavior when route assigning is requested with pose + * - the goal is to test throwing error. */ TEST(MiscObjectEntity, requestAssignRoute_pose) { @@ -182,7 +209,8 @@ TEST(MiscObjectEntity, requestAssignRoute_pose) } /** - * @note + * @note Test function behavior when position acquiring is requested with lanelet pose + * - the goal is to test throwing error. */ TEST(MiscObjectEntity, requestAcquirePosition_laneletPose) { @@ -204,9 +232,10 @@ TEST(MiscObjectEntity, requestAcquirePosition_laneletPose) } /** - * @note + * @note Test function behavior when position acquiring is requested with pose + * - the goal is to test throwing error. */ -TEST(MiscObjectEntity, getCurrentAction_npcStarted) +TEST(MiscObjectEntity, requestAcquirePosition_pose) { lanelet::Id id = 120659; const double initial_speed = 0.0; @@ -214,44 +243,20 @@ TEST(MiscObjectEntity, getCurrentAction_npcStarted) auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); auto bbox = makeBoundingBox(); - auto non_canonicalized_status = - makeEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); - non_canonicalized_status.action_status.current_action = "purposelessly_existing"; - traffic_simulator::entity_status::CanonicalizedEntityStatus status( - non_canonicalized_status, hdmap_utils_ptr); + auto status = + makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); traffic_simulator_msgs::msg::MiscObjectParameters params{}; traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); - blob.startNpcLogic(); - EXPECT_TRUE(blob.isNpcLogicStarted()); - EXPECT_TRUE(blob.getCurrentAction() == "purposelessly_existing"); -} - -/** - * @note - */ -TEST(MiscObjectEntity, getCurrentAction_npcNotStarted) -{ - lanelet::Id id = 120659; - const double initial_speed = 0.0; - std::string entity_name("blob"); - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto non_canonicalized_status = - makeEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); - non_canonicalized_status.action_status.current_action = "purposelessly_existing"; - traffic_simulator::entity_status::CanonicalizedEntityStatus status( - non_canonicalized_status, hdmap_utils_ptr); - traffic_simulator_msgs::msg::MiscObjectParameters params{}; - traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); + geometry_msgs::msg::Pose target_pose; + target_pose.position.x = 3759.34; + target_pose.position.y = 73791.38; - EXPECT_FALSE(blob.isNpcLogicStarted()); - EXPECT_TRUE(blob.getCurrentAction() == "waiting"); + EXPECT_THROW(blob.requestAcquirePosition(target_pose), common::Error); } /** - * @note + * @note Test function behavior when called with any argument - the goal is to test error throwing. */ TEST(MiscObjectEntity, getRouteLanelets) { diff --git a/simulation/traffic_simulator/test/src/job/test_job.cpp b/simulation/traffic_simulator/test/src/job/test_job.cpp index 2470974aa54..f6b4c5678e0 100644 --- a/simulation/traffic_simulator/test/src/job/test_job.cpp +++ b/simulation/traffic_simulator/test/src/job/test_job.cpp @@ -23,7 +23,10 @@ int main(int argc, char ** argv) } /** - * @note + * @note Test basic functionality. Test functions execution correctness with a function on update + * that returns true and function on cleanup that marks that it was called + * - the goal is to check whether the function on cleanup is called + * after calling onUpdate (which immediately finishes). */ TEST(Job, onUpdate) { @@ -40,69 +43,3 @@ TEST(Job, onUpdate) EXPECT_TRUE(was_cleanup_func_called); } - -/** - * @note - */ -TEST(Job, get_type) -{ - bool was_cleanup_func_called = false; - auto update_func = [](const double) { return true; }; - auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; - const auto type = traffic_simulator::job::Type::UNKOWN; - const auto event = traffic_simulator::job::Event::POST_UPDATE; - const bool is_exclusive = true; - - auto job = traffic_simulator::job::Job(update_func, cleanup_func, type, is_exclusive, event); - - EXPECT_TRUE(job.type == type); - - const double step_time = 0.0; - job.onUpdate(step_time); - - EXPECT_TRUE(job.type == type); -} - -/** - * @note - */ -TEST(Job, get_exclusive) -{ - bool was_cleanup_func_called = false; - auto update_func = [](const double) { return true; }; - auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; - const auto type = traffic_simulator::job::Type::UNKOWN; - const auto event = traffic_simulator::job::Event::POST_UPDATE; - const bool is_exclusive = true; - - auto job = traffic_simulator::job::Job(update_func, cleanup_func, type, is_exclusive, event); - - EXPECT_TRUE(job.exclusive == is_exclusive); - - const double step_time = 0.0; - job.onUpdate(step_time); - - EXPECT_TRUE(job.exclusive == is_exclusive); -} - -/** - * @note - */ -TEST(Job, get_event) -{ - bool was_cleanup_func_called = false; - auto update_func = [](const double) { return true; }; - auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; - const auto type = traffic_simulator::job::Type::UNKOWN; - const auto event = traffic_simulator::job::Event::POST_UPDATE; - const bool is_exclusive = true; - - auto job = traffic_simulator::job::Job(update_func, cleanup_func, type, is_exclusive, event); - - EXPECT_TRUE(job.event == event); - - const double step_time = 0.0; - job.onUpdate(step_time); - - EXPECT_TRUE(job.event == event); -} diff --git a/simulation/traffic_simulator/test/src/job/test_job_list.cpp b/simulation/traffic_simulator/test/src/job/test_job_list.cpp index 8764d5822fd..ad9c97d8427 100644 --- a/simulation/traffic_simulator/test/src/job/test_job_list.cpp +++ b/simulation/traffic_simulator/test/src/job/test_job_list.cpp @@ -23,7 +23,8 @@ int main(int argc, char ** argv) } /** - * @note + * @note Test basic functionality. + * Test adding a job to an empty job list and (test whether adding worked by calling update). */ TEST(JobList, append) { @@ -45,7 +46,9 @@ TEST(JobList, append) } /** - * @note + * @note Test basic functionality. Test adding a job to a job list that already + * has an identical job (test whether adding worked by calling update) + * - the goal is to test whether the old job was removed when adding identical one. */ TEST(JobList, append_doubled) { @@ -78,7 +81,8 @@ TEST(JobList, append_doubled) } /** - * @note + * @note Test basic functionality. Test updating process with a job that will end after 2 updates + * and will mark that it has cleaned up. */ TEST(JobList, update) { diff --git a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp index 81228867af9..57d0be7addf 100644 --- a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp +++ b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp @@ -17,7 +17,9 @@ #include /** - * @note + * @note Test basic functionality used in API. + * Test initialization logic by calling update without initialized clock + * - the goal is to verify that mandatory initialization works. */ TEST(SimulationClock, Initialize) { @@ -37,7 +39,9 @@ TEST(SimulationClock, Initialize) } /** - * @note + * @note Test basic functionality used in API. Test time obtaining correctness with initialized object + * and not using raw clock - the goal is to test whether the time has increased + * according to the times of update function calls. */ TEST(SimulationClock, getCurrentRosTime) { @@ -64,7 +68,8 @@ TEST(SimulationClock, getCurrentRosTime) } /** - * @note + * @note Test basic functionality used in API. Test scenario time calculation correctness with initialized obejct, + * npc logic started after several update() calls and additional update() calls after starting npc logic. */ TEST(SimulationClock, getCurrentScenarioTime) { @@ -91,7 +96,8 @@ TEST(SimulationClock, getCurrentScenarioTime) } /** - * @note + * @note Test basic functionality used in API. Test updating correctness with initialized object + * by calling update several times expecting the current time to increase accordingly. */ TEST(SimulationClock, Update) { diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp index 954328da72b..c6c73ef1df8 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp @@ -374,7 +374,8 @@ TEST(TrafficLight, TrafficLight) } /** - * @note + * @note Test basic functionality. Test whether the function + * creates Color object appropriate to the argument. */ TEST(TrafficLight, Color_make) { @@ -426,7 +427,7 @@ TEST(TrafficLight, Color_make) } /** - * @note + * @note Test basic functionality. Test whether the function creates Color object appropriate to the argument. */ TEST(TrafficLight, Shape_make) { @@ -544,7 +545,7 @@ TEST(TrafficLight, Shape_make) } /** - * @note + * @note Test basic functionality. Test whether the function creates Status object appropriate to the argument. */ TEST(TrafficLight, Status_make) { @@ -588,7 +589,7 @@ TEST(TrafficLight, Status_make) } /** - * @note + * @note Test basic functionality. Test whether the function creates Color object appropriate to the argument. */ TEST(TrafficLight, Bulb_make) { @@ -662,7 +663,8 @@ TEST(TrafficLight, Bulb_make) } /** - * @note + * @note Test basic functionality. Test whether the TrafficLight message + * is constructed configured according to the Bulb object. */ TEST(TrafficLight, Bulb_trafficLightMessageConversion) { @@ -723,7 +725,7 @@ TEST(TrafficLight, Bulb_trafficLightMessageConversion) } /** - * @note + * @note Test basic functionality. Test function behavior when called with invalid name. */ TEST(TrafficLight, Color_make_wrong) { @@ -734,7 +736,7 @@ TEST(TrafficLight, Color_make_wrong) } /** - * @note + * @note Test basic functionality. Test function behavior when called with invalid name. */ TEST(TrafficLight, Shape_make_wrong) { @@ -745,7 +747,7 @@ TEST(TrafficLight, Shape_make_wrong) } /** - * @note + * @note Test basic functionality. Test function behavior when called with invalid name. */ TEST(TrafficLight, Status_make_wrong) { @@ -756,7 +758,7 @@ TEST(TrafficLight, Status_make_wrong) } /** - * @note + * @note Test basic functionality. Test function behavior when called with invalid name. */ TEST(TrafficLight, Bulb_make_wrong) { From bd99b7d744747c23cca1cef159ee6cc73f125c5a Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 6 Jun 2024 13:43:28 +0200 Subject: [PATCH 043/118] newlines --- .../traffic_simulator/test/src/behavior/test_behavior.cpp | 2 +- .../test/src/traffic_lights/test_traffic_light.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp b/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp index 434e7fcbbac..f98797d8e99 100644 --- a/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp +++ b/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp @@ -61,4 +61,4 @@ TEST(Behavior, getRequestString) EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); EXPECT_TRUE("walk_straight" == str); } -} \ No newline at end of file +} diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp index c6c73ef1df8..c0869153d66 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp @@ -767,6 +767,5 @@ TEST(TrafficLight, Bulb_make_wrong) EXPECT_THROW(Bulb("red flashing wrong_shape"), std::runtime_error); EXPECT_THROW(Bulb("red wrong_status circle"), std::runtime_error); EXPECT_THROW(Bulb("wrong_color flashing circle"), std::runtime_error); - EXPECT_THROW(Bulb("wrong_color wrong_status wrong_shape"), std::runtime_error); } From b1572a65c5013dbcfc69c51f163a9ec66941bdae Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 6 Jun 2024 13:49:06 +0200 Subject: [PATCH 044/118] remove unnecesary checks --- .../test/src/traffic_lights/test_traffic_light.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp index c0869153d66..a60f9c5a3de 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp @@ -732,7 +732,6 @@ TEST(TrafficLight, Color_make_wrong) using Color = traffic_simulator::TrafficLight::Color; Color color; EXPECT_THROW(color = Color::make("wrong_color"), std::runtime_error); - EXPECT_FALSE(boost::lexical_cast(color) == "wrong_color"); } /** @@ -743,7 +742,6 @@ TEST(TrafficLight, Shape_make_wrong) using Shape = traffic_simulator::TrafficLight::Shape; Shape shape; EXPECT_THROW(shape = Shape::make("wrong_shape"), std::runtime_error); - EXPECT_FALSE(boost::lexical_cast(shape) == "wrong_shape"); } /** @@ -754,7 +752,6 @@ TEST(TrafficLight, Status_make_wrong) using Status = traffic_simulator::TrafficLight::Status; Status status; EXPECT_THROW(status = Status::make("wrong_status"), std::runtime_error); - EXPECT_FALSE(boost::lexical_cast(status) == "wrong_status"); } /** From a3c644714724336a401e7bb42955d8d1b369d21a Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 6 Jun 2024 15:37:54 +0200 Subject: [PATCH 045/118] remove variables that are used only once --- .../test/src/behavior/test_behavior.cpp | 25 +- .../test/src/behavior/test_route_planner.cpp | 77 ++---- .../src/entity/test_misc_object_entity.cpp | 254 ++++++++---------- .../test/src/job/test_job.cpp | 14 +- .../test/src/job/test_job_list.cpp | 44 ++- .../traffic_simulator/test/src/job/txt.txt | 33 +++ .../test_simulation_clock.cpp | 44 +-- 7 files changed, 225 insertions(+), 266 deletions(-) create mode 100644 simulation/traffic_simulator/test/src/job/txt.txt diff --git a/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp b/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp index f98797d8e99..bddada3f41a 100644 --- a/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp +++ b/simulation/traffic_simulator/test/src/behavior/test_behavior.cpp @@ -28,37 +28,42 @@ int main(int argc, char ** argv) TEST(Behavior, getRequestString) { { - const auto req = traffic_simulator::behavior::Request::NONE; std::string str; - EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); + EXPECT_NO_THROW( + str = + traffic_simulator::behavior::getRequestString(traffic_simulator::behavior::Request::NONE)); EXPECT_TRUE("none" == str); } { - const auto req = traffic_simulator::behavior::Request::LANE_CHANGE; std::string str; - EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); + EXPECT_NO_THROW( + str = traffic_simulator::behavior::getRequestString( + traffic_simulator::behavior::Request::LANE_CHANGE)); EXPECT_TRUE("lane_change" == str); } { - const auto req = traffic_simulator::behavior::Request::FOLLOW_LANE; std::string str; - EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); + EXPECT_NO_THROW( + str = traffic_simulator::behavior::getRequestString( + traffic_simulator::behavior::Request::FOLLOW_LANE)); EXPECT_TRUE("follow_lane" == str); } { - const auto req = traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY; std::string str; - EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); + EXPECT_NO_THROW( + str = traffic_simulator::behavior::getRequestString( + traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY)); EXPECT_TRUE("follow_polyline_trajectory" == str); } { - const auto req = traffic_simulator::behavior::Request::WALK_STRAIGHT; std::string str; - EXPECT_NO_THROW(str = traffic_simulator::behavior::getRequestString(req)); + EXPECT_NO_THROW( + str = traffic_simulator::behavior::getRequestString( + traffic_simulator::behavior::Request::WALK_STRAIGHT)); EXPECT_TRUE("walk_straight" == str); } } diff --git a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp index 650a0ffb463..446f56e6c7e 100644 --- a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp +++ b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp @@ -33,22 +33,16 @@ int main(int argc, char ** argv) TEST(RoutePlanner, getGoalPoses) { auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - traffic_simulator::RoutePlanner planner(hdmap_utils_ptr); - lanelet::Id id_0 = 120659; - lanelet::Id id_1 = 120660; - lanelet::Id id_2 = 34468; - auto pose_0 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); - auto pose_1 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_1); - auto pose_2 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_2); - - std::vector in_poses; - in_poses.push_back(pose_0); - in_poses.push_back(pose_1); - in_poses.push_back(pose_2); + auto planner = traffic_simulator::RoutePlanner(hdmap_utils_ptr); + + const auto in_poses = std::vector{ + makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), + makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660), + makeCanonicalizedLaneletPose(hdmap_utils_ptr, 34468)}; planner.setWaypoints(in_poses); - auto out_poses = planner.getGoalPoses(); + const auto out_poses = planner.getGoalPoses(); EXPECT_EQ(in_poses.size(), out_poses.size()); for (auto it_in = in_poses.begin(), it_out = out_poses.end(); @@ -66,22 +60,16 @@ TEST(RoutePlanner, getGoalPoses) TEST(RoutePlanner, getGoalPosesInWorldFrame) { auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - traffic_simulator::RoutePlanner planner(hdmap_utils_ptr); - lanelet::Id id_0 = 120659; - lanelet::Id id_1 = 120660; - lanelet::Id id_2 = 34468; - auto pose_0 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); - auto pose_1 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_1); - auto pose_2 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_2); - - std::vector in_poses; - in_poses.push_back(pose_0); - in_poses.push_back(pose_1); - in_poses.push_back(pose_2); + auto planner = traffic_simulator::RoutePlanner(hdmap_utils_ptr); + + const auto in_poses = std::vector{ + makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), + makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660), + makeCanonicalizedLaneletPose(hdmap_utils_ptr, 34468)}; planner.setWaypoints(in_poses); - auto out_poses = planner.getGoalPosesInWorldFrame(); + const auto out_poses = planner.getGoalPosesInWorldFrame(); EXPECT_EQ(in_poses.size(), out_poses.size()); for (size_t i = 0; i < in_poses.size(); i++) { @@ -96,17 +84,13 @@ TEST(RoutePlanner, getGoalPosesInWorldFrame) TEST(RoutePlanner, getRouteLanelets_horizon) { auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - traffic_simulator::RoutePlanner planner(hdmap_utils_ptr); - - lanelet::Id id_start = 120659; - lanelet::Id id_target = 34579; - auto pose_start = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_start); - auto pose_target = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_target); + auto planner = traffic_simulator::RoutePlanner(hdmap_utils_ptr); - const double distance = 1000; + const lanelet::Id id_target = 34579; - planner.setWaypoints({pose_target}); - auto route = planner.getRouteLanelets(pose_start, distance); + planner.setWaypoints({makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_target)}); + auto route = + planner.getRouteLanelets(makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), 1000.0); EXPECT_TRUE(std::find(route.begin(), route.end(), id_target) != route.end()); } @@ -118,17 +102,13 @@ TEST(RoutePlanner, getRouteLanelets_horizon) TEST(RoutePlanner, getRouteLanelets_noHorizon) { auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - traffic_simulator::RoutePlanner planner(hdmap_utils_ptr); + auto planner = traffic_simulator::RoutePlanner(hdmap_utils_ptr); - lanelet::Id id_start = 120659; lanelet::Id id_target = 34579; - auto pose_start = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_start); - auto pose_target = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_target); - - const double distance = 100; - planner.setWaypoints({pose_target}); - auto route = planner.getRouteLanelets(pose_start, distance); + planner.setWaypoints({makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_target)}); + const auto route = + planner.getRouteLanelets(makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), 100.0); EXPECT_FALSE(std::find(route.begin(), route.end(), id_target) != route.end()); } @@ -141,16 +121,13 @@ TEST(RoutePlanner, getRouteLanelets_noHorizon) TEST(RoutePlanner, getRouteLanelets_empty) { auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - traffic_simulator::RoutePlanner planner(hdmap_utils_ptr); - - lanelet::Id id_0 = 120659; - auto pose_0 = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); + auto planner = traffic_simulator::RoutePlanner(hdmap_utils_ptr); - const double distance = 100; - lanelet::Ids following_ids({120659, 120660, 34468, 34465, 34462}); + const lanelet::Ids following_ids({120659, 120660, 34468, 34465, 34462}); planner.setWaypoints({}); - auto route = planner.getRouteLanelets(pose_0, distance); + const auto route = + planner.getRouteLanelets(makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), 100.0); EXPECT_EQ(route.size(), following_ids.size()); for (size_t i = 0; i < route.size(); i++) { diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp index e9ae2ac3a7b..202957c1c2b 100644 --- a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp @@ -34,19 +34,19 @@ int main(int argc, char ** argv) */ TEST(MiscObjectEntity, getCurrentAction_npcNotStarted) { - lanelet::Id id = 120659; - const double initial_speed = 0.0; - std::string entity_name("blob"); + const auto entity_name = std::string("blob"); auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto non_canonicalized_status = - makeEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); + + auto non_canonicalized_status = makeEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), 0.0, + entity_name); non_canonicalized_status.action_status.current_action = "purposelessly_existing"; - traffic_simulator::entity_status::CanonicalizedEntityStatus status( - non_canonicalized_status, hdmap_utils_ptr); - traffic_simulator_msgs::msg::MiscObjectParameters params{}; - traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); + + const auto blob = traffic_simulator::entity::MiscObjectEntity( + entity_name, + traffic_simulator::entity_status::CanonicalizedEntityStatus( + non_canonicalized_status, hdmap_utils_ptr), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}); EXPECT_FALSE(blob.isNpcLogicStarted()); EXPECT_TRUE(blob.getCurrentAction() == "waiting"); @@ -57,19 +57,19 @@ TEST(MiscObjectEntity, getCurrentAction_npcNotStarted) */ TEST(MiscObjectEntity, getCurrentAction_npcStarted) { - lanelet::Id id = 120659; - const double initial_speed = 0.0; - std::string entity_name("blob"); + const auto entity_name = std::string("blob"); auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto non_canonicalized_status = - makeEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); + + auto non_canonicalized_status = makeEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), 0.0, + entity_name); non_canonicalized_status.action_status.current_action = "purposelessly_existing"; - traffic_simulator::entity_status::CanonicalizedEntityStatus status( - non_canonicalized_status, hdmap_utils_ptr); - traffic_simulator_msgs::msg::MiscObjectParameters params{}; - traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); + + auto blob = traffic_simulator::entity::MiscObjectEntity( + entity_name, + traffic_simulator::entity_status::CanonicalizedEntityStatus( + non_canonicalized_status, hdmap_utils_ptr), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}); blob.startNpcLogic(); EXPECT_TRUE(blob.isNpcLogicStarted()); @@ -81,20 +81,17 @@ TEST(MiscObjectEntity, getCurrentAction_npcStarted) */ TEST(MiscObjectEntity, requestSpeedChange_absolute) { - lanelet::Id id = 120659; - const double initial_speed = 0.0; - std::string entity_name("blob"); + const auto entity_name = std::string("blob"); auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = - makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); - traffic_simulator_msgs::msg::MiscObjectParameters params{}; - traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); - - const double target_speed = 10.0; - const bool continuous = false; - EXPECT_THROW(blob.requestSpeedChange(target_speed, continuous), common::Error); + EXPECT_THROW( + traffic_simulator::entity::MiscObjectEntity( + entity_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), + 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .requestSpeedChange(10.0, false), + common::SemanticError); } /** @@ -102,30 +99,27 @@ TEST(MiscObjectEntity, requestSpeedChange_absolute) */ TEST(MiscObjectEntity, requestSpeedChange_relative) { - lanelet::Id id = 120659; - const double initial_speed = 0.0; - std::string entity_name("blob"); + const auto entity_name = std::string("blob"); auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659); auto bbox = makeBoundingBox(); - auto status = - makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); - traffic_simulator_msgs::msg::MiscObjectParameters params{}; - traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); - const double bob_speed = 17.0; - const double delta_speed = 3.0; - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto blob = traffic_simulator::entity::MiscObjectEntity( + entity_name, makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}); + std::unordered_map others; - others.emplace("bob_entity", bob_status); + others.emplace( + "bob_entity", makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, 17.0, "bob")); blob.setOtherStatus(others); - traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( - "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); - bool continuous = true; - - EXPECT_THROW(blob.requestSpeedChange(relative_taget_speed, continuous), common::Error); + EXPECT_THROW( + blob.requestSpeedChange( + traffic_simulator::speed_change::RelativeTargetSpeed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, 3.0), + true), + common::SemanticError); } /** @@ -134,25 +128,22 @@ TEST(MiscObjectEntity, requestSpeedChange_relative) */ TEST(MiscObjectEntity, requestSpeedChange_absoluteTransition) { - lanelet::Id id = 120659; - const double initial_speed = 0.0; - std::string entity_name("blob"); + const auto entity_name = std::string("blob"); auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = - makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); - traffic_simulator_msgs::msg::MiscObjectParameters params{}; - traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); - - const double target_speed = 10.0; - auto transition = traffic_simulator::speed_change::Transition::AUTO; - traffic_simulator::speed_change::Constraint constraint( - traffic_simulator::speed_change::Constraint::Type::NONE, 0.0); - const bool continuous = false; EXPECT_THROW( - blob.requestSpeedChange(target_speed, transition, constraint, continuous), common::Error); + traffic_simulator::entity::MiscObjectEntity( + entity_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), + 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .requestSpeedChange( + 10.0, traffic_simulator::speed_change::Transition::AUTO, + traffic_simulator::speed_change::Constraint( + traffic_simulator::speed_change::Constraint::Type::NONE, 0.0), + false), + common::SemanticError); } /** @@ -161,24 +152,19 @@ TEST(MiscObjectEntity, requestSpeedChange_absoluteTransition) */ TEST(MiscObjectEntity, requestAssignRoute_laneletPose) { - lanelet::Id id_0 = 120659; - lanelet::Id id_1 = 120660; - const double initial_speed = 0.0; - std::string entity_name("blob"); + const auto entity_name = std::string("blob"); auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); - auto bbox = makeBoundingBox(); - auto status = - makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); - traffic_simulator_msgs::msg::MiscObjectParameters params{}; - traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); - auto target_pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_1); - - std::vector target_poses; - target_poses.push_back(target_pose); - - EXPECT_THROW(blob.requestAssignRoute(target_poses), common::Error); + EXPECT_THROW( + traffic_simulator::entity::MiscObjectEntity( + entity_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), + 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .requestAssignRoute(std::vector{ + makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660)}), + common::SemanticError); } /** @@ -187,25 +173,21 @@ TEST(MiscObjectEntity, requestAssignRoute_laneletPose) */ TEST(MiscObjectEntity, requestAssignRoute_pose) { - lanelet::Id id = 120659; - const double initial_speed = 0.0; - std::string entity_name("blob"); + const auto entity_name = std::string("blob"); auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = - makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); - traffic_simulator_msgs::msg::MiscObjectParameters params{}; - traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); - geometry_msgs::msg::Pose target_pose; - target_pose.position.x = 3759.34; - target_pose.position.y = 73791.38; - - std::vector target_poses; - target_poses.push_back(target_pose); - - EXPECT_THROW(blob.requestAssignRoute(target_poses), common::Error); + EXPECT_THROW( + traffic_simulator::entity::MiscObjectEntity( + entity_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), + 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .requestAssignRoute( + std::vector{geometry_msgs::build() + .position(makePoint(3759.34, 73791.38)) + .orientation(makeQuaternionFromYaw(0.0))}), + common::SemanticError); } /** @@ -214,21 +196,18 @@ TEST(MiscObjectEntity, requestAssignRoute_pose) */ TEST(MiscObjectEntity, requestAcquirePosition_laneletPose) { - lanelet::Id id_0 = 120659; - lanelet::Id id_1 = 120660; - const double initial_speed = 0.0; - std::string entity_name("blob"); + const auto entity_name = std::string("blob"); auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); - auto bbox = makeBoundingBox(); - auto status = - makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); - traffic_simulator_msgs::msg::MiscObjectParameters params{}; - traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); - auto target_pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_1); - - EXPECT_THROW(blob.requestAcquirePosition(target_pose), common::Error); + EXPECT_THROW( + traffic_simulator::entity::MiscObjectEntity( + entity_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), + 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .requestAcquirePosition(makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660)), + common::SemanticError); } /** @@ -237,22 +216,20 @@ TEST(MiscObjectEntity, requestAcquirePosition_laneletPose) */ TEST(MiscObjectEntity, requestAcquirePosition_pose) { - lanelet::Id id = 120659; - const double initial_speed = 0.0; - std::string entity_name("blob"); + const auto entity_name = std::string("blob"); auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = - makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); - traffic_simulator_msgs::msg::MiscObjectParameters params{}; - traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); - - geometry_msgs::msg::Pose target_pose; - target_pose.position.x = 3759.34; - target_pose.position.y = 73791.38; - EXPECT_THROW(blob.requestAcquirePosition(target_pose), common::Error); + EXPECT_THROW( + traffic_simulator::entity::MiscObjectEntity( + entity_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), + 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .requestAcquirePosition(geometry_msgs::build() + .position(makePoint(3759.34, 73791.38)) + .orientation(makeQuaternionFromYaw(0.0))), + common::SemanticError); } /** @@ -260,17 +237,16 @@ TEST(MiscObjectEntity, requestAcquirePosition_pose) */ TEST(MiscObjectEntity, getRouteLanelets) { - lanelet::Id id = 120659; - const double initial_speed = 0.0; - std::string entity_name("blob"); + const auto entity_name = std::string("blob"); auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = - makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, initial_speed, entity_name); - traffic_simulator_msgs::msg::MiscObjectParameters params{}; - traffic_simulator::entity::MiscObjectEntity blob(entity_name, status, hdmap_utils_ptr, params); - const double horizon = 100.0; - EXPECT_THROW(blob.getRouteLanelets(horizon), common::Error); + EXPECT_THROW( + traffic_simulator::entity::MiscObjectEntity( + entity_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), + 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .getRouteLanelets(100.0), + common::SemanticError); } diff --git a/simulation/traffic_simulator/test/src/job/test_job.cpp b/simulation/traffic_simulator/test/src/job/test_job.cpp index f6b4c5678e0..517d9dd6221 100644 --- a/simulation/traffic_simulator/test/src/job/test_job.cpp +++ b/simulation/traffic_simulator/test/src/job/test_job.cpp @@ -31,15 +31,11 @@ int main(int argc, char ** argv) TEST(Job, onUpdate) { bool was_cleanup_func_called = false; - auto update_func = [](const double) { return true; }; - auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; - const auto type = traffic_simulator::job::Type::UNKOWN; - const auto event = traffic_simulator::job::Event::POST_UPDATE; - const bool is_exclusive = true; - - auto job = traffic_simulator::job::Job(update_func, cleanup_func, type, is_exclusive, event); - const double step_time = 0.0; - job.onUpdate(step_time); + auto job = traffic_simulator::job::Job( + [](const double) { return true; }, + [&was_cleanup_func_called]() { was_cleanup_func_called = true; }, + traffic_simulator::job::Type::UNKOWN, true, traffic_simulator::job::Event::POST_UPDATE); + job.onUpdate(0.0); EXPECT_TRUE(was_cleanup_func_called); } diff --git a/simulation/traffic_simulator/test/src/job/test_job_list.cpp b/simulation/traffic_simulator/test/src/job/test_job_list.cpp index ad9c97d8427..1b77b7dfc28 100644 --- a/simulation/traffic_simulator/test/src/job/test_job_list.cpp +++ b/simulation/traffic_simulator/test/src/job/test_job_list.cpp @@ -29,18 +29,16 @@ int main(int argc, char ** argv) TEST(JobList, append) { bool was_cleanup_func_called = false; - auto update_func = [](const double) { return true; }; - auto cleanup_func = [&was_cleanup_func_called]() { was_cleanup_func_called = true; }; - const auto type = traffic_simulator::job::Type::UNKOWN; const auto event = traffic_simulator::job::Event::POST_UPDATE; - const bool is_exclusive = true; auto job_list = traffic_simulator::job::JobList(); - job_list.append(update_func, cleanup_func, type, is_exclusive, event); - const double step_time = 0.0; + job_list.append( + [](const double) { return true; }, + [&was_cleanup_func_called]() { was_cleanup_func_called = true; }, + traffic_simulator::job::Type::UNKOWN, true, event); - job_list.update(step_time, event); + job_list.update(0.0, event); EXPECT_TRUE(was_cleanup_func_called); } @@ -57,22 +55,20 @@ TEST(JobList, append_doubled) bool second_cleanup = false; bool second_update = false; - auto first_update_func = [&first_update](const double) { return first_update = true; }; - auto first_cleanup_func = [&first_cleanup]() { first_cleanup = true; }; - auto second_update_func = [&second_update](const double) { return second_update = true; }; - auto second_cleanup_func = [&second_cleanup]() { second_cleanup = true; }; - const auto type = traffic_simulator::job::Type::UNKOWN; const auto event = traffic_simulator::job::Event::POST_UPDATE; const bool is_exclusive = true; auto job_list = traffic_simulator::job::JobList(); - job_list.append(first_update_func, first_cleanup_func, type, is_exclusive, event); - job_list.append(second_update_func, second_cleanup_func, type, is_exclusive, event); + job_list.append( + [&first_update](const double) { return first_update = true; }, + [&first_cleanup]() { first_cleanup = true; }, type, is_exclusive, event); + job_list.append( + [&second_update](const double) { return second_update = true; }, + [&second_cleanup]() { second_cleanup = true; }, type, is_exclusive, event); - const double step_time = 0.0; - job_list.update(step_time, event); + job_list.update(0.0, event); EXPECT_TRUE(first_cleanup); EXPECT_FALSE(first_update); @@ -89,19 +85,15 @@ TEST(JobList, update) int update_count = 0; int cleanup_count = 0; - auto update_func = [&update_count](const double) { - update_count++; - return update_count >= 2; - }; - auto cleanup_func = [&cleanup_count]() { cleanup_count++; }; - - const auto type = traffic_simulator::job::Type::UNKOWN; const auto event = traffic_simulator::job::Event::POST_UPDATE; - const bool is_exclusive = true; - auto job_list = traffic_simulator::job::JobList(); - job_list.append(update_func, cleanup_func, type, is_exclusive, event); + job_list.append( + [&update_count](const double) { + update_count++; + return update_count >= 2; + }, + [&cleanup_count]() { cleanup_count++; }, traffic_simulator::job::Type::UNKOWN, true, event); const double step_time = 0.0; diff --git a/simulation/traffic_simulator/test/src/job/txt.txt b/simulation/traffic_simulator/test/src/job/txt.txt new file mode 100644 index 00000000000..eedd99178c2 --- /dev/null +++ b/simulation/traffic_simulator/test/src/job/txt.txt @@ -0,0 +1,33 @@ +TrafficLight.Color_make +TrafficLight.Shape_make +TrafficLight.Status_make +TrafficLight.Bulb_make +TrafficLight.Bulb_trafficLightMessageConversion +TrafficLight.Color_make_wrong +TrafficLight.Shape_make_wrong +TrafficLight.Status_make_wrong +TrafficLight.Bulb_make_wrong +MiscObjectEntity.getCurrentAction_npcNotStarted +MiscObjectEntity.getCurrentAction_npcStarted +MiscObjectEntity.requestSpeedChange_absolute +MiscObjectEntity.requestSpeedChange_relative +MiscObjectEntity.requestSpeedChange_absoluteTransition +MiscObjectEntity.requestAssignRoute_laneletPose +MiscObjectEntity.requestAssignRoute_pose +MiscObjectEntity.requestAcquirePosition_laneletPose +MiscObjectEntity.requestAcquirePosition_pose +MiscObjectEntity.getRouteLanelets +Behavior.getRequestString +RoutePlanner.getGoalPoses +RoutePlanner.getGoalPosesInWorldFrame +RoutePlanner.getRouteLanelets_horizon +RoutePlanner.getRouteLanelets_noHorizon +RoutePlanner.getRouteLanelets_empty +Job.onUpdate +JobList.append +JobList.append_doubled +JobList.update +SimulationClock.Initialize +SimulationClock.getCurrentRosTime +SimulationClock.getCurrentScenarioTime +SimulationClock.Update \ No newline at end of file diff --git a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp index 57d0be7addf..1994230fbda 100644 --- a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp +++ b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp @@ -23,11 +23,7 @@ */ TEST(SimulationClock, Initialize) { - const double realtime_factor = 1.0; - const double frame_rate = 10.0; - const bool use_sim_time = true; - auto simulation_clock = - traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); + auto simulation_clock = traffic_simulator::SimulationClock(true, 1.0, 10.0); EXPECT_FALSE(simulation_clock.started()); simulation_clock.update(); @@ -47,9 +43,7 @@ TEST(SimulationClock, getCurrentRosTime) { const double realtime_factor = 2.0; const double frame_rate = 10.0; - const bool use_sim_time = true; - auto simulation_clock = - traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); + auto simulation_clock = traffic_simulator::SimulationClock(true, realtime_factor, frame_rate); simulation_clock.start(); const auto initial_time = simulation_clock.getCurrentRosTime(); @@ -59,12 +53,9 @@ TEST(SimulationClock, getCurrentRosTime) simulation_clock.update(); } - const auto current_time = simulation_clock.getCurrentRosTime(); - const double result_elapsed_time = (current_time - initial_time).seconds(); - const double actual_elapsed_time = static_cast(iterations) * realtime_factor / frame_rate; - const double epsilon = 1e-6; - - EXPECT_NEAR(result_elapsed_time, actual_elapsed_time, epsilon); + EXPECT_NEAR( + (simulation_clock.getCurrentRosTime() - initial_time).seconds(), + static_cast(iterations) * realtime_factor / frame_rate, 1e-6); } /** @@ -75,9 +66,7 @@ TEST(SimulationClock, getCurrentScenarioTime) { const double realtime_factor = 1.0; const double frame_rate = 30.0; - const bool use_sim_time = true; - auto simulation_clock = - traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); + auto simulation_clock = traffic_simulator::SimulationClock(true, realtime_factor, frame_rate); simulation_clock.start(); @@ -88,11 +77,9 @@ TEST(SimulationClock, getCurrentScenarioTime) simulation_clock.update(); } - const double result_elapsed_time = simulation_clock.getCurrentScenarioTime(); - const double actual_elapsed_time = static_cast(iterations) * realtime_factor / frame_rate; - const double epsilon = 1e-6; - - EXPECT_NEAR(actual_elapsed_time, result_elapsed_time, epsilon); + EXPECT_NEAR( + static_cast(iterations) * realtime_factor / frame_rate, + simulation_clock.getCurrentScenarioTime(), 1e-6); } /** @@ -101,25 +88,18 @@ TEST(SimulationClock, getCurrentScenarioTime) */ TEST(SimulationClock, Update) { - const double realtime_factor = 1.0; - const double frame_rate = 10.0; - const bool use_sim_time = true; - auto simulation_clock = - traffic_simulator::SimulationClock(use_sim_time, realtime_factor, frame_rate); + auto simulation_clock = traffic_simulator::SimulationClock(true, 1.0, 10.0); simulation_clock.start(); const double initial_simulation_time = simulation_clock.getCurrentSimulationTime(); - - const int iterations = 10; const double step_time = simulation_clock.getStepTime(); - const double tolerance = 1e-6; - for (int i = 0; i < iterations; ++i) { + for (int i = 0; i < 10; ++i) { simulation_clock.update(); const double expected_simulation_time = initial_simulation_time + static_cast(i + 1) * step_time; const double actual_simulation_time = simulation_clock.getCurrentSimulationTime(); - EXPECT_NEAR(actual_simulation_time, expected_simulation_time, tolerance); + EXPECT_NEAR(actual_simulation_time, expected_simulation_time, 1e-6); } } From 4753237698d50702f83ebf8fa2e3a51d05f43297 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 6 Jun 2024 15:49:59 +0200 Subject: [PATCH 046/118] use test fixture --- .../test/src/behavior/test_route_planner.cpp | 34 +++++------ .../src/entity/test_misc_object_entity.cpp | 56 ++++++------------- 2 files changed, 32 insertions(+), 58 deletions(-) diff --git a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp index 446f56e6c7e..ccbc28ff863 100644 --- a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp +++ b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp @@ -26,15 +26,21 @@ int main(int argc, char ** argv) return RUN_ALL_TESTS(); } +class RoutePlannerTest : public testing::Test +{ +protected: + RoutePlannerTest() : hdmap_utils_ptr(makeHdMapUtilsSharedPointer()), planner(hdmap_utils_ptr) {} + + std::shared_ptr hdmap_utils_ptr; + traffic_simulator::RoutePlanner planner; +}; + /** * @note Test functionality used by other units. * Test accessor getGoalPoses. */ -TEST(RoutePlanner, getGoalPoses) +TEST_F(RoutePlannerTest, getGoalPoses) { - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto planner = traffic_simulator::RoutePlanner(hdmap_utils_ptr); - const auto in_poses = std::vector{ makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660), @@ -57,11 +63,8 @@ TEST(RoutePlanner, getGoalPoses) * @note Test functionality used by other units. * Test performing transformations by getGoalPosesInWorldFrame accessor. */ -TEST(RoutePlanner, getGoalPosesInWorldFrame) +TEST_F(RoutePlannerTest, getGoalPosesInWorldFrame) { - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto planner = traffic_simulator::RoutePlanner(hdmap_utils_ptr); - const auto in_poses = std::vector{ makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660), @@ -81,11 +84,8 @@ TEST(RoutePlanner, getGoalPosesInWorldFrame) * @note Test functionality used by other units. * Test routing correctness with an entity pose and target pose spaced apart for more than horizon. */ -TEST(RoutePlanner, getRouteLanelets_horizon) +TEST_F(RoutePlannerTest, getRouteLanelets_horizon) { - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto planner = traffic_simulator::RoutePlanner(hdmap_utils_ptr); - const lanelet::Id id_target = 34579; planner.setWaypoints({makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_target)}); @@ -99,11 +99,8 @@ TEST(RoutePlanner, getRouteLanelets_horizon) * @note Test functionality used by other units. * Test routing correctness with an entity pose and target pose spaced apart for less than horizon. */ -TEST(RoutePlanner, getRouteLanelets_noHorizon) +TEST_F(RoutePlannerTest, getRouteLanelets_noHorizon) { - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto planner = traffic_simulator::RoutePlanner(hdmap_utils_ptr); - lanelet::Id id_target = 34579; planner.setWaypoints({makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_target)}); @@ -118,11 +115,8 @@ TEST(RoutePlanner, getRouteLanelets_noHorizon) * Test routing correctness with an entity pose and empty waypoints vector * - the goal is to test function behavior when empty vector is passed. */ -TEST(RoutePlanner, getRouteLanelets_empty) +TEST_F(RoutePlannerTest, getRouteLanelets_empty) { - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto planner = traffic_simulator::RoutePlanner(hdmap_utils_ptr); - const lanelet::Ids following_ids({120659, 120660, 34468, 34465, 34462}); planner.setWaypoints({}); diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp index 202957c1c2b..fec7c04330d 100644 --- a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp @@ -29,14 +29,19 @@ int main(int argc, char ** argv) return RUN_ALL_TESTS(); } +class MiscObjectEntityTest : public testing::Test +{ +protected: + MiscObjectEntityTest() : hdmap_utils_ptr(makeHdMapUtilsSharedPointer()), entity_name("blob") {} + + std::shared_ptr hdmap_utils_ptr; + const std::string entity_name; +}; /** * @note Test basic functionality. Test current action obtaining when NPC logic is not started. */ -TEST(MiscObjectEntity, getCurrentAction_npcNotStarted) +TEST_F(MiscObjectEntityTest, getCurrentAction_npcNotStarted) { - const auto entity_name = std::string("blob"); - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto non_canonicalized_status = makeEntityStatus( hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), 0.0, entity_name); @@ -55,11 +60,8 @@ TEST(MiscObjectEntity, getCurrentAction_npcNotStarted) /** * @note Test basic functionality. Test current action obtaining when NPC logic is started. */ -TEST(MiscObjectEntity, getCurrentAction_npcStarted) +TEST_F(MiscObjectEntityTest, getCurrentAction_npcStarted) { - const auto entity_name = std::string("blob"); - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto non_canonicalized_status = makeEntityStatus( hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), 0.0, entity_name); @@ -79,10 +81,8 @@ TEST(MiscObjectEntity, getCurrentAction_npcStarted) /** * @note Test function behavior when absolute speed change is requested - the goal is to test throwing error. */ -TEST(MiscObjectEntity, requestSpeedChange_absolute) +TEST_F(MiscObjectEntityTest, requestSpeedChange_absolute) { - const auto entity_name = std::string("blob"); - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); EXPECT_THROW( traffic_simulator::entity::MiscObjectEntity( entity_name, @@ -97,10 +97,8 @@ TEST(MiscObjectEntity, requestSpeedChange_absolute) /** * @note Test function behavior when relative speed change is requested - the goal is to test throwing error. */ -TEST(MiscObjectEntity, requestSpeedChange_relative) +TEST_F(MiscObjectEntityTest, requestSpeedChange_relative) { - const auto entity_name = std::string("blob"); - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659); auto bbox = makeBoundingBox(); @@ -126,11 +124,8 @@ TEST(MiscObjectEntity, requestSpeedChange_relative) * @note Test function behavior when relative speed change with transition type is requested * - the goal is to test throwing error. */ -TEST(MiscObjectEntity, requestSpeedChange_absoluteTransition) +TEST_F(MiscObjectEntityTest, requestSpeedChange_absoluteTransition) { - const auto entity_name = std::string("blob"); - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - EXPECT_THROW( traffic_simulator::entity::MiscObjectEntity( entity_name, @@ -150,11 +145,8 @@ TEST(MiscObjectEntity, requestSpeedChange_absoluteTransition) * @note Test function behavior when route assigning is requested with lanelet pose * - the goal is to test throwing error. */ -TEST(MiscObjectEntity, requestAssignRoute_laneletPose) +TEST_F(MiscObjectEntityTest, requestAssignRoute_laneletPose) { - const auto entity_name = std::string("blob"); - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - EXPECT_THROW( traffic_simulator::entity::MiscObjectEntity( entity_name, @@ -171,11 +163,8 @@ TEST(MiscObjectEntity, requestAssignRoute_laneletPose) * @note Test function behavior when route assigning is requested with pose * - the goal is to test throwing error. */ -TEST(MiscObjectEntity, requestAssignRoute_pose) +TEST_F(MiscObjectEntityTest, requestAssignRoute_pose) { - const auto entity_name = std::string("blob"); - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - EXPECT_THROW( traffic_simulator::entity::MiscObjectEntity( entity_name, @@ -194,11 +183,8 @@ TEST(MiscObjectEntity, requestAssignRoute_pose) * @note Test function behavior when position acquiring is requested with lanelet pose * - the goal is to test throwing error. */ -TEST(MiscObjectEntity, requestAcquirePosition_laneletPose) +TEST_F(MiscObjectEntityTest, requestAcquirePosition_laneletPose) { - const auto entity_name = std::string("blob"); - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - EXPECT_THROW( traffic_simulator::entity::MiscObjectEntity( entity_name, @@ -214,11 +200,8 @@ TEST(MiscObjectEntity, requestAcquirePosition_laneletPose) * @note Test function behavior when position acquiring is requested with pose * - the goal is to test throwing error. */ -TEST(MiscObjectEntity, requestAcquirePosition_pose) +TEST_F(MiscObjectEntityTest, requestAcquirePosition_pose) { - const auto entity_name = std::string("blob"); - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - EXPECT_THROW( traffic_simulator::entity::MiscObjectEntity( entity_name, @@ -235,11 +218,8 @@ TEST(MiscObjectEntity, requestAcquirePosition_pose) /** * @note Test function behavior when called with any argument - the goal is to test error throwing. */ -TEST(MiscObjectEntity, getRouteLanelets) +TEST_F(MiscObjectEntityTest, getRouteLanelets) { - const auto entity_name = std::string("blob"); - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - EXPECT_THROW( traffic_simulator::entity::MiscObjectEntity( entity_name, From 63f9e5014c621c29e60a55783736dda02d2dca2f Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 6 Jun 2024 15:59:48 +0200 Subject: [PATCH 047/118] rename literal --- .../test/src/entity/test_misc_object_entity.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp index fec7c04330d..964d0dfdf76 100644 --- a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp @@ -45,7 +45,7 @@ TEST_F(MiscObjectEntityTest, getCurrentAction_npcNotStarted) auto non_canonicalized_status = makeEntityStatus( hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), 0.0, entity_name); - non_canonicalized_status.action_status.current_action = "purposelessly_existing"; + non_canonicalized_status.action_status.current_action = "current_action_name"; const auto blob = traffic_simulator::entity::MiscObjectEntity( entity_name, @@ -65,7 +65,7 @@ TEST_F(MiscObjectEntityTest, getCurrentAction_npcStarted) auto non_canonicalized_status = makeEntityStatus( hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), 0.0, entity_name); - non_canonicalized_status.action_status.current_action = "purposelessly_existing"; + non_canonicalized_status.action_status.current_action = "current_action_name"; auto blob = traffic_simulator::entity::MiscObjectEntity( entity_name, @@ -75,7 +75,7 @@ TEST_F(MiscObjectEntityTest, getCurrentAction_npcStarted) blob.startNpcLogic(); EXPECT_TRUE(blob.isNpcLogicStarted()); - EXPECT_TRUE(blob.getCurrentAction() == "purposelessly_existing"); + EXPECT_TRUE(blob.getCurrentAction() == "current_action_name"); } /** From ff8e0d46774acf6e3d0f7f4c678eefef0caec8ab Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 6 Jun 2024 16:00:43 +0200 Subject: [PATCH 048/118] use test fixtures --- simulation/traffic_simulator/test/src/job/test_job.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/simulation/traffic_simulator/test/src/job/test_job.cpp b/simulation/traffic_simulator/test/src/job/test_job.cpp index 517d9dd6221..cccdb367e70 100644 --- a/simulation/traffic_simulator/test/src/job/test_job.cpp +++ b/simulation/traffic_simulator/test/src/job/test_job.cpp @@ -31,6 +31,7 @@ int main(int argc, char ** argv) TEST(Job, onUpdate) { bool was_cleanup_func_called = false; + auto job = traffic_simulator::job::Job( [](const double) { return true; }, [&was_cleanup_func_called]() { was_cleanup_func_called = true; }, From 33ff1728f9121bf4d100539985e35cfbb0d50429 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 6 Jun 2024 16:00:23 +0200 Subject: [PATCH 049/118] ref(ParameterManager): rename to NodeParameterHandler, improve --- .../src/cpp_scenario_node.cpp | 2 +- .../simulator_core.hpp | 6 +- .../include/traffic_simulator/api/api.hpp | 16 +++--- .../traffic_simulator/entity/ego_entity.hpp | 7 ++- .../entity/entity_manager.hpp | 16 +++--- .../utils/node_parameter_handler.hpp | 57 +++++++++++++++++++ .../utils/parameter_manager.hpp | 51 ----------------- simulation/traffic_simulator/src/api/api.cpp | 6 +- .../src/entity/ego_entity.cpp | 27 +++++---- 9 files changed, 101 insertions(+), 87 deletions(-) create mode 100644 simulation/traffic_simulator/include/traffic_simulator/utils/node_parameter_handler.hpp delete mode 100644 simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index b339e781252..4817ef28752 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -101,7 +101,7 @@ void CppScenarioNode::spawnEgoEntity( api_.attachOccupancyGridSensor([this] { simulation_api_schema::OccupancyGridSensorConfiguration configuration; // clang-format off - configuration.set_architecture_type(api_.getParameter("architecture_type", "awf/universe")); + configuration.set_architecture_type(api_.getParameterOrDeclare("architecture_type", "awf/universe")); configuration.set_entity("ego"); configuration.set_filter_by_range(true); configuration.set_height(200); diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 760a54caba9..42f3b828c96 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -338,7 +338,7 @@ class SimulatorCore core->attachDetectionSensor([&]() { simulation_api_schema::DetectionSensorConfiguration configuration; // clang-format off - configuration.set_architecture_type(core->getParameter("architecture_type", "awf/universe")); + configuration.set_architecture_type(core->getParameterOrDeclare("architecture_type", "awf/universe")); configuration.set_entity(entity_ref); configuration.set_detect_all_objects_in_range(controller.properties.template get("isClairvoyant")); configuration.set_object_recognition_delay(controller.properties.template get("detectedObjectPublishingDelay")); @@ -355,7 +355,7 @@ class SimulatorCore core->attachOccupancyGridSensor([&]() { simulation_api_schema::OccupancyGridSensorConfiguration configuration; // clang-format off - configuration.set_architecture_type(core->getParameter("architecture_type", "awf/universe")); + configuration.set_architecture_type(core->getParameterOrDeclare("architecture_type", "awf/universe")); configuration.set_entity(entity_ref); configuration.set_filter_by_range(controller.properties.template get("isClairvoyant")); configuration.set_height(200); @@ -370,7 +370,7 @@ class SimulatorCore core->attachPseudoTrafficLightDetector([&]() { simulation_api_schema::PseudoTrafficLightDetectorConfiguration configuration; configuration.set_architecture_type( - core->getParameter("architecture_type", "awf/universe")); + core->getParameterOrDeclare("architecture_type", "awf/universe")); return configuration; }()); diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 911410c241b..22f37c8fd2a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -65,9 +65,9 @@ class API template , typename... Ts> explicit API(NodeT && node, const Configuration & configuration, Ts &&... xs) : configuration(configuration), - parameter_manager_(node), + node_parameter_handler_(std::make_shared(node)), entity_manager_ptr_( - std::make_shared(node, configuration, parameter_manager_)), + std::make_shared(node, configuration, node_parameter_handler_)), traffic_controller_ptr_(std::make_shared( entity_manager_ptr_->getHdmapUtils(), [this]() { return API::getEntityNames(); }, [this](const auto & name) { return API::getMapPose(name); }, @@ -133,7 +133,7 @@ class API if (behavior == VehicleBehavior::autoware()) { return entity_manager_ptr_->entityExists(name) or entity_manager_ptr_->spawnEntity( - name, pose, parameters, configuration, parameter_manager_); + name, pose, parameters, configuration, node_parameter_handler_); } else { return entity_manager_ptr_->spawnEntity( name, pose, parameters, behavior); @@ -388,11 +388,11 @@ class API public: #undef FORWARD_TO_ENTITY_MANAGER - template - decltype(auto) getParameter(const std::string & name, const ParameterT & default_value = {}) const + template + auto getParameterOrDeclare( + const std::string & name, const ParameterType & default_value = {}) const -> decltype(auto) { - return parameter_manager_.getParameter( - std::forward(name), std::forward(default_value)); + return node_parameter_handler_->getParameterOrDeclare(name, default_value); } auto canonicalize(const LaneletPose & maybe_non_canonicalized_lanelet_pose) const @@ -412,7 +412,7 @@ class API const Configuration configuration; - const ParameterManager parameter_manager_; + const std::shared_ptr node_parameter_handler_; const std::shared_ptr entity_manager_ptr_; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index 4091812d52e..1f5998c2ea8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include #include @@ -37,7 +37,8 @@ class EgoEntity : public VehicleEntity { const std::unique_ptr field_operator_application; - static auto makeFieldOperatorApplication(const Configuration &, const ParameterManager &) + static auto makeFieldOperatorApplication( + const Configuration &, const std::shared_ptr &) -> std::unique_ptr; bool is_controlled_by_simulator_{false}; @@ -52,7 +53,7 @@ class EgoEntity : public VehicleEntity const std::string & name, const CanonicalizedEntityStatus &, const std::shared_ptr &, const traffic_simulator_msgs::msg::VehicleParameters &, const Configuration &, - const ParameterManager &); + const std::shared_ptr &); explicit EgoEntity(EgoEntity &&) = delete; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 06184896cbd..72148d00918 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include #include @@ -74,7 +74,7 @@ class EntityManager Configuration configuration; std::shared_ptr node_topics_interface; - const ParameterManager parameter_manager_; + const std::shared_ptr node_parameter_handler_; tf2_ros::StaticTransformBroadcaster broadcaster_; tf2_ros::TransformBroadcaster base_link_broadcaster_; @@ -132,8 +132,8 @@ class EntityManager template auto makeV2ITrafficLightPublisher(Ts &&... xs) -> std::shared_ptr { - if (const auto architecture_type = - parameter_manager_.getParameter("architecture_type", "awf/universe"); + if (const auto architecture_type = node_parameter_handler_->getParameterOrDeclare( + "architecture_type", "awf/universe"); architecture_type.find("awf/universe") != std::string::npos) { return std::make_shared< TrafficLightPublisher>( @@ -147,10 +147,11 @@ class EntityManager template > explicit EntityManager( - NodeT && node, const Configuration & configuration, const ParameterManager & parameter_manager) + NodeT && node, const Configuration & configuration, + const std::shared_ptr & node_parameter_handler) : configuration(configuration), node_topics_interface(rclcpp::node_interfaces::get_node_topics_interface(node)), - parameter_manager_(parameter_manager), + node_parameter_handler_(node_parameter_handler), broadcaster_(node), base_link_broadcaster_(node), clock_ptr_(node->get_clock()), @@ -498,7 +499,8 @@ class EntityManager entity_status.lanelet_pose = *lanelet_pose; entity_status.lanelet_pose_valid = true; /// @note fix z, roll and pitch to fitting to the lanelet - if (parameter_manager_.getParameter("consider_pose_by_road_slope", false)) { + if (node_parameter_handler_->getParameterOrDeclare( + "consider_pose_by_road_slope", false)) { entity_status.pose = hdmap_utils_ptr_->toMapPose(*lanelet_pose).pose; } else { entity_status.pose = pose; diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameter_handler.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameter_handler.hpp new file mode 100644 index 00000000000..e8c0937cefc --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameter_handler.hpp @@ -0,0 +1,57 @@ +// Copyright 2024 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 TRAFFIC_SIMULATOR__UTILS__NODE_PARAMETER_HANDLER_HPP_ +#define TRAFFIC_SIMULATOR__UTILS__NODE_PARAMETER_HANDLER_HPP_ + +#include +#include + +namespace traffic_simulator +{ +class NodeParameterHandler +{ +public: + template + explicit NodeParameterHandler(NodeType && node) + : node_parameters_interface_( + rclcpp::node_interfaces::get_node_parameters_interface(std::forward(node))) + { + } + + template + auto getParameterOrDeclare( + const std::string & name, const ParameterType & default_value = {}) const -> ParameterType + { + if (not node_parameters_interface_->has_parameter(name)) { + node_parameters_interface_->declare_parameter(name, rclcpp::ParameterValue(default_value)); + } + return node_parameters_interface_->get_parameter(name).get_value(); + } + + template + auto getParameter(const std::string & name) const -> ParameterType + { + if (not node_parameters_interface_->has_parameter(name)) { + THROW_SEMANTIC_ERROR("Parameter ", std::quoted(name), " does not exist"); + } + return node_parameters_interface_->get_parameter(name).get_value(); + } + +private: + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface_; +}; +} // namespace traffic_simulator + +#endif //TRAFFIC_SIMULATOR__UTILS__NODE_PARAMETER_HANDLER_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp deleted file mode 100644 index 5cae116fb42..00000000000 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/parameter_manager.hpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2024 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 TRAFFIC_SIMULATOR__UTILS__PARAMETER_MANAGER_HPP_ -#define TRAFFIC_SIMULATOR__UTILS__PARAMETER_MANAGER_HPP_ - -#include - -namespace traffic_simulator -{ - -class ParameterManager -{ -public: - template - explicit ParameterManager(NodeT && node) - : node_parameters_interface_(rclcpp::node_interfaces::get_node_parameters_interface(node)) - { - } - - ParameterManager() = delete; - - ParameterManager(const ParameterManager &) = default; - - template - auto getParameter(const std::string & name, const ParameterT & default_value = {}) const - { - if (not node_parameters_interface_->has_parameter(name)) { - node_parameters_interface_->declare_parameter(name, rclcpp::ParameterValue(default_value)); - } - return node_parameters_interface_->get_parameter(name).get_value(); - } - -private: - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface_; -}; - -} // namespace traffic_simulator - -#endif //TRAFFIC_SIMULATOR__UTILS__PARAMETER_MANAGER_HPP_ diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index d62b57e6ede..a6cbb859fd3 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -217,7 +217,7 @@ bool API::attachDetectionSensor( double object_recognition_delay) { return attachDetectionSensor(helper::constructDetectionSensorConfiguration( - entity_name, getParameter("architecture_type", "awf/universe"), 0.1, + entity_name, getParameterOrDeclare("architecture_type", "awf/universe"), 0.1, detection_sensor_range, detect_all_objects_in_range, pos_noise_stddev, random_seed, probability_of_lost, object_recognition_delay)); } @@ -250,8 +250,8 @@ bool API::attachLidarSensor( const helper::LidarType lidar_type) { return attachLidarSensor(helper::constructLidarConfiguration( - lidar_type, entity_name, getParameter("architecture_type", "awf/universe"), - lidar_sensor_delay)); + lidar_type, entity_name, + getParameterOrDeclare("architecture_type", "awf/universe"), lidar_sensor_delay)); } bool API::updateTimeInSim() diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 1f350298c95..b9adbac4b89 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -35,23 +35,27 @@ namespace traffic_simulator namespace entity { auto EgoEntity::makeFieldOperatorApplication( - const Configuration & configuration, const ParameterManager & parameter_manager) + const Configuration & configuration, + const std::shared_ptr & node_parameter_handler) -> std::unique_ptr { - if (const auto architecture_type = - parameter_manager.getParameter("architecture_type", "awf/universe"); + if (const auto architecture_type = node_parameter_handler->getParameterOrDeclare( + "architecture_type", "awf/universe"); architecture_type.find("awf/universe") != std::string::npos) { - std::string rviz_config = parameter_manager.getParameter("rviz_config", ""); - return parameter_manager.getParameter("launch_autoware", true) + std::string rviz_config = + node_parameter_handler->getParameterOrDeclare("rviz_config", ""); + return node_parameter_handler->getParameterOrDeclare("launch_autoware", true) ? std::make_unique< concealer::FieldOperatorApplicationFor>( - parameter_manager.getParameter("autoware_launch_package"), - parameter_manager.getParameter("autoware_launch_file"), + node_parameter_handler->getParameter("autoware_launch_package"), + node_parameter_handler->getParameter("autoware_launch_file"), "map_path:=" + configuration.map_path.string(), "lanelet2_map_file:=" + configuration.getLanelet2MapFile(), "pointcloud_map_file:=" + configuration.getPointCloudMapFile(), - "sensor_model:=" + parameter_manager.getParameter("sensor_model"), - "vehicle_model:=" + parameter_manager.getParameter("vehicle_model"), + "sensor_model:=" + + node_parameter_handler->getParameter("sensor_model"), + "vehicle_model:=" + + node_parameter_handler->getParameter("vehicle_model"), "rviz_config:=" + ((rviz_config == "") ? configuration.rviz_config_path.string() : Configuration::Pathname(rviz_config).string()), @@ -70,9 +74,10 @@ EgoEntity::EgoEntity( const std::string & name, const CanonicalizedEntityStatus & entity_status, const std::shared_ptr & hdmap_utils_ptr, const traffic_simulator_msgs::msg::VehicleParameters & parameters, - const Configuration & configuration, const ParameterManager & parameter_manager) + const Configuration & configuration, + const std::shared_ptr & node_parameter_handler) : VehicleEntity(name, entity_status, hdmap_utils_ptr, parameters), - field_operator_application(makeFieldOperatorApplication(configuration, parameter_manager)) + field_operator_application(makeFieldOperatorApplication(configuration, node_parameter_handler)) { } From e52e0c2fa13ea5d8e9e977901181d5e0beaa0535 Mon Sep 17 00:00:00 2001 From: robomic Date: Fri, 7 Jun 2024 11:41:27 +0200 Subject: [PATCH 050/118] remove test list file --- .../traffic_simulator/test/src/job/txt.txt | 33 ------------------- 1 file changed, 33 deletions(-) delete mode 100644 simulation/traffic_simulator/test/src/job/txt.txt diff --git a/simulation/traffic_simulator/test/src/job/txt.txt b/simulation/traffic_simulator/test/src/job/txt.txt deleted file mode 100644 index eedd99178c2..00000000000 --- a/simulation/traffic_simulator/test/src/job/txt.txt +++ /dev/null @@ -1,33 +0,0 @@ -TrafficLight.Color_make -TrafficLight.Shape_make -TrafficLight.Status_make -TrafficLight.Bulb_make -TrafficLight.Bulb_trafficLightMessageConversion -TrafficLight.Color_make_wrong -TrafficLight.Shape_make_wrong -TrafficLight.Status_make_wrong -TrafficLight.Bulb_make_wrong -MiscObjectEntity.getCurrentAction_npcNotStarted -MiscObjectEntity.getCurrentAction_npcStarted -MiscObjectEntity.requestSpeedChange_absolute -MiscObjectEntity.requestSpeedChange_relative -MiscObjectEntity.requestSpeedChange_absoluteTransition -MiscObjectEntity.requestAssignRoute_laneletPose -MiscObjectEntity.requestAssignRoute_pose -MiscObjectEntity.requestAcquirePosition_laneletPose -MiscObjectEntity.requestAcquirePosition_pose -MiscObjectEntity.getRouteLanelets -Behavior.getRequestString -RoutePlanner.getGoalPoses -RoutePlanner.getGoalPosesInWorldFrame -RoutePlanner.getRouteLanelets_horizon -RoutePlanner.getRouteLanelets_noHorizon -RoutePlanner.getRouteLanelets_empty -Job.onUpdate -JobList.append -JobList.append_doubled -JobList.update -SimulationClock.Initialize -SimulationClock.getCurrentRosTime -SimulationClock.getCurrentScenarioTime -SimulationClock.Update \ No newline at end of file From 8a2f3333096b5a43698c877ab7471e7429f123b1 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 11 Jun 2024 11:15:30 +0200 Subject: [PATCH 051/118] remove misc tests --- .../test/src/entity/CMakeLists.txt | 3 - .../src/entity/test_misc_object_entity.cpp | 232 ------------------ .../test/src/entity_helper_functions.hpp | 133 ---------- 3 files changed, 368 deletions(-) delete mode 100644 simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp delete mode 100644 simulation/traffic_simulator/test/src/entity_helper_functions.hpp diff --git a/simulation/traffic_simulator/test/src/entity/CMakeLists.txt b/simulation/traffic_simulator/test/src/entity/CMakeLists.txt index 0dcb3f4ca29..0e6b9eee47d 100644 --- a/simulation/traffic_simulator/test/src/entity/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/entity/CMakeLists.txt @@ -1,5 +1,2 @@ ament_add_gtest(test_vehicle_entity test_vehicle_entity.cpp) target_link_libraries(test_vehicle_entity traffic_simulator) - -ament_add_gtest(test_misc_object_entity test_misc_object_entity.cpp) -target_link_libraries(test_misc_object_entity traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp deleted file mode 100644 index 964d0dfdf76..00000000000 --- a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp +++ /dev/null @@ -1,232 +0,0 @@ -// 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 - -#include -#include -#include -#include - -#include "../catalogs.hpp" -#include "../entity_helper_functions.hpp" -#include "../expect_eq_macros.hpp" - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -class MiscObjectEntityTest : public testing::Test -{ -protected: - MiscObjectEntityTest() : hdmap_utils_ptr(makeHdMapUtilsSharedPointer()), entity_name("blob") {} - - std::shared_ptr hdmap_utils_ptr; - const std::string entity_name; -}; -/** - * @note Test basic functionality. Test current action obtaining when NPC logic is not started. - */ -TEST_F(MiscObjectEntityTest, getCurrentAction_npcNotStarted) -{ - auto non_canonicalized_status = makeEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), 0.0, - entity_name); - non_canonicalized_status.action_status.current_action = "current_action_name"; - - const auto blob = traffic_simulator::entity::MiscObjectEntity( - entity_name, - traffic_simulator::entity_status::CanonicalizedEntityStatus( - non_canonicalized_status, hdmap_utils_ptr), - hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}); - - EXPECT_FALSE(blob.isNpcLogicStarted()); - EXPECT_TRUE(blob.getCurrentAction() == "waiting"); -} - -/** - * @note Test basic functionality. Test current action obtaining when NPC logic is started. - */ -TEST_F(MiscObjectEntityTest, getCurrentAction_npcStarted) -{ - auto non_canonicalized_status = makeEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), 0.0, - entity_name); - non_canonicalized_status.action_status.current_action = "current_action_name"; - - auto blob = traffic_simulator::entity::MiscObjectEntity( - entity_name, - traffic_simulator::entity_status::CanonicalizedEntityStatus( - non_canonicalized_status, hdmap_utils_ptr), - hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}); - - blob.startNpcLogic(); - EXPECT_TRUE(blob.isNpcLogicStarted()); - EXPECT_TRUE(blob.getCurrentAction() == "current_action_name"); -} - -/** - * @note Test function behavior when absolute speed change is requested - the goal is to test throwing error. - */ -TEST_F(MiscObjectEntityTest, requestSpeedChange_absolute) -{ - EXPECT_THROW( - traffic_simulator::entity::MiscObjectEntity( - entity_name, - makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name), - hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) - .requestSpeedChange(10.0, false), - common::SemanticError); -} - -/** - * @note Test function behavior when relative speed change is requested - the goal is to test throwing error. - */ -TEST_F(MiscObjectEntityTest, requestSpeedChange_relative) -{ - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659); - auto bbox = makeBoundingBox(); - - auto blob = traffic_simulator::entity::MiscObjectEntity( - entity_name, makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, 0.0, entity_name), - hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}); - - std::unordered_map - others; - others.emplace( - "bob_entity", makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, 17.0, "bob")); - blob.setOtherStatus(others); - - EXPECT_THROW( - blob.requestSpeedChange( - traffic_simulator::speed_change::RelativeTargetSpeed( - "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, 3.0), - true), - common::SemanticError); -} - -/** - * @note Test function behavior when relative speed change with transition type is requested - * - the goal is to test throwing error. - */ -TEST_F(MiscObjectEntityTest, requestSpeedChange_absoluteTransition) -{ - EXPECT_THROW( - traffic_simulator::entity::MiscObjectEntity( - entity_name, - makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name), - hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) - .requestSpeedChange( - 10.0, traffic_simulator::speed_change::Transition::AUTO, - traffic_simulator::speed_change::Constraint( - traffic_simulator::speed_change::Constraint::Type::NONE, 0.0), - false), - common::SemanticError); -} - -/** - * @note Test function behavior when route assigning is requested with lanelet pose - * - the goal is to test throwing error. - */ -TEST_F(MiscObjectEntityTest, requestAssignRoute_laneletPose) -{ - EXPECT_THROW( - traffic_simulator::entity::MiscObjectEntity( - entity_name, - makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name), - hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) - .requestAssignRoute(std::vector{ - makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660)}), - common::SemanticError); -} - -/** - * @note Test function behavior when route assigning is requested with pose - * - the goal is to test throwing error. - */ -TEST_F(MiscObjectEntityTest, requestAssignRoute_pose) -{ - EXPECT_THROW( - traffic_simulator::entity::MiscObjectEntity( - entity_name, - makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name), - hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) - .requestAssignRoute( - std::vector{geometry_msgs::build() - .position(makePoint(3759.34, 73791.38)) - .orientation(makeQuaternionFromYaw(0.0))}), - common::SemanticError); -} - -/** - * @note Test function behavior when position acquiring is requested with lanelet pose - * - the goal is to test throwing error. - */ -TEST_F(MiscObjectEntityTest, requestAcquirePosition_laneletPose) -{ - EXPECT_THROW( - traffic_simulator::entity::MiscObjectEntity( - entity_name, - makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name), - hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) - .requestAcquirePosition(makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660)), - common::SemanticError); -} - -/** - * @note Test function behavior when position acquiring is requested with pose - * - the goal is to test throwing error. - */ -TEST_F(MiscObjectEntityTest, requestAcquirePosition_pose) -{ - EXPECT_THROW( - traffic_simulator::entity::MiscObjectEntity( - entity_name, - makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name), - hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) - .requestAcquirePosition(geometry_msgs::build() - .position(makePoint(3759.34, 73791.38)) - .orientation(makeQuaternionFromYaw(0.0))), - common::SemanticError); -} - -/** - * @note Test function behavior when called with any argument - the goal is to test error throwing. - */ -TEST_F(MiscObjectEntityTest, getRouteLanelets) -{ - EXPECT_THROW( - traffic_simulator::entity::MiscObjectEntity( - entity_name, - makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name), - hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) - .getRouteLanelets(100.0), - common::SemanticError); -} diff --git a/simulation/traffic_simulator/test/src/entity_helper_functions.hpp b/simulation/traffic_simulator/test/src/entity_helper_functions.hpp deleted file mode 100644 index de50a1dbb12..00000000000 --- a/simulation/traffic_simulator/test/src/entity_helper_functions.hpp +++ /dev/null @@ -1,133 +0,0 @@ -// 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 TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ -#define TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ - -#include -#include -#include -#include - -#include "catalogs.hpp" -#include "expect_eq_macros.hpp" - -auto makeHdMapUtilsSharedPointer() -> std::shared_ptr -{ - std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; - geographic_msgs::msg::GeoPoint origin; - origin.latitude = 35.9037067912303; - origin.longitude = 139.9337945139059; - return std::make_shared(path, origin); -} - -auto makeCanonicalizedLaneletPose( - std::shared_ptr hdmap_utils, const lanelet::Id id = 120659, - const double s = 0.0, const double offset = 0.0) - -> traffic_simulator::lanelet_pose::CanonicalizedLaneletPose -{ - return traffic_simulator::lanelet_pose::CanonicalizedLaneletPose( - traffic_simulator::helper::constructLaneletPose(id, s, offset), hdmap_utils); -} - -auto makeBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs::msg::BoundingBox -{ - traffic_simulator_msgs::msg::BoundingBox bbox; - bbox.center.x = 1.0; - bbox.center.y = center_y; - bbox.dimensions.x = 4.0; - bbox.dimensions.y = 2.0; - bbox.dimensions.z = 1.5; - return bbox; -} - -auto makeEntityStatus( - std::shared_ptr hdmap_utils, - traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, - traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, - const std::string name = "dummy_entity") -> traffic_simulator::EntityStatus -{ - traffic_simulator::EntityStatus entity_status; - entity_status.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; - entity_status.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; - entity_status.time = 0.0; - entity_status.name = name; - entity_status.bounding_box = bbox; - geometry_msgs::msg::Twist twist; - entity_status.action_status = - traffic_simulator::helper::constructActionStatus(speed, 0.0, 0.0, 0.0); - entity_status.lanelet_pose_valid = true; - entity_status.lanelet_pose = static_cast(pose); - entity_status.pose = hdmap_utils->toMapPose(entity_status.lanelet_pose).pose; - return entity_status; -} - -auto makeEntityStatus( - std::shared_ptr /* hdmap_utils */, geometry_msgs::msg::Pose pose, - traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, - const std::string name = "dummy_entity") -> traffic_simulator::EntityStatus -{ - traffic_simulator::EntityStatus entity_status; - entity_status.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; - entity_status.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; - entity_status.time = 0.0; - entity_status.name = name; - entity_status.bounding_box = bbox; - geometry_msgs::msg::Twist twist; - entity_status.action_status = - traffic_simulator::helper::constructActionStatus(speed, 0.0, 0.0, 0.0); - entity_status.lanelet_pose_valid = false; - entity_status.pose = pose; - return entity_status; -} - -auto makeCanonicalizedEntityStatus( - std::shared_ptr hdmap_utils, - traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, - traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, - const std::string name = "dummy_entity") - -> traffic_simulator::entity_status::CanonicalizedEntityStatus -{ - return traffic_simulator::entity_status::CanonicalizedEntityStatus( - makeEntityStatus(hdmap_utils, pose, bbox, speed, name), hdmap_utils); -} - -auto makeCanonicalizedEntityStatus( - std::shared_ptr hdmap_utils, geometry_msgs::msg::Pose pose, - traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, - const std::string name = "dummy_entity") - -> traffic_simulator::entity_status::CanonicalizedEntityStatus -{ - return traffic_simulator::entity_status::CanonicalizedEntityStatus( - makeEntityStatus(hdmap_utils, pose, bbox, speed, name), hdmap_utils); -} - -auto makePoint(const double x, const double y, const double z = 0.0) -> geometry_msgs::msg::Point -{ - geometry_msgs::msg::Point point; - point.x = x; - point.y = y; - point.z = z; - return point; -} - -auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion -{ - geometry_msgs::msg::Vector3 v; - v.z = yaw; - return quaternion_operation::convertEulerAngleToQuaternion(v); -} - -#endif // TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ From 466d3647a3ad6aff31c9035a46bd5fbfc134fa1b Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Tue, 11 Jun 2024 19:55:26 +0900 Subject: [PATCH 052/118] Remove quaternion_operation --- .../quaternion/euler_to_quaternion.hpp | 47 +++++++++++++++ .../geometry/quaternion/get_rotation.hpp | 43 ++++++++++++++ .../quaternion/get_rotation_matrix.hpp | 48 +++++++++++++++ .../quaternion/quaternion_to_euler.hpp | 45 ++++++++++++++ .../include/geometry/quaternion/slerp.hpp | 59 +++++++++++++++++++ .../include/geometry/spline/hermite_curve.hpp | 1 - common/math/geometry/package.xml | 2 +- common/math/geometry/src/bounding_box.cpp | 2 - .../geometry/src/intersection/collision.cpp | 2 - .../geometry/src/polygon/line_segment.cpp | 5 +- common/math/geometry/src/polygon/polygon.cpp | 2 - .../geometry/src/spline/hermite_curve.cpp | 3 +- common/math/geometry/src/transform.cpp | 9 +-- .../math/geometry/test/test_bounding_box.cpp | 4 +- .../geometry/test/test_catmull_rom_spline.cpp | 5 +- .../math/geometry/test/test_line_segment.cpp | 10 ++-- common/math/geometry/test/test_transform.cpp | 6 +- docs/developer_guide/TrafficSimulator.md | 2 +- .../load_do_nothing_plugin.cpp | 2 - .../src/collision/crashing_npc.cpp | 2 - .../src/collision/spawn_with_offset.cpp | 2 - .../src/crosswalk/stop_at_crosswalk.cpp | 2 - .../accelerate_and_follow.cpp | 2 - .../decelerate_and_follow.cpp | 2 - .../acquire_position_in_world_frame.cpp | 2 - .../assign_route_in_world_frame.cpp | 2 - .../src/follow_lane/cancel_request.cpp | 2 - .../src/follow_lane/follow_with_offset.cpp | 2 - ...line_trajectory_with_do_nothing_plugin.cpp | 2 - .../src/lane_change/lanechange_left.cpp | 2 - .../lane_change/lanechange_left_with_id.cpp | 2 - .../src/lane_change/lanechange_linear.cpp | 2 - .../lanechange_linear_lateral_velocity.cpp | 2 - .../lane_change/lanechange_linear_time.cpp | 2 - .../lanechange_longitudinal_distance.cpp | 2 - .../src/lane_change/lanechange_right.cpp | 2 - .../lane_change/lanechange_right_with_id.cpp | 2 - .../src/lane_change/lanechange_time.cpp | 2 - ...t_distance_in_lane_coordinate_distance.cpp | 2 - .../get_distance_to_lane_bound.cpp | 2 - .../src/merge/merge_left.cpp | 2 - .../src/metrics/traveled_distance.cpp | 2 - .../src/move_backward/move_backward.cpp | 2 - .../src/pedestrian/walk_straight.cpp | 2 - .../src/random_scenario/random001.cpp | 2 - .../speed_planning/request_speed_change.cpp | 2 - .../request_speed_change_continuous_false.cpp | 2 - .../request_speed_change_relative.cpp | 2 - .../request_speed_change_step.cpp | 2 - .../request_speed_change_with_limit.cpp | 2 - ...uest_speed_change_with_time_constraint.cpp | 2 - ...eed_change_with_time_constraint_linear.cpp | 2 - ...d_change_with_time_constraint_relative.cpp | 2 - .../src/traffic_simulation_demo.cpp | 2 - .../define_traffic_source_delay.cpp | 4 +- .../define_traffic_source_high_rate.cpp | 4 +- .../define_traffic_source_large.cpp | 4 +- .../define_traffic_source_mixed.cpp | 4 +- .../define_traffic_source_multiple.cpp | 6 +- .../define_traffic_source_outside_lane.cpp | 4 +- .../define_traffic_source_pedestrian.cpp | 4 +- .../define_traffic_source_vehicle.cpp | 4 +- .../openscenario_interpreter/CHANGELOG.rst | 2 +- .../simulator_core.hpp | 4 +- .../src/syntax/world_position.cpp | 5 +- simulation/behavior_tree_plugin/package.xml | 2 +- .../behavior_tree_plugin/src/action_node.cpp | 18 +++--- .../src/pedestrian/follow_lane_action.cpp | 2 - simulation/do_nothing_plugin/package.xml | 2 +- simulation/do_nothing_plugin/src/plugin.cpp | 3 +- .../sensor_simulation/lidar/lidar_sensor.hpp | 1 + .../sensor_simulation/lidar/raycaster.hpp | 5 +- .../simple_sensor_simulator/package.xml | 2 +- .../detection_sensor/detection_sensor.cpp | 5 +- .../sensor_simulation/lidar/lidar_sensor.cpp | 4 +- .../src/sensor_simulation/lidar/raycaster.cpp | 6 +- .../occupancy_grid/occupancy_grid_builder.cpp | 3 +- .../occupancy_grid/occupancy_grid_sensor.cpp | 5 +- .../primitives/primitive.cpp | 2 - .../src/simple_sensor_simulator.cpp | 2 - .../ego_entity_simulation.cpp | 23 +++++--- simulation/traffic_simulator/CMakeLists.txt | 1 - simulation/traffic_simulator/package.xml | 1 - simulation/traffic_simulator/src/api/api.cpp | 3 +- .../src/behavior/follow_trajectory.cpp | 32 +++++----- .../src/entity/ego_entity.cpp | 2 - .../src/entity/pedestrian_entity.cpp | 2 - .../src/entity/vehicle_entity.cpp | 2 - .../src/hdmap_utils/hdmap_utils.cpp | 21 +++---- .../traffic_simulator/src/helper/helper.cpp | 9 ++- .../src/traffic/traffic_source.cpp | 3 +- .../visualization/visualization_component.cpp | 2 - .../test/src/hdmap_utils/test_hdmap_utils.cpp | 3 +- .../scenario_test_runner/CHANGELOG.rst | 2 +- 94 files changed, 358 insertions(+), 222 deletions(-) create mode 100644 common/math/geometry/include/geometry/quaternion/euler_to_quaternion.hpp create mode 100644 common/math/geometry/include/geometry/quaternion/get_rotation.hpp create mode 100644 common/math/geometry/include/geometry/quaternion/get_rotation_matrix.hpp create mode 100644 common/math/geometry/include/geometry/quaternion/quaternion_to_euler.hpp create mode 100644 common/math/geometry/include/geometry/quaternion/slerp.hpp diff --git a/common/math/geometry/include/geometry/quaternion/euler_to_quaternion.hpp b/common/math/geometry/include/geometry/quaternion/euler_to_quaternion.hpp new file mode 100644 index 00000000000..0106164c027 --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/euler_to_quaternion.hpp @@ -0,0 +1,47 @@ +// 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 GEOMETRY__QUATERNION__EULER_TO_QUATERNION_HPP_ +#define GEOMETRY__QUATERNION__EULER_TO_QUATERNION_HPP_ + +#include + +#include +#include +#include + +namespace math +{ +namespace geometry +{ +template < + typename T, std::enable_if_t>, std::nullptr_t> = nullptr> +auto convertEulerAngleToQuaternion(const T & v) +{ + geometry_msgs::msg::Quaternion ret; + const double & roll = v.x; + const double & pitch = v.y; + const double & yaw = v.z; + tf2::Quaternion tf_quat; + tf_quat.setRPY(roll, pitch, yaw); + ret.x = tf_quat.x(); + ret.y = tf_quat.y(); + ret.z = tf_quat.z(); + ret.w = tf_quat.w(); + return ret; +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__QUATERNION__EULER_TO_QUATERNION_HPP_ diff --git a/common/math/geometry/include/geometry/quaternion/get_rotation.hpp b/common/math/geometry/include/geometry/quaternion/get_rotation.hpp new file mode 100644 index 00000000000..16e64ff192f --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/get_rotation.hpp @@ -0,0 +1,43 @@ +// 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 GEOMETRY__QUATERNION__GET_ROTATION_HPP_ +#define GEOMETRY__QUATERNION__GET_ROTATION_HPP_ + +#include + +#include +#include +#include + +namespace math +{ +namespace geometry +{ +template < + typename T, typename U, + std::enable_if_t, IsLikeQuaternion>, std::nullptr_t> = + nullptr> +auto getRotation(T from, U to) +{ + from.x *= -1; + from.y *= -1; + from.z *= -1; + auto ans = from * to; + return ans; +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__QUATERNION__GET_ROTATION_HPP_ diff --git a/common/math/geometry/include/geometry/quaternion/get_rotation_matrix.hpp b/common/math/geometry/include/geometry/quaternion/get_rotation_matrix.hpp new file mode 100644 index 00000000000..7e430fe2fe5 --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/get_rotation_matrix.hpp @@ -0,0 +1,48 @@ +// 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 GEOMETRY__QUATERNION__GET_ROTATION_MATRIX_HPP_ +#define GEOMETRY__QUATERNION__GET_ROTATION_MATRIX_HPP_ + +#include + +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +namespace math +{ +namespace geometry +{ +template < + typename T, std::enable_if_t>, std::nullptr_t> = nullptr> +auto getRotationMatrix(T quat) +{ + auto x = quat.x; + auto y = quat.y; + auto z = quat.z; + auto w = quat.w; + Eigen::Matrix3d ret(3, 3); + ret << x * x - y * y - z * z + w * w, 2 * (x * y - z * w), 2 * (z * x + w * y), + 2 * (x * y + z * w), -x * x + y * y - z * z + w * w, 2 * (y * z - x * w), 2 * (z * x - w * y), + 2 * (y * z + w * x), -x * x - y * y + z * z + w * w; + return ret; +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__QUATERNION__GET_ROTATION_MATRIX_HPP_ diff --git a/common/math/geometry/include/geometry/quaternion/quaternion_to_euler.hpp b/common/math/geometry/include/geometry/quaternion/quaternion_to_euler.hpp new file mode 100644 index 00000000000..e73a8c03ea0 --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/quaternion_to_euler.hpp @@ -0,0 +1,45 @@ +// 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 GEOMETRY__QUATERNION__QUATERNION_TO_EULER_HPP_ +#define GEOMETRY__QUATERNION__QUATERNION_TO_EULER_HPP_ + +#include + +#include +#include +#include + +namespace math +{ +namespace geometry +{ +template < + typename T, std::enable_if_t>, std::nullptr_t> = nullptr> +auto convertQuaternionToEulerAngle(const T & q) +{ + geometry_msgs::msg::Vector3 ret; + tf2::Quaternion tf_quat(q.x, q.y, q.z, q.w); + tf2::Matrix3x3 mat(tf_quat); + double roll, pitch, yaw; + mat.getRPY(roll, pitch, yaw); + ret.x = roll; + ret.y = pitch; + ret.z = yaw; + return ret; +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__QUATERNION__QUATERNION_TO_EULER_HPP_ diff --git a/common/math/geometry/include/geometry/quaternion/slerp.hpp b/common/math/geometry/include/geometry/quaternion/slerp.hpp new file mode 100644 index 00000000000..0b91e64828d --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/slerp.hpp @@ -0,0 +1,59 @@ +// 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 GEOMETRY__QUATERNION__SLERP_HPP_ +#define GEOMETRY__QUATERNION__SLERP_HPP_ + +#include +#include +#include +namespace math +{ +namespace geometry +{ +template < + typename T, typename U, typename V, + std::enable_if_t< + std::conjunction_v, IsLikeQuaternion, std::is_scalar>, + std::nullptr_t> = nullptr> +auto slerp(T quat1, U quat2, V t) +{ + geometry_msgs::msg::Quaternion q; + double qr = quat1.w * quat2.w + quat1.x * quat2.x + quat1.y * quat2.y + quat1.z * quat2.z; + double ss = 1.0 - qr * qr; + constexpr double e = std::numeric_limits::epsilon(); + if (std::fabs(ss) <= e) { + q.w = quat1.w; + q.x = quat1.x; + q.y = quat1.y; + q.z = quat1.z; + return q; + } else { + double sp = std::sqrt(ss); + double ph = std::acos(qr); + double pt = ph * t; + double t1 = std::sin(pt) / sp; + double t0 = std::sin(ph - pt) / sp; + + q.w = quat1.w * t0 + quat2.w * t1; + q.x = quat1.x * t0 + quat2.x * t1; + q.y = quat1.y * t0 + quat2.y * t1; + q.z = quat1.z * t0 + quat2.z * t1; + return q; + } +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__QUATERNION__SLERP_HPP_ diff --git a/common/math/geometry/include/geometry/spline/hermite_curve.hpp b/common/math/geometry/include/geometry/spline/hermite_curve.hpp index 1722b05f951..3b6bf8a3b91 100644 --- a/common/math/geometry/include/geometry/spline/hermite_curve.hpp +++ b/common/math/geometry/include/geometry/spline/hermite_curve.hpp @@ -16,7 +16,6 @@ #define GEOMETRY__SPLINE__HERMITE_CURVE_HPP_ #include -#include #include #include diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index de8fa10a1e5..1fd8b697143 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -12,10 +12,10 @@ boost geometry_msgs - quaternion_operation rclcpp scenario_simulator_exception tf2_geometry_msgs + tf2_ros traffic_simulator_msgs ament_cmake_clang_format diff --git a/common/math/geometry/src/bounding_box.cpp b/common/math/geometry/src/bounding_box.cpp index 2d2de3c2a2e..073ef4383f7 100644 --- a/common/math/geometry/src/bounding_box.cpp +++ b/common/math/geometry/src/bounding_box.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include diff --git a/common/math/geometry/src/intersection/collision.cpp b/common/math/geometry/src/intersection/collision.cpp index d35e7d272e6..f8578d58926 100644 --- a/common/math/geometry/src/intersection/collision.cpp +++ b/common/math/geometry/src/intersection/collision.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/common/math/geometry/src/polygon/line_segment.cpp b/common/math/geometry/src/polygon/line_segment.cpp index b942d1d2f8e..5238a24b307 100644 --- a/common/math/geometry/src/polygon/line_segment.cpp +++ b/common/math/geometry/src/polygon/line_segment.cpp @@ -12,10 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include +#include #include #include #include @@ -104,7 +103,7 @@ auto LineSegment::getPose(const double s, const bool denormalize_s, const bool f .position(getPoint(s, denormalize_s)) .orientation([this, fill_pitch]() -> geometry_msgs::msg::Quaternion { const auto tangent_vec = getVector(); - return quaternion_operation::convertEulerAngleToQuaternion( + return math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build() .x(0.0) .y( diff --git a/common/math/geometry/src/polygon/polygon.cpp b/common/math/geometry/src/polygon/polygon.cpp index a72d9d55ba5..0fd949371e0 100644 --- a/common/math/geometry/src/polygon/polygon.cpp +++ b/common/math/geometry/src/polygon/polygon.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/common/math/geometry/src/spline/hermite_curve.cpp b/common/math/geometry/src/spline/hermite_curve.cpp index d04254adf16..86c1b190411 100644 --- a/common/math/geometry/src/spline/hermite_curve.cpp +++ b/common/math/geometry/src/spline/hermite_curve.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -292,7 +293,7 @@ const geometry_msgs::msg::Pose HermiteCurve::getPose( rpy.x = 0.0; rpy.y = fill_pitch ? std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)) : 0.0; rpy.z = std::atan2(tangent_vec.y, tangent_vec.x); - pose.orientation = quaternion_operation::convertEulerAngleToQuaternion(rpy); + pose.orientation = math::geometry::convertEulerAngleToQuaternion(rpy); pose.position = getPoint(s); return pose; } diff --git a/common/math/geometry/src/transform.cpp b/common/math/geometry/src/transform.cpp index 69c1af40f9c..d730bf44601 100644 --- a/common/math/geometry/src/transform.cpp +++ b/common/math/geometry/src/transform.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include +#include #include #include @@ -64,7 +65,7 @@ const geometry_msgs::msg::Pose getRelativePose( const geometry_msgs::msg::Point transformPoint( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & point) { - auto mat = quaternion_operation::getRotationMatrix(pose.orientation); + auto mat = math::geometry::getRotationMatrix(pose.orientation); Eigen::VectorXd v(3); v(0) = point.x; v(1) = point.y; @@ -84,8 +85,8 @@ const geometry_msgs::msg::Point transformPoint( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & sensor_pose, const geometry_msgs::msg::Point & point) { - auto mat = quaternion_operation::getRotationMatrix( - quaternion_operation::getRotation(sensor_pose.orientation, pose.orientation)); + auto mat = math::geometry::getRotationMatrix( + math::geometry::getRotation(sensor_pose.orientation, pose.orientation)); Eigen::VectorXd v(3); v(0) = point.x; v(1) = point.y; diff --git a/common/math/geometry/test/test_bounding_box.cpp b/common/math/geometry/test/test_bounding_box.cpp index cbb40eb6ab5..c6df82911d0 100644 --- a/common/math/geometry/test/test_bounding_box.cpp +++ b/common/math/geometry/test/test_bounding_box.cpp @@ -13,9 +13,9 @@ // limitations under the License. #include -#include #include +#include #include #include "expect_eq_macros.hpp" @@ -79,7 +79,7 @@ TEST(BoundingBox, get2DPolygonOnlyTranslation) TEST(BoundingBox, get2DPolygonFullPose) { geometry_msgs::msg::Pose pose = makePose(1.0, 2.0); - pose.orientation = quaternion_operation::convertEulerAngleToQuaternion( + pose.orientation = math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z(30.0 * M_PI / 180.0)); traffic_simulator_msgs::msg::BoundingBox bounding_box = makeBbox(2.0, 2.0, 2.0); boost::geometry::model::polygon> poly = diff --git a/common/math/geometry/test/test_catmull_rom_spline.cpp b/common/math/geometry/test/test_catmull_rom_spline.cpp index 0ec17c5b42c..b29aba00130 100644 --- a/common/math/geometry/test/test_catmull_rom_spline.cpp +++ b/common/math/geometry/test/test_catmull_rom_spline.cpp @@ -14,6 +14,7 @@ #include +#include #include #include @@ -470,7 +471,7 @@ TEST(CatmullRomSpline, getPoseLine) EXPECT_POINT_NEAR(pose.position, makePoint(1.5, 4.5), EPS); EXPECT_QUATERNION_NEAR( pose.orientation, - quaternion_operation::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, std::atan(3.0))), EPS); + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, std::atan(3.0))), EPS); } TEST(CatmullRomSpline, getPoseLineWithPitch) @@ -480,7 +481,7 @@ TEST(CatmullRomSpline, getPoseLineWithPitch) EXPECT_POINT_NEAR(pose.position, makePoint(1.5, 4.5, 1.5), EPS); EXPECT_QUATERNION_NEAR( pose.orientation, - quaternion_operation::convertEulerAngleToQuaternion( + math::geometry::convertEulerAngleToQuaternion( makeVector(0.0, std::atan2(-1.0, std::sqrt(1.0 + 3.0 * 3.0)), std::atan(3.0))), EPS); } diff --git a/common/math/geometry/test/test_line_segment.cpp b/common/math/geometry/test/test_line_segment.cpp index cb646135004..4c21cc35626 100644 --- a/common/math/geometry/test/test_line_segment.cpp +++ b/common/math/geometry/test/test_line_segment.cpp @@ -13,10 +13,10 @@ // limitations under the License. #include -#include #include #include +#include #include #include "expect_eq_macros.hpp" @@ -310,7 +310,7 @@ TEST(LineSegment, getSValue_parallel) const auto s = line.getSValue( makePose( 1.0, 0.0, 0.0, - quaternion_operation::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4 * 3.0))), + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4 * 3.0))), 1000.0, false); EXPECT_FALSE(s); } @@ -321,7 +321,7 @@ TEST(LineSegment, getSValue_parallelDenormalize) const auto s = line.getSValue( makePose( 1.0, 0.0, 0.0, - quaternion_operation::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4 * 3.0))), + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4 * 3.0))), 1000.0, true); EXPECT_FALSE(s); } @@ -395,7 +395,7 @@ TEST(LineSegment, GetPose) line.getPose(0, false, true), geometry_msgs::build() .position(geometry_msgs::build().x(0).y(0).z(0)) - .orientation(quaternion_operation::convertEulerAngleToQuaternion( + .orientation(math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0).y(-M_PI * 0.5).z(0)))); // [Snippet_getPose_with_s_0] /// @snippet test/test_line_segment.cpp Snippet_getPose_with_s_0 @@ -412,7 +412,7 @@ TEST(LineSegment, GetPose) line.getPose(1, false, true), geometry_msgs::build() .position(geometry_msgs::build().x(0).y(0).z(1)) - .orientation(quaternion_operation::convertEulerAngleToQuaternion( + .orientation(math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0).y(-M_PI * 0.5).z(0)))); // [Snippet_getPose_with_s_1] /// @snippet test/test_line_segment.cpp Snippet_getPose_with_s_1 diff --git a/common/math/geometry/test/test_transform.cpp b/common/math/geometry/test/test_transform.cpp index f544d02f4cc..57e910cc18f 100644 --- a/common/math/geometry/test/test_transform.cpp +++ b/common/math/geometry/test/test_transform.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include -#include +#include #include #include "expect_eq_macros.hpp" @@ -27,7 +27,7 @@ geometry_msgs::msg::Pose getFilledPose() geometry_msgs::msg::Pose pose; pose.position = makePoint(1.0, 2.0, 3.0); pose.orientation = - quaternion_operation::convertEulerAngleToQuaternion(makeVector(90.0 * M_PI / 180.0, 0.0)); + math::geometry::convertEulerAngleToQuaternion(makeVector(90.0 * M_PI / 180.0, 0.0)); return pose; } @@ -38,7 +38,7 @@ TEST(Transform, getRelativePoseDifferent) geometry_msgs::msg::Pose ans = makePose( 2.0, -3.0, 2.0, - quaternion_operation::convertEulerAngleToQuaternion(makeVector(-90.0 * M_PI / 180.0, 0.0))); + math::geometry::convertEulerAngleToQuaternion(makeVector(-90.0 * M_PI / 180.0, 0.0))); EXPECT_POSE_NEAR(math::geometry::getRelativePose(pose0, pose1), ans, EPS); } diff --git a/docs/developer_guide/TrafficSimulator.md b/docs/developer_guide/TrafficSimulator.md index 67d8839a7a1..223a9c0b3bd 100644 --- a/docs/developer_guide/TrafficSimulator.md +++ b/docs/developer_guide/TrafficSimulator.md @@ -24,7 +24,7 @@ You can also see the detailed documentation of the API classes [here](https://ti ```c++ #include -#include + #include #include #include diff --git a/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp b/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp index 124f53f1597..89a9fc8075c 100644 --- a/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp +++ b/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp b/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp index a7cb7b95605..26ad2a400d1 100644 --- a/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp +++ b/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp b/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp index 04fa1eceac6..6d997733c82 100644 --- a/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp +++ b/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index cd19c42a469..26e6a273e36 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp index d89f8584c03..a5d2847a2cd 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp index 6079e54af5e..0b178194b62 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp index 9ca26be1585..5fad7981307 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp index ea6d6cbf02d..635d91ef5ae 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp index acb92544a19..65d13fe6dc3 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp b/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp index dc2d0df7616..38963edcf91 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp b/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp index 6bf26f4bfc6..001c24f9589 100644 --- a/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp +++ b/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp index 0da53d6be17..6c68583ed97 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp index f44fd8cf1db..c4b7ba29580 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp index 96e4f951e36..8379d7e3a0e 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp index 5fb7bf04e9c..5d04ac0c4c7 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp index ce85da9136b..0962d15531e 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp index 18b71d0914a..24fda49308e 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp index 99a832208b4..c5832108900 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp index aca94784ae8..4ebb73d0d9b 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp index d51f0629965..a9c82824707 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp index 56bd0f2bd42..98c428e23fc 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp index 5cb649c30a4..a177ccaa4bb 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/merge/merge_left.cpp b/mock/cpp_mock_scenarios/src/merge/merge_left.cpp index 58657664c30..bf3cdea6b7c 100644 --- a/mock/cpp_mock_scenarios/src/merge/merge_left.cpp +++ b/mock/cpp_mock_scenarios/src/merge/merge_left.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp b/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp index a9fb6e778d1..c99152021b7 100644 --- a/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp +++ b/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp index 86472368741..a326b433423 100644 --- a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp +++ b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp index c43a293acaa..b5d4999a694 100644 --- a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp +++ b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 18ba59f2115..2cd4746d5dd 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp index 79b7c5dfb05..bd1f63a5a7b 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp index b49c3b12311..1875dcebd71 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp index 87721639833..869754527fb 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp index 2cdec5c0d60..36cdfbae984 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp index 5e9e161d001..b4639aa6530 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp index 3552d6dbb75..d5f21a2e0fa 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp index b6999ecd263..c0df3f38c68 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp index be3fa4e59d9..e40b06d635d 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp index d323f921542..a87fada6355 100644 --- a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp index 68fc8abafaf..1f9f6966f8f 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include @@ -57,7 +55,7 @@ class DefineTrafficSourceDelay : public cpp_mock_scenarios::CppScenarioNode 5.0, 2.0, 10.0, geometry_msgs::build() .position(geometry_msgs::build().x(3755.0).y(73692.5).z(0.0)) - .orientation(quaternion_operation::convertEulerAngleToQuaternion( + .orientation(math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z(0.321802))), // clang-format off { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp index 29c75300e5b..0cc96e172d7 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include @@ -78,7 +76,7 @@ class DefineTrafficSourceHighRate : public cpp_mock_scenarios::CppScenarioNode 5.0, 50.0, 10.0, geometry_msgs::build() .position(geometry_msgs::build().x(3755.0).y(73692.5).z(0.0)) - .orientation(quaternion_operation::convertEulerAngleToQuaternion( + .orientation(math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z(0.321802))), // clang-format off { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp index 5b16c43c2f5..4f1fcc8b67d 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include @@ -74,7 +72,7 @@ class DefineTrafficSourceLarge : public cpp_mock_scenarios::CppScenarioNode 200.0, 2.0, 10.0, geometry_msgs::build() .position(geometry_msgs::build().x(3764.18).y(73745.45).z(0.0)) - .orientation(quaternion_operation::convertEulerAngleToQuaternion( + .orientation(math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z(0.321802))), // clang-format off { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp index 211ae7cd7c1..b1169c8aef8 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include @@ -91,7 +89,7 @@ class DefineTrafficSourceMixed : public cpp_mock_scenarios::CppScenarioNode 5.0, 3.0, 5.0, geometry_msgs::build() .position(geometry_msgs::build().x(3755.0).y(73692.5).z(0.0)) - .orientation(quaternion_operation::convertEulerAngleToQuaternion( + .orientation(math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z(0.321802))), // clang-format off { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp index 316953dccff..1c0cb7de4a1 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include @@ -100,7 +98,7 @@ class DefineTrafficSourceMultiple : public cpp_mock_scenarios::CppScenarioNode 5.0, 2.0, 10.0, geometry_msgs::build() .position(geometry_msgs::build().x(3755.0).y(73692.5).z(0.0)) - .orientation(quaternion_operation::convertEulerAngleToQuaternion( + .orientation(math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z(0.321802))), // clang-format off { @@ -113,7 +111,7 @@ class DefineTrafficSourceMultiple : public cpp_mock_scenarios::CppScenarioNode 1.0, 5.0, 3.0, geometry_msgs::build() .position(geometry_msgs::build().x(3757.0).y(73750.3).z(0.0)) - .orientation(quaternion_operation::convertEulerAngleToQuaternion( + .orientation(math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z(0.321802))), // clang-format off diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp index bd51fe41d91..aa77e88828f 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include @@ -74,7 +72,7 @@ class DefineTrafficSourceOutsideLane : public cpp_mock_scenarios::CppScenarioNod 10.0, 2.0, 3.0, geometry_msgs::build() .position(geometry_msgs::build().x(3745.0).y(73710.0).z(0.0)) - .orientation(quaternion_operation::convertEulerAngleToQuaternion( + .orientation(math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z(0.0))), // clang-format off diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp index a86c9a59146..13237b0810b 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include @@ -77,7 +75,7 @@ class DefineTrafficSourcePedestrian : public cpp_mock_scenarios::CppScenarioNode 1.0, 5.0, 3.0, geometry_msgs::build() .position(geometry_msgs::build().x(3757.0).y(73750.3).z(0.0)) - .orientation(quaternion_operation::convertEulerAngleToQuaternion( + .orientation(math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z(0.321802))), // clang-format off diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp index f4d9d95aaa4..d8ecac1f33d 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include @@ -78,7 +76,7 @@ class DefineTrafficSourceVehicle : public cpp_mock_scenarios::CppScenarioNode 5.0, 2.0, 10.0, geometry_msgs::build() .position(geometry_msgs::build().x(3755.0).y(73692.5).z(0.0)) - .orientation(quaternion_operation::convertEulerAngleToQuaternion( + .orientation(math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z(0.321802))), // clang-format off { diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index c1b65a9878e..d674a34e234 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -2850,7 +2850,7 @@ Changelog for package openscenario_interpreter * Lipsticks * Add optional argument verbose (= True) to openscenario_utility * Rename 'step_time_ms' to 'frame-rate' -* Fix external/quaternion_operation's commit hash +* Fix external/math::geometry's commit hash * Add parameter 'real-time-factor' to openscenario_interpreter * enable pass colcon test * enable pass test case diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 5e1f4ef6a76..a56ae6daebc 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -15,6 +15,7 @@ #ifndef OPENSCENARIO_INTERPRETER__SIMULATOR_CORE_HPP_ #define OPENSCENARIO_INTERPRETER__SIMULATOR_CORE_HPP_ +#include #include #include #include @@ -562,8 +563,7 @@ class SimulatorCore const auto relative_pose = traffic_simulator::pose::relativePose(from_map_pose, to_map_pose)) { return static_cast(std::abs( - quaternion_operation::convertQuaternionToEulerAngle(relative_pose.value().orientation) - .z)); + math::geometry::convertQuaternionToEulerAngle(relative_pose.value().orientation).z)); } } return Double::nan(); diff --git a/openscenario/openscenario_interpreter/src/syntax/world_position.cpp b/openscenario/openscenario_interpreter/src/syntax/world_position.cpp index f21e6d3089e..1969d597d90 100644 --- a/openscenario/openscenario_interpreter/src/syntax/world_position.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/world_position.cpp @@ -12,8 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - +#include #include #include #include @@ -43,7 +42,7 @@ WorldPosition::operator NativeWorldPosition() const native_world_position.position.x = x; native_world_position.position.y = y; native_world_position.position.z = z; - native_world_position.orientation = quaternion_operation::convertEulerAngleToQuaternion( + native_world_position.orientation = math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(r).y(p).z(h)); return native_world_position; } diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 22a099d3fac..b98f5fe07d3 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -12,7 +12,7 @@ behaviortree_cpp_v3 geometry pluginlib - quaternion_operation + math::geometry rclcpp traffic_simulator diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index c621d3120bc..0109b24187f 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include +#include +#include +#include +#include #include #include #include @@ -238,14 +240,14 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf std::vector entities; for (const auto & each : other_entity_status) { const auto distance = getDistanceToTargetEntityPolygon(spline, each.first); - const auto quat = quaternion_operation::getRotation( + const auto quat = math::geometry::getRotation( entity_status->getMapPose().orientation, other_entity_status.at(each.first).getMapPose().orientation); /** * @note hard-coded parameter, if the Yaw value of RPY is in ~1.5708 -> 1.5708, entity is a candidate of front entity. */ if ( - std::fabs(quaternion_operation::convertQuaternionToEulerAngle(quat).z) <= + std::fabs(math::geometry::convertQuaternionToEulerAngle(quat).z) <= boost::math::constants::half_pi()) { if (distance && distance.value() < 40) { entities.emplace_back(each.first); @@ -477,6 +479,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const -> traffic_simulator::CanonicalizedEntityStatus { + using math::geometry::operator*; const auto speed_planner = traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner( step_time, getEntityName()); @@ -489,14 +492,13 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( geometry_msgs::msg::Vector3 angular_trans_vec; angular_trans_vec.z = twist_new.angular.z * step_time; geometry_msgs::msg::Quaternion angular_trans_quat = - quaternion_operation::convertEulerAngleToQuaternion(angular_trans_vec); - pose_new.orientation = - quaternion_operation::rotation(entity_status->getMapPose().orientation, angular_trans_quat); + math::geometry::convertEulerAngleToQuaternion(angular_trans_vec); + pose_new.orientation = entity_status->getMapPose().orientation * angular_trans_quat; Eigen::Vector3d trans_vec; trans_vec(0) = twist_new.linear.x * step_time; trans_vec(1) = twist_new.linear.y * step_time; trans_vec(2) = 0; - Eigen::Matrix3d rotation_mat = quaternion_operation::getRotationMatrix(pose_new.orientation); + Eigen::Matrix3d rotation_mat = math::geometry::getRotationMatrix(pose_new.orientation); trans_vec = rotation_mat * trans_vec; pose_new.position.x = trans_vec(0) + entity_status->getMapPose().position.x; pose_new.position.y = trans_vec(1) + entity_status->getMapPose().position.y; diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp index 55f6fda2dfb..afdb84d22b4 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index a4246b99be7..d63f3674914 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -12,7 +12,7 @@ behaviortree_cpp_v3 pluginlib - quaternion_operation + math::geometry rclcpp traffic_simulator diff --git a/simulation/do_nothing_plugin/src/plugin.cpp b/simulation/do_nothing_plugin/src/plugin.cpp index 2bc20fe9041..ca84dc72406 100644 --- a/simulation/do_nothing_plugin/src/plugin.cpp +++ b/simulation/do_nothing_plugin/src/plugin.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -88,7 +89,7 @@ auto interpolateEntityStatusFromPolylineTrajectory( .position( v0.position.position * (1 - interpolation_ratio) + v1.position.position * interpolation_ratio) - .orientation(quaternion_operation::slerp( + .orientation(math::geometry::slerp( v0.position.orientation, v1.position.orientation, interpolation_ratio)); const double linear_velocity = math::geometry::hypot(v1.position.position, v0.position.position) / (v1.time - v0.time); diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp index f28d48ca608..1b78e99770d 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp @@ -17,6 +17,7 @@ #include +#include #include #include #include diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/raycaster.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/raycaster.hpp index d0bd697c469..7cb4675a075 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/raycaster.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/raycaster.hpp @@ -17,8 +17,9 @@ #include #include -#include +#include +#include #include #include #include @@ -83,7 +84,7 @@ class Raycaster { auto & rotation_matrices = ref_rotation_matrices.get(); auto & thread_detected_ids = ref_thread_detected_ids.get(); - const auto orientation_matrix = quaternion_operation::getRotationMatrix(origin.orientation); + const auto orientation_matrix = math::geometry::getRotationMatrix(origin.orientation); for (unsigned int i = thread_id; i < rotation_matrices.size(); i += thread_count) { RTCRayHit rayhit = {}; rayhit.ray.org_x = origin.position.x; diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 82a64042c00..39be4e2fa8f 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -43,7 +43,7 @@ libpcl-all-dev nav_msgs pcl_conversions - quaternion_operation + math::geometry rclcpp_components simulation_interface traffic_simulator_msgs diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index b7f32b6b57b..23b38f4b094 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include @@ -21,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -117,7 +116,7 @@ auto make(const traffic_simulator_msgs::EntityStatus & status) -> geometry_msgs: auto center_point = geometry_msgs::msg::Point(); simulation_interface::toMsg(status.bounding_box().center(), center_point); - Eigen::Vector3d center = quaternion_operation::getRotationMatrix(pose.orientation) * + Eigen::Vector3d center = math::geometry::getRotationMatrix(pose.orientation) * Eigen::Vector3d(center_point.x, center_point.y, center_point.z); pose.position.x = pose.position.x + center.x(); diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/lidar/lidar_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/lidar/lidar_sensor.cpp index 14d4a427f2d..84f4d44ce7e 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/lidar/lidar_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/lidar/lidar_sensor.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include @@ -39,7 +37,7 @@ auto LidarSensor::raycast( } else { geometry_msgs::msg::Pose pose; simulation_interface::toMsg(entity.pose(), pose); - auto rotation = quaternion_operation::getRotationMatrix(pose.orientation); + auto rotation = math::geometry::getRotationMatrix(pose.orientation); geometry_msgs::msg::Point center_point; simulation_interface::toMsg(entity.bounding_box().center(), center_point); Eigen::Vector3d center(center_point.x, center_point.y, center_point.z); diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/lidar/raycaster.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/lidar/raycaster.cpp index e1325bfa354..ebfb547171c 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/lidar/raycaster.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/lidar/raycaster.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include @@ -61,7 +59,7 @@ void Raycaster::setDirection( configuration.horizontal_resolution()); rotation_matrices_.clear(); for (const auto & q : quat_directions) { - rotation_matrices_.push_back(quaternion_operation::getRotationMatrix(q)); + rotation_matrices_.push_back(math::geometry::getRotationMatrix(q)); } } @@ -83,7 +81,7 @@ std::vector Raycaster::getDirections( rpy.x = 0; rpy.y = vertical_angle; rpy.z = horizontal_angle; - auto quat = quaternion_operation::convertEulerAngleToQuaternion(rpy); + auto quat = math::geometry::convertEulerAngleToQuaternion(rpy); directions.emplace_back(quat); } } diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_builder.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_builder.cpp index ec081adb11d..53e84567a44 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_builder.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_builder.cpp @@ -12,9 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include +#include #include #include #include diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp index d947efe5d5d..c48ecc4a93c 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp @@ -12,9 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include +#include #include #include #include @@ -126,7 +125,7 @@ auto OccupancyGridSensor::getOccupancyGrid( simulation_interface::toMsg(s.pose(), pose); auto point = geometry_msgs::msg::Point(); simulation_interface::toMsg(s.bounding_box().center(), point); - auto rotation = quaternion_operation::getRotationMatrix(pose.orientation); + auto rotation = math::geometry::getRotationMatrix(pose.orientation); auto center = (rotation * Eigen::Vector3d(point.x, point.y, point.z)).eval(); pose.position.x += center.x(); diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/primitives/primitive.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/primitives/primitive.cpp index ca998ac1fdb..4ed7a01e06e 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/primitives/primitive.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/primitives/primitive.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index ad7d92218e2..95110509a6e 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp index f58e618fe20..982a4379f1d 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp @@ -14,6 +14,10 @@ #include #include +#include +#include +#include +#include #include #include @@ -215,8 +219,8 @@ void EgoEntitySimulation::overwrite( const traffic_simulator_msgs::msg::EntityStatus & status, double current_scenario_time, double step_time, bool npc_logic_started) { - using quaternion_operation::convertQuaternionToEulerAngle; - using quaternion_operation::getRotationMatrix; + using math::geometry::convertQuaternionToEulerAngle; + using math::geometry::getRotationMatrix; autoware->rethrow(); @@ -280,7 +284,7 @@ void EgoEntitySimulation::overwrite( void EgoEntitySimulation::update( double current_scenario_time, double step_time, bool npc_logic_started) { - using quaternion_operation::getRotationMatrix; + using math::geometry::getRotationMatrix; autoware->rethrow(); @@ -440,9 +444,10 @@ auto EgoEntitySimulation::getCurrentTwist() const -> geometry_msgs::msg::Twist auto EgoEntitySimulation::getCurrentPose(const double pitch_angle = 0.) const -> geometry_msgs::msg::Pose { + using math::geometry::operator*; const auto relative_position = - quaternion_operation::getRotationMatrix(initial_pose_.orientation) * world_relative_position_; - const auto relative_orientation = quaternion_operation::convertEulerAngleToQuaternion( + math::geometry::getRotationMatrix(initial_pose_.orientation) * world_relative_position_; + const auto relative_orientation = math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build() .x(0) .y(pitch_angle) @@ -531,17 +536,17 @@ auto EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet( if (const auto s_value = spline.getSValue(status.pose)) { status.pose.position.z = spline.getPoint(s_value.value()).z; if (consider_pose_by_road_slope_) { - const auto rpy = quaternion_operation::convertQuaternionToEulerAngle( + const auto rpy = math::geometry::convertQuaternionToEulerAngle( spline.getPose(s_value.value(), true).orientation); const auto original_rpy = - quaternion_operation::convertQuaternionToEulerAngle(status.pose.orientation); - status.pose.orientation = quaternion_operation::convertEulerAngleToQuaternion( + math::geometry::convertQuaternionToEulerAngle(status.pose.orientation); + status.pose.orientation = math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build() .x(original_rpy.x) .y(rpy.y) .z(original_rpy.z)); lanelet_pose->rpy = - quaternion_operation::convertQuaternionToEulerAngle(quaternion_operation::getRotation( + math::geometry::convertQuaternionToEulerAngle(math::geometry::getRotation( spline.getPose(s_value.value(), true).orientation, status.pose.orientation)); } } diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index 00b349d12ad..c7e9c3e212a 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -20,7 +20,6 @@ find_package(traffic_simulator_msgs REQUIRED) find_package(Boost COMPONENTS filesystem) find_package(lanelet2_matching REQUIRED) find_package(tinyxml2_vendor REQUIRED) -find_package(quaternion_operation REQUIRED) find_package(pluginlib REQUIRED) include(FindProtobuf REQUIRED) diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 85f6e6a1a01..9623e534d52 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -27,7 +27,6 @@ libomp-dev nlohmann-json-dev pugixml-dev - quaternion_operation rclcpp rclcpp_components rosgraph_msgs diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 68a67a8d5d2..09ad9ec99a2 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -14,6 +14,7 @@ #include +#include #include #include #include @@ -112,7 +113,7 @@ auto API::setEntityStatus( { geometry_msgs::msg::Pose relative_pose; relative_pose.position = relative_position; - relative_pose.orientation = quaternion_operation::convertEulerAngleToQuaternion(relative_rpy); + relative_pose.orientation = math::geometry::convertEulerAngleToQuaternion(relative_rpy); setEntityStatus(name, reference_entity_name, relative_pose, action_status); } diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 4b98cc16c8b..c27cabdd538 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include +#include +#include +#include #include #include #include @@ -92,18 +93,17 @@ auto makeUpdatedStatus( status.lanelet_pose = lanelet_pose.value(); const CatmullRomSpline spline(hdmap_utils->getCenterPoints(status.lanelet_pose.lanelet_id)); const auto lanelet_quaternion = spline.getPose(status.lanelet_pose.s, true).orientation; - const auto lanelet_rpy = - quaternion_operation::convertQuaternionToEulerAngle(lanelet_quaternion); + const auto lanelet_rpy = math::geometry::convertQuaternionToEulerAngle(lanelet_quaternion); const auto entity_rpy = - quaternion_operation::convertQuaternionToEulerAngle(status.pose.orientation); + math::geometry::convertQuaternionToEulerAngle(status.pose.orientation); // adjust orientation in EntityStatus.pose (only pitch) and in EntityStatus.LaneletPose - status.pose.orientation = quaternion_operation::convertEulerAngleToQuaternion( + status.pose.orientation = math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build() .x(entity_rpy.x) .y(lanelet_rpy.y) .z(entity_rpy.z)); - status.lanelet_pose.rpy = quaternion_operation::convertQuaternionToEulerAngle( - quaternion_operation::getRotation(lanelet_quaternion, status.pose.orientation)); + status.lanelet_pose.rpy = math::geometry::convertQuaternionToEulerAngle( + math::geometry::getRotation(lanelet_quaternion, status.pose.orientation)); status.lanelet_pose_valid = true; } else { status.lanelet_pose_valid = false; @@ -386,7 +386,7 @@ auto makeUpdatedStatus( // if entity is on lane use pitch from lanelet, otherwise use pitch on target const auto pitch = entity_status.lanelet_pose_valid - ? -quaternion_operation::convertQuaternionToEulerAngle( + ? -math::geometry::convertQuaternionToEulerAngle( entity_status.pose.orientation) .y : std::atan2(target_position.z - position.z, std::hypot(dy, dx)); @@ -417,12 +417,10 @@ auto makeUpdatedStatus( desired_velocity.y, ", ", desired_velocity.z, "]."); } else if (const auto current_velocity = [&]() { - const auto pitch = -quaternion_operation::convertQuaternionToEulerAngle( - entity_status.pose.orientation) - .y; - const auto yaw = quaternion_operation::convertQuaternionToEulerAngle( - entity_status.pose.orientation) - .z; + const auto pitch = + -math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation).y; + const auto yaw = + math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation).z; return geometry_msgs::build() .x(std::cos(pitch) * std::cos(yaw) * speed) .y(std::cos(pitch) * std::sin(yaw) * speed) @@ -593,7 +591,7 @@ auto makeUpdatedStatus( .x(0.0) .y(std::atan2(-desired_velocity.z, std::hypot(desired_velocity.x, desired_velocity.y))) .z(std::atan2(desired_velocity.y, desired_velocity.x)); - return quaternion_operation::convertEulerAngleToQuaternion(direction); + return math::geometry::convertEulerAngleToQuaternion(direction); } }(); @@ -611,7 +609,7 @@ auto makeUpdatedStatus( updated_status.action_status.twist.linear.z = 0; updated_status.action_status.twist.angular = - quaternion_operation::convertQuaternionToEulerAngle(quaternion_operation::getRotation( + math::geometry::convertQuaternionToEulerAngle(math::geometry::getRotation( entity_status.pose.orientation, updated_status.pose.orientation)) / step_time; diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index ca5e551b472..24c89bed0ef 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp index 631a9b4b9fe..ba21c07c37d 100644 --- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp +++ b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index 8a1963640fd..cfe1f741104 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 5576c8203c2..c6e24e9d1ec 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -16,7 +16,6 @@ #include #include #include -#include #include #include @@ -27,7 +26,10 @@ #include #include #include +#include +#include #include +#include #include #include #include @@ -505,9 +507,9 @@ auto HdMapUtils::matchToLane( std::optional id; lanelet::matching::Object2d obj; obj.pose.translation() = toPoint2d(pose.position); - obj.pose.linear() = Eigen::Rotation2D( - quaternion_operation::convertQuaternionToEulerAngle(pose.orientation).z) - .matrix(); + obj.pose.linear() = + Eigen::Rotation2D(math::geometry::convertQuaternionToEulerAngle(pose.orientation).z) + .matrix(); obj.absoluteHull = absoluteHull( lanelet::matching::Hull2d{ lanelet::BasicPoint2d{ @@ -567,8 +569,8 @@ auto HdMapUtils::toLaneletPose( return std::nullopt; } auto pose_on_centerline = spline->getPose(s.value()); - auto rpy = quaternion_operation::convertQuaternionToEulerAngle( - quaternion_operation::getRotation(pose_on_centerline.orientation, pose.orientation)); + auto rpy = math::geometry::convertQuaternionToEulerAngle( + math::geometry::getRotation(pose_on_centerline.orientation, pose.orientation)); double offset = std::sqrt(spline->getSquaredDistanceIn2D(pose.position, s.value())); /** * @note Hard coded parameter @@ -1383,8 +1385,7 @@ auto HdMapUtils::getLaneChangeTrajectory( auto HdMapUtils::getVectorFromPose(const geometry_msgs::msg::Pose & pose, const double magnitude) const -> geometry_msgs::msg::Vector3 { - geometry_msgs::msg::Vector3 dir = - quaternion_operation::convertQuaternionToEulerAngle(pose.orientation); + geometry_msgs::msg::Vector3 dir = math::geometry::convertQuaternionToEulerAngle(pose.orientation); geometry_msgs::msg::Vector3 vector; vector.x = magnitude * std::cos(dir.z); vector.y = magnitude * std::sin(dir.z); @@ -1429,8 +1430,8 @@ auto HdMapUtils::toMapPose( rpy.x = 0.0; rpy.y = fill_pitch ? std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)) : 0.0; rpy.z = std::atan2(tangent_vec.y, tangent_vec.x); - ret.pose.orientation = quaternion_operation::convertEulerAngleToQuaternion(rpy) * - quaternion_operation::convertEulerAngleToQuaternion(pose->rpy); + ret.pose.orientation = math::geometry::convertEulerAngleToQuaternion(rpy) * + math::geometry::convertEulerAngleToQuaternion(pose->rpy); return ret; } else { THROW_SEMANTIC_ERROR( diff --git a/simulation/traffic_simulator/src/helper/helper.cpp b/simulation/traffic_simulator/src/helper/helper.cpp index 9ebbb59622a..943cf16ea73 100644 --- a/simulation/traffic_simulator/src/helper/helper.cpp +++ b/simulation/traffic_simulator/src/helper/helper.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - +#include +#include #include #include @@ -60,7 +60,7 @@ geometry_msgs::msg::Vector3 constructRPY(double roll, double pitch, double yaw) geometry_msgs::msg::Vector3 constructRPYfromQuaternion(geometry_msgs::msg::Quaternion quaternion) { - return quaternion_operation::convertQuaternionToEulerAngle(quaternion); + return math::geometry::convertQuaternionToEulerAngle(quaternion); } geometry_msgs::msg::Pose constructPose( @@ -70,8 +70,7 @@ geometry_msgs::msg::Pose constructPose( pose.position.x = x; pose.position.y = y; pose.position.z = z; - pose.orientation = - quaternion_operation::convertEulerAngleToQuaternion(constructRPY(roll, pitch, yaw)); + pose.orientation = math::geometry::convertEulerAngleToQuaternion(constructRPY(roll, pitch, yaw)); return pose; } diff --git a/simulation/traffic_simulator/src/traffic/traffic_source.cpp b/simulation/traffic_simulator/src/traffic/traffic_source.cpp index 459786e56af..350920a42d1 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_source.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_source.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -83,7 +84,7 @@ auto TrafficSource::makeRandomPose(const bool random_orientation) -> geometry_ms random_pose.position.y += radius * std::sin(angle); if (random_orientation) { - random_pose.orientation = quaternion_operation::convertEulerAngleToQuaternion( + random_pose.orientation = math::geometry::convertEulerAngleToQuaternion( traffic_simulator::helper::constructRPY(0.0, 0.0, angle_distribution_(engine_))); } diff --git a/simulation/traffic_simulator/src/visualization/visualization_component.cpp b/simulation/traffic_simulator/src/visualization/visualization_component.cpp index 24f9e1ebffb..6cea709f9d8 100644 --- a/simulation/traffic_simulator/src/visualization/visualization_component.cpp +++ b/simulation/traffic_simulator/src/visualization/visualization_component.cpp @@ -42,8 +42,6 @@ */ -#include - #include #include #include diff --git a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp index 834a0576386..3cd66501b35 100644 --- a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp +++ b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -52,7 +53,7 @@ auto makeSmallBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion { - return quaternion_operation::convertEulerAngleToQuaternion( + return math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z(yaw)); } diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index c4c97408eef..0cca17a6734 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -1728,7 +1728,7 @@ Changelog for package scenario_test_runner * Rename option 'timeout' to 'global-timeout' * Add optional argument verbose (= True) to openscenario_utility * Rename 'step_time_ms' to 'frame-rate' -* Fix external/quaternion_operation's commit hash +* Fix external/math::geometry's commit hash * Add parameter 'real-time-factor' to openscenario_interpreter * Rename option to '--global-real-time-factor' from 'real-time-factor' * Add launch argument '--real-time-factor' From 3abd2e9915566d3a00b642dae47c21d4d6292ff5 Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Tue, 11 Jun 2024 20:05:04 +0900 Subject: [PATCH 053/118] fix package xml --- simulation/behavior_tree_plugin/package.xml | 1 - simulation/do_nothing_plugin/package.xml | 1 - simulation/simple_sensor_simulator/package.xml | 1 - 3 files changed, 3 deletions(-) diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index b98f5fe07d3..45f345e2d57 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -12,7 +12,6 @@ behaviortree_cpp_v3 geometry pluginlib - math::geometry rclcpp traffic_simulator diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index d63f3674914..c210afc6c83 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -12,7 +12,6 @@ behaviortree_cpp_v3 pluginlib - math::geometry rclcpp traffic_simulator diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 39be4e2fa8f..ccdbf554933 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -43,7 +43,6 @@ libpcl-all-dev nav_msgs pcl_conversions - math::geometry rclcpp_components simulation_interface traffic_simulator_msgs From 77cdef15d4a6322bde6102286f67a6163035d7ff Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 11 Jun 2024 13:41:19 +0200 Subject: [PATCH 054/118] collect tests, will not compile --- .../test/src/entity/CMakeLists.txt | 6 + ...st_entity_base_with_misc_object_entity.cpp | 798 ++++++++++++++++++ .../src/entity/test_misc_object_entity.cpp | 211 +++++ .../test/src/helper_functions.hpp | 133 +++ 4 files changed, 1148 insertions(+) create mode 100644 simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp create mode 100644 simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp create mode 100644 simulation/traffic_simulator/test/src/helper_functions.hpp diff --git a/simulation/traffic_simulator/test/src/entity/CMakeLists.txt b/simulation/traffic_simulator/test/src/entity/CMakeLists.txt index 0e6b9eee47d..68de39065b2 100644 --- a/simulation/traffic_simulator/test/src/entity/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/entity/CMakeLists.txt @@ -1,2 +1,8 @@ ament_add_gtest(test_vehicle_entity test_vehicle_entity.cpp) target_link_libraries(test_vehicle_entity traffic_simulator) + +ament_add_gtest(test_misc_object_entity test_misc_object_entity.cpp) +target_link_libraries(test_misc_object_entity traffic_simulator) + +ament_add_gtest(test_entity_base_with_misc_object_entity test_entity_base_with_misc_object_entity.cpp) +target_link_libraries(test_entity_base_with_misc_object_entity traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp new file mode 100644 index 00000000000..94a27027bd1 --- /dev/null +++ b/simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp @@ -0,0 +1,798 @@ +// 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 + +#include +#include +#include +#include + +#include "../catalogs.hpp" +#include "../expect_eq_macros.hpp" +#include "../helper_functions.hpp" + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +class EntityBaseTest : public testing::Test +{ +protected: + EntityBaseTest() + : id(120659), + hdmap_utils(makeHdMapUtilsSharedPointer()), + pose(makeCanonicalizedLaneletPose(hdmap_utils, id)), + bbox(makeBoundingBox()), + status(makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox)), + dummy("dummy_entity", status, hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}), + dummy_base(&dummy) + { + } + + const lanelet::Id id; + std::shared_ptr hdmap_utils; + traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose; + traffic_simulator_msgs::msg::BoundingBox bbox; + traffic_simulator::entity_status::CanonicalizedEntityStatus status; + traffic_simulator::entity::MiscObjectEntity dummy; + traffic_simulator::entity::EntityBase * dummy_base; +}; + +/** + * @note Test basic functionality; test whether the function does nothing. + */ +TEST_F(EntityBaseTest, appendDebugMarker) +{ + visualization_msgs::msg::MarkerArray markers{}; + + { + auto markers_copy = markers; + dummy.appendDebugMarker(markers); + EXPECT_EQ(markers.markers.size(), markers_copy.markers.size()); + } + + visualization_msgs::msg::Marker marker{}; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.pose.position.x = 10.0; + marker.pose.position.y = 10.0; + + markers.markers.push_back(marker); + + { + auto markers_copy = markers; + dummy.appendDebugMarker(markers); + EXPECT_EQ(markers.markers.size(), markers_copy.markers.size()); + } +} + +/** + * @note Test basic functionality; test whether the function throws an error. + */ +TEST_F(EntityBaseTest, asFieldOperatorApplication) +{ + EXPECT_THROW(dummy.asFieldOperatorApplication(), common::Error); +} + +/** + * @note Test functionality used by other units; test correctness of 2d polygon calculations. + */ +TEST_F(EntityBaseTest, get2DPolygon) +{ + const auto polygon = dummy.get2DPolygon(); + + std::vector ref_poly{}; + + ref_poly.push_back(makePoint(-1.0, -1.0)); + ref_poly.push_back(makePoint(-1.0, 1.0)); + ref_poly.push_back(makePoint(3.0, 1.0)); + ref_poly.push_back(makePoint(3.0, -1.0)); + ref_poly.push_back(ref_poly.front()); + + EXPECT_EQ(polygon.size(), ref_poly.size()); + EXPECT_POINT_EQ(polygon.at(0), ref_poly.at(0)); + EXPECT_POINT_EQ(polygon.at(1), ref_poly.at(1)); + EXPECT_POINT_EQ(polygon.at(2), ref_poly.at(2)); + EXPECT_POINT_EQ(polygon.at(3), ref_poly.at(3)); + EXPECT_POINT_EQ(polygon.at(4), ref_poly.at(4)); +} + +/** + * @note Test basic functionality; test whether the NPC logic is started correctly. + */ +TEST_F(EntityBaseTest, startNpcLogic) +{ + EXPECT_FALSE(dummy.isNpcLogicStarted()); + dummy.startNpcLogic(0.0); + EXPECT_TRUE(dummy.isNpcLogicStarted()); +} + +/** + * @note Test basic functionality; test activating an out of range job with + * an entity that has a positive speed and a speed range specified in the job = [0, 0] + */ +TEST_F(EntityBaseTest, activateOutOfRangeJob_speed) +{ + double min_velocity = 0.0; + double max_velocity = 0.0; + double min_acceleration = -100.0; + double max_acceleration = 100.0; + double min_jerk = -100.0; + double max_jerk = 100.0; + double velocity = 1.0; + dummy.setLinearVelocity(velocity); + dummy.activateOutOfRangeJob( + min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); + double current_time = 0.0; + double step_time = 0.0; + EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); + EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); +} + +/** + * @note Test basic functionality; test activating an out of range job + * with an entity that has a positive acceleration + * and an acceleration range specified in the job = [0, 0]. + */ +TEST_F(EntityBaseTest, activateOutOfRangeJob_acceleration) +{ + double min_velocity = -100.0; + double max_velocity = 100.0; + double min_acceleration = 0.0; + double max_acceleration = 0.0; + double min_jerk = -100.0; + double max_jerk = 100.0; + double acceleration = 1.0; + dummy.setLinearAcceleration(acceleration); + dummy.activateOutOfRangeJob( + min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); + double current_time = 0.0; + double step_time = 0.0; + EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); + EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); +} + +/** + * @note Test basic functionality; test activating an out of range job + * with an entity that has a positive jerk + * and a jerk range specified in the job = [0, 0]. + */ +TEST_F(EntityBaseTest, activateOutOfRangeJob_jerk) +{ + double min_velocity = -100.0; + double max_velocity = 100.0; + double min_acceleration = -100.0; + double max_acceleration = 100.0; + double min_jerk = 0.0; + double max_jerk = 0.0; + double jerk = 1.0; + dummy.setLinearJerk(jerk); + dummy.activateOutOfRangeJob( + min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); + double current_time = 0.0; + double step_time = 0.0; + EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); + EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); +} + +/** + * @note Test functionality used by other units; test update execution correctness + * with a PRE_UPDATE task type added to the task queue (can be done in one of + * the implemented pure virtual member functions) - the goal is to check whether + * the task has been executed and whether the status and speed planner have been updated. + */ +TEST_F(EntityBaseTest, onUpdate) +{ + bool first_cleanup = false; + bool first_update = false; + bool second_cleanup = false; + bool second_update = false; + + auto first_update_func = [&first_update](const double) { return first_update = true; }; + auto first_cleanup_func = [&first_cleanup]() { first_cleanup = true; }; + auto second_update_func = [&second_update](const double) { return second_update = true; }; + auto second_cleanup_func = [&second_cleanup]() { second_cleanup = true; }; + + auto type_first = traffic_simulator::job::Type::LINEAR_VELOCITY; + auto type_second = traffic_simulator::job::Type::LINEAR_ACCELERATION; + auto first_event = traffic_simulator::job::Event::PRE_UPDATE; + auto second_event = traffic_simulator::job::Event::POST_UPDATE; + auto is_exclusive = true; + + dummy.appendToJobList( + first_update_func, first_cleanup_func, type_first, is_exclusive, first_event); + dummy.appendToJobList( + second_update_func, second_cleanup_func, type_second, is_exclusive, second_event); + + double current_time = 0.0; + double step_time = 0.0; + dummy.onUpdate(current_time, step_time); + + EXPECT_TRUE(first_cleanup); + EXPECT_TRUE(first_update); + EXPECT_FALSE(second_cleanup); + EXPECT_FALSE(second_update); +} + +/** + * @note Test functionality used by other units; test post update execution + * correctness with a POST_UPDATE task type added to the task queue (can be done + * in one of the implemented pure virtual member functions) + * - the goal is to check whether the task has been executed. + */ +TEST_F(EntityBaseTest, onPostUpdate) +{ + bool first_cleanup = false; + bool first_update = false; + bool second_cleanup = false; + bool second_update = false; + + auto first_update_func = [&first_update](const double) { return first_update = true; }; + auto first_cleanup_func = [&first_cleanup]() { first_cleanup = true; }; + auto second_update_func = [&second_update](const double) { return second_update = true; }; + auto second_cleanup_func = [&second_cleanup]() { second_cleanup = true; }; + + auto type_first = traffic_simulator::job::Type::LINEAR_VELOCITY; + auto type_second = traffic_simulator::job::Type::LINEAR_ACCELERATION; + auto first_event = traffic_simulator::job::Event::PRE_UPDATE; + auto second_event = traffic_simulator::job::Event::POST_UPDATE; + auto is_exclusive = true; + + dummy.appendToJobList( + first_update_func, first_cleanup_func, type_first, is_exclusive, first_event); + dummy.appendToJobList( + second_update_func, second_cleanup_func, type_second, is_exclusive, second_event); + + double current_time = 0.0; + double step_time = 0.0; + dummy.onPostUpdate(current_time, step_time); + + EXPECT_FALSE(first_cleanup); + EXPECT_FALSE(first_update); + EXPECT_TRUE(second_cleanup); + EXPECT_TRUE(second_update); +} + +/** + * @note Test basic functionality; test dynamic constraints resetting correctness + * - the goal is to check whether the dynamic constraints of the object are set + * to the same values as the ones returned by getDefaultDynamicConstraints. + */ +TEST_F(EntityBaseTest, resetDynamicConstraints) +{ + auto default_constraints = dummy.getDefaultDynamicConstraints(); + dummy.resetDynamicConstraints(); + auto current_constraints = dummy.getDynamicConstraints(); + + EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); +} + +/** + * @note Test basic functionality; test wrapper function with absolute target. + */ +TEST_F(EntityBaseTest, requestLaneChange_absoluteTarget) +{ + traffic_simulator::lane_change::AbsoluteTarget target(id, 1.0); + + traffic_simulator::lane_change::TrajectoryShape trajectory_shape = + traffic_simulator::lane_change::TrajectoryShape::LINEAR; + + traffic_simulator::lane_change::Constraint constraint( + traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, + traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT); + + dummy_base->requestLaneChange(target, trajectory_shape, constraint); + auto result_param = dummy.getLaneChangeParameter(); + + EXPECT_LANE_CHANGE_ABSOLUTE_TARGET_EQ(result_param.target, target); + EXPECT_EQ(result_param.trajectory_shape, trajectory_shape); + EXPECT_LANE_CHANGE_CONSTRAINT_EQ(result_param.constraint, constraint); +} + +/** + * @note Test basic functionality; test wrapper function with valid relative target. + */ +TEST_F(EntityBaseTest, requestLaneChange_relativeTarget) +{ + const lanelet::Id target_id = 34468; + const std::string target_name = "target_name"; + auto target_pose = makeCanonicalizedLaneletPose(hdmap_utils, target_id, 5.0); + auto target_status = makeCanonicalizedEntityStatus(hdmap_utils, target_pose, bbox); + + const double target_offset = 1.0; + traffic_simulator::lane_change::RelativeTarget target( + target_name, traffic_simulator::lane_change::Direction::STRAIGHT, 3, target_offset); + + traffic_simulator::lane_change::TrajectoryShape trajectory_shape = + traffic_simulator::lane_change::TrajectoryShape::LINEAR; + + traffic_simulator::lane_change::Constraint constraint( + traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, + traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT); + + std::unordered_map other_status; + other_status.emplace(target_name, target_status); + + dummy_base->setOtherStatus(other_status); + + dummy_base->requestLaneChange(target, trajectory_shape, constraint); + auto result_param = dummy.getLaneChangeParameter(); + + traffic_simulator::lane_change::AbsoluteTarget ref_target(target_id, target_offset); + + EXPECT_LANE_CHANGE_ABSOLUTE_TARGET_EQ(result_param.target, ref_target); + EXPECT_EQ(result_param.trajectory_shape, trajectory_shape); + EXPECT_LANE_CHANGE_CONSTRAINT_EQ(result_param.constraint, constraint); +} + +/** + * @note Test basic functionality; test wrapper function with invalid relative target lanelet pose. + */ +TEST_F(EntityBaseTest, requestLaneChange_relativeTargetLaneletPose) +{ + const std::string target_name = "target_name"; + geometry_msgs::msg::Pose target_pose; + target_pose.position = makePoint(3810.0, 73745.0); // outside of road + + auto target_status = makeCanonicalizedEntityStatus(hdmap_utils, target_pose, bbox); + + const double target_offset = 1.0; + traffic_simulator::lane_change::RelativeTarget target( + target_name, traffic_simulator::lane_change::Direction::STRAIGHT, 3, target_offset); + + traffic_simulator::lane_change::TrajectoryShape trajectory_shape = + traffic_simulator::lane_change::TrajectoryShape::LINEAR; + + traffic_simulator::lane_change::Constraint constraint( + traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, + traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT); + + std::unordered_map other_status; + other_status.emplace(target_name, target_status); + + dummy_base->setOtherStatus(other_status); + + EXPECT_THROW(dummy_base->requestLaneChange(target, trajectory_shape, constraint);, common::Error); +} + +/** + * @note Test basic functionality; test wrapper function with invalid relative target name. + */ +TEST_F(EntityBaseTest, requestLaneChange_relativeTargetName) +{ + const lanelet::Id target_id = 34468; + const std::string target_name = "target_name"; + auto target_pose = makeCanonicalizedLaneletPose(hdmap_utils, target_id, 5.0); + auto target_status = makeCanonicalizedEntityStatus(hdmap_utils, target_pose, bbox); + + const double target_offset = 1.0; + traffic_simulator::lane_change::RelativeTarget target( + target_name + "_wrong", traffic_simulator::lane_change::Direction::STRAIGHT, 3, target_offset); + + traffic_simulator::lane_change::TrajectoryShape trajectory_shape = + traffic_simulator::lane_change::TrajectoryShape::LINEAR; + + traffic_simulator::lane_change::Constraint constraint( + traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, + traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT); + + std::unordered_map other_status; + other_status.emplace(target_name, target_status); + + dummy_base->setOtherStatus(other_status); + EXPECT_THROW(dummy_base->requestLaneChange(target, trajectory_shape, constraint);, common::Error); +} + +/** + * @note Test basic functionality; test wrapper function with invalid relative target lane change + * - the goal is to request a lane change in the location where the lane change is impossible. + */ +TEST_F(EntityBaseTest, requestLaneChange_relativeTargetInvalid) +{ + const lanelet::Id target_id = 34468; + const std::string target_name = "target_name"; + auto target_pose = makeCanonicalizedLaneletPose(hdmap_utils, target_id, 5.0); + auto target_status = makeCanonicalizedEntityStatus(hdmap_utils, target_pose, bbox); + + const double target_offset = 1.0; + traffic_simulator::lane_change::RelativeTarget target( + target_name, traffic_simulator::lane_change::Direction::RIGHT, + std::numeric_limits::max(), target_offset); + + traffic_simulator::lane_change::TrajectoryShape trajectory_shape = + traffic_simulator::lane_change::TrajectoryShape::LINEAR; + + traffic_simulator::lane_change::Constraint constraint( + traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, + traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT); + + std::unordered_map other_status; + other_status.emplace(target_name, target_status); + + dummy_base->setOtherStatus(other_status); + EXPECT_THROW(dummy_base->requestLaneChange(target, trajectory_shape, constraint), common::Error); +} + +/** + * @note Test basic functionality; test setting dynamic constraints correctness with a sample dynamic constraints. + */ +TEST_F(EntityBaseTest, setDynamicConstraints) +{ + traffic_simulator_msgs::msg::DynamicConstraints custom_constraints{}; + + custom_constraints.max_speed = 5.0; + custom_constraints.max_acceleration = 7.0; + custom_constraints.max_deceleration = 11.0; + custom_constraints.max_acceleration_rate = 13.0; + custom_constraints.max_deceleration_rate = 17.0; + dummy.setDynamicConstraints(custom_constraints); + auto result_constraints = dummy.getDynamicConstraints(); + + EXPECT_DYNAMIC_CONSTRAINTS_EQ(custom_constraints, result_constraints); +} + +/** + * @note Test basic functionality; test setting other Entities status correctness + * with a sample other entities status unordered map. + */ +TEST_F(EntityBaseTest, setOtherStatus) +{ + const std::string name0 = "other_entity0", name1 = "other_entity1"; + + std::unordered_map other_status; + { + auto pose = makeCanonicalizedLaneletPose(hdmap_utils, id, 5.0); + auto status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, 0.0, name0); + other_status.emplace(name0, status); + } + { + geometry_msgs::msg::Pose pose; + pose.position = makePoint(3810.0, 73745.0); // outside of road + auto status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, 0.0, name1); + other_status.emplace(name1, status); + } + + dummy_base->setOtherStatus(other_status); + + const auto & result_status = dummy.getOtherStatus(); + + EXPECT_EQ(other_status.size(), result_status.size()); + EXPECT_EQ( + static_cast(other_status.at(name0)), + static_cast(result_status.at(name0))); + EXPECT_EQ( + static_cast(other_status.at(name1)), + static_cast(result_status.at(name1))); +} + +/** + * @note Test basic functionality; test setting a status correctness with a sample status. + */ +TEST_F(EntityBaseTest, setStatus) +{ + dummy.setEntityType(traffic_simulator_msgs::msg::EntityType::VEHICLE); + + auto new_type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + auto new_subtype = traffic_simulator_msgs::msg::EntitySubtype::CAR; + double new_time = 1.0; + const std::string new_name = "dummy_entity_new"; + traffic_simulator_msgs::msg::BoundingBox new_bounding_box; + new_bounding_box.center.x = 2.0; + new_bounding_box.dimensions.x = 3.0; + new_bounding_box.dimensions.y = 1.5; + traffic_simulator_msgs::msg::ActionStatus new_action_status; + new_action_status.twist.linear.x = 100.0; + new_action_status.linear_jerk = 20.0; + new_action_status.current_action = "new_current_status"; + geometry_msgs::msg::Pose new_pose; + new_pose.position = makePoint(3810.0, 73745.0); // outside of road + bool new_lanelet_pose_valid = false; + + traffic_simulator_msgs::msg::EntityStatus new_status; + { + new_status.type.type = new_type; + new_status.subtype.value = new_subtype; + new_status.time = new_time; + new_status.name = new_name; + new_status.bounding_box = new_bounding_box; + new_status.action_status = new_action_status; + new_status.pose = new_pose; + new_status.lanelet_pose_valid = new_lanelet_pose_valid; + } + + auto ref_status = static_cast(dummy_base->getStatus()); + { + ref_status.time = new_time; + ref_status.action_status = [&]() -> traffic_simulator_msgs::msg::ActionStatus { + auto tmp_action = ref_status.action_status.current_action; + auto ret_status = new_action_status; + ret_status.current_action = tmp_action; + return ret_status; + }(); + ref_status.pose = new_pose; + ref_status.lanelet_pose = traffic_simulator_msgs::msg::LaneletPose(); + ref_status.lanelet_pose_valid = new_lanelet_pose_valid; + } + + dummy_base->setStatus( + traffic_simulator::entity_status::CanonicalizedEntityStatus(new_status, hdmap_utils)); + + EXPECT_EQ( + static_cast(dummy_base->getStatus()), + static_cast(ref_status)); +} + +/** + * @note Test function behavior when called with any argument - the goal is to test error throwing. + */ +TEST_F(EntityBaseTest, requestFollowTrajectory) +{ + auto ptr = std::make_shared(); + EXPECT_THROW(dummy.requestFollowTrajectory(ptr), common::Error); +} + +/** + * @note Test function behavior when called with any argument - the goal is to test error throwing. + */ +TEST_F(EntityBaseTest, requestWalkStraight) +{ + EXPECT_THROW(dummy.requestWalkStraight(), common::Error); +} + +/** + * @note test basic functionality; test updating stand still duration + * when NPC logic is started and velocity is greater than 0. + */ +TEST_F(EntityBaseTest, updateStandStillDuration_startedMoving) +{ + dummy.startNpcLogic(); + dummy.setLinearVelocity(3.0); + + EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); +} + +/** + * @note Test basic functionality; test updating stand still duration + * when NPC logic is not started. + */ +TEST_F(EntityBaseTest, updateStandStillDuration_notStarted) +{ + dummy.setLinearVelocity(3.0); + EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); + + dummy.setLinearVelocity(0.0); + EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); +} + +/** + * @note Test basic functionality; test updating traveled distance correctness + * with NPC logic started and velocity greater than 0. + */ +TEST_F(EntityBaseTest, updateTraveledDistance_startedMoving) +{ + double velocity = 3.0; + double step_time = 0.1; + dummy.startNpcLogic(); + dummy.setLinearVelocity(velocity); + + EXPECT_EQ(1.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); + EXPECT_EQ(2.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); + EXPECT_EQ(3.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); + EXPECT_EQ(4.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); +} + +/** + * @note Test basic functionality; test updating traveled distance correctness with NPC not started. + */ +TEST_F(EntityBaseTest, updateTraveledDistance_notStarted) +{ + double velocity = 3.0; + double step_time = 0.1; + dummy.setLinearVelocity(velocity); + EXPECT_EQ(0.0, dummy.updateTraveledDistance(step_time)); + + dummy.setLinearVelocity(0.0); + EXPECT_EQ(0.0, dummy.updateTraveledDistance(step_time)); +} + +/** + * @note Test basic functionality; test stopping correctness - the goal + * is to check whether the entity status is changed to stopped (no velocity etc.). + */ +TEST_F(EntityBaseTest, stopAtCurrentPosition) +{ + double velocity = 3.0; + dummy.setLinearVelocity(velocity); + auto curr_twist = dummy.getCurrentTwist(); + EXPECT_EQ(curr_twist.linear.x, velocity); + + dummy.stopAtCurrentPosition(); + curr_twist = dummy.getCurrentTwist(); + EXPECT_EQ(curr_twist.linear.x, 0.0); +} + +/** + * @note Test functionality used by other units; test lanelet pose obtaining + * with a matching distance smaller than a distance from an entity to the lanelet + * (both crosswalk and road) and status_.type.type = PEDESTRIAN. + */ +TEST(EntityBase, getLaneletPose_notOnRoadAndCrosswalkPedestrian) +{ + auto hdmap_utils = makeHdMapUtilsSharedPointer(); + auto bbox = makeBoundingBox(); + + geometry_msgs::msg::Pose pose; + pose.position.x = 3810.0; + pose.position.y = 73745.0; + + auto status_base = makeEntityStatus(hdmap_utils, pose, bbox); + status_base.type.type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; + traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); + + DummyEntity dummy("dummy_entity", status, hdmap_utils); + dummy.setEntityType(status_base.type.type); + + auto lanelet_pose = dummy.getLaneletPose(5.0); + EXPECT_FALSE(lanelet_pose); +} + +/** + * @note Test functionality used by other units; test lanelet pose obtaining + * with a matching distance greater than a distance from an entity to the lanelet + * (both crosswalk and road) and status_.type.type = PEDESTRIAN. + */ +TEST(EntityBase, getLaneletPose_onRoadAndCrosswalkPedestrian) +{ + auto hdmap_utils = makeHdMapUtilsSharedPointer(); + auto bbox = makeBoundingBox(); + + geometry_msgs::msg::Pose pose; + pose.position.x = 3766.1; + pose.position.y = 73738.2; + pose.orientation = makeQuaternionFromYaw((30.0) * M_PI / 180.0); + + auto status_base = makeEntityStatus(hdmap_utils, pose, bbox); + status_base.type.type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; + traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); + + DummyEntity dummy("dummy_entity", status, hdmap_utils); + dummy.setEntityType(status_base.type.type); + + auto lanelet_pose = dummy.getLaneletPose(1.0); + EXPECT_TRUE(lanelet_pose); +} + +/** + * @note Test functionality used by other units; test lanelet pose obtaining + * with a matching distance greater than a distance from an entity to the crosswalk lanelet, + * but smaller than to the road lanelet and status_.type.type = PEDESTRIAN. + */ +TEST(EntityBase, getLaneletPose_onCrosswalkNotOnRoadPedestrian) +{ + auto hdmap_utils = makeHdMapUtilsSharedPointer(); + auto bbox = makeBoundingBox(); + + geometry_msgs::msg::Pose pose; + pose.position.x = 3764.5; + pose.position.y = 73737.5; + pose.orientation = makeQuaternionFromYaw((30.0) * M_PI / 180.0); + + auto status_base = makeEntityStatus(hdmap_utils, pose, bbox); + status_base.type.type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; + traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); + + DummyEntity dummy("dummy_entity", status, hdmap_utils); + dummy.setEntityType(status_base.type.type); + + auto lanelet_pose = dummy.getLaneletPose(1.0); + EXPECT_TRUE(lanelet_pose); +} + +/** + * @note Test functionality used by other units; test lanelet pose obtaining + * with a matching distance smaller than a distance from an entity to the lanelet + * (both crosswalk and road) and status_.type.type != PEDESTRIAN. + */ +TEST(EntityBase, getLaneletPose_notOnRoadAndCrosswalkNotPedestrian) +{ + auto hdmap_utils = makeHdMapUtilsSharedPointer(); + auto bbox = makeBoundingBox(); + + geometry_msgs::msg::Pose pose; + pose.position.x = 3810.0; + pose.position.y = 73745.0; + + auto status_base = makeEntityStatus(hdmap_utils, pose, bbox); + status_base.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); + + DummyEntity dummy("dummy_entity", status, hdmap_utils); + dummy.setEntityType(status_base.type.type); + + auto lanelet_pose = dummy.getLaneletPose(5.0); + EXPECT_FALSE(lanelet_pose); +} + +/** + * @note Test functionality used by other units; test lanelet pose obtaining + * with a matching distance greater than a distance from an entity to the lanelet + * (both crosswalk and road) and status_.type.type != PEDESTRIAN. + */ +TEST(EntityBase, getLaneletPose_onRoadAndCrosswalkNotPedestrian) +{ + auto hdmap_utils = makeHdMapUtilsSharedPointer(); + auto bbox = makeBoundingBox(); + + geometry_msgs::msg::Pose pose; + pose.position.x = 3766.1; + pose.position.y = 73738.2; + pose.orientation = makeQuaternionFromYaw((120.0) * M_PI / 180.0); + + auto status_base = makeEntityStatus(hdmap_utils, pose, bbox); + status_base.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); + + DummyEntity dummy("dummy_entity", status, hdmap_utils); + dummy.setEntityType(status_base.type.type); + + auto lanelet_pose = dummy.getLaneletPose(1.0); + EXPECT_TRUE(lanelet_pose); +} + +/** + * @note Test functionality used by other units; test lanelet pose obtaining + * with a matching distance greater than a distance from an entity to the crosswalk lanelet, + * but smaller than to the road lanelet and status_.type.type != PEDESTRIAN. + */ +TEST(EntityBase, getLaneletPose_onCrosswalkNotOnRoadNotPedestrian) +{ + auto hdmap_utils = makeHdMapUtilsSharedPointer(); + auto bbox = makeBoundingBox(); + + geometry_msgs::msg::Pose pose; + pose.position.x = 3764.5; + pose.position.y = 73737.5; + pose.orientation = makeQuaternionFromYaw((120.0) * M_PI / 180.0); + + auto status_base = makeEntityStatus(hdmap_utils, pose, bbox); + status_base.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); + + DummyEntity dummy("dummy_entity", status, hdmap_utils); + dummy.setEntityType(status_base.type.type); + + auto lanelet_pose = dummy.getLaneletPose(1.0); + EXPECT_FALSE(lanelet_pose); +} + +/** + * @note Test functionality used by other units; test relative pose calculations + * correctness with a transformation argument passed. + */ +TEST_F(EntityBaseTest, getMapPoseFromRelativePose_relative) +{ + const double s = 5.0; + constexpr double eps = 0.1; + + geometry_msgs::msg::Pose relative_pose; + relative_pose.position.x = s; + + auto result_pose = dummy.getMapPoseFromRelativePose(relative_pose); + + auto ref_pose = makeCanonicalizedLaneletPose(hdmap_utils, id, s); + EXPECT_POSE_NEAR(result_pose, static_cast(ref_pose), eps); +} diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp new file mode 100644 index 00000000000..319ae27c793 --- /dev/null +++ b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp @@ -0,0 +1,211 @@ +// 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 + +#include +#include +#include +#include + +#include "../catalogs.hpp" +#include "../expect_eq_macros.hpp" +#include "../helper_functions.hpp" + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +class MiscObjectEntityTest : public testing::Test +{ +protected: + MiscObjectEntityTest() : hdmap_utils_ptr(makeHdMapUtilsSharedPointer()), entity_name("blob") {} + + std::shared_ptr hdmap_utils_ptr; + const std::string entity_name; +}; +/** + * @note Test basic functionality. Test current action obtaining when NPC logic is not started. + */ +TEST_F(MiscObjectEntityTest, getCurrentAction_npcNotStarted) +{ + auto non_canonicalized_status = makeEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), 0.0, + entity_name); + non_canonicalized_status.action_status.current_action = "current_action_name"; + + const auto blob = traffic_simulator::entity::MiscObjectEntity( + entity_name, + traffic_simulator::entity_status::CanonicalizedEntityStatus( + non_canonicalized_status, hdmap_utils_ptr), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}); + + EXPECT_FALSE(blob.isNpcLogicStarted()); + EXPECT_EQ(blob.getCurrentAction(), "waiting"); +} + +/** + * @note Test function behavior when absolute speed change is requested - the goal is to test throwing error. + */ +TEST_F(MiscObjectEntityTest, requestSpeedChange_absolute) +{ + EXPECT_THROW( + traffic_simulator::entity::MiscObjectEntity( + entity_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), + 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .requestSpeedChange(10.0, false), + common::SemanticError); +} + +/** + * @note Test function behavior when relative speed change is requested - the goal is to test throwing error. + */ +TEST_F(MiscObjectEntityTest, requestSpeedChange_relative) +{ + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659); + auto bbox = makeBoundingBox(); + + auto blob = traffic_simulator::entity::MiscObjectEntity( + entity_name, makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}); + + std::unordered_map + others; + others.emplace( + "bob_entity", makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, 17.0, "bob")); + blob.setOtherStatus(others); + + EXPECT_THROW( + blob.requestSpeedChange( + traffic_simulator::speed_change::RelativeTargetSpeed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, 3.0), + true), + common::SemanticError); +} + +/** + * @note Test function behavior when relative speed change with transition type is requested + * - the goal is to test throwing error. + */ +TEST_F(MiscObjectEntityTest, requestSpeedChange_absoluteTransition) +{ + EXPECT_THROW( + traffic_simulator::entity::MiscObjectEntity( + entity_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), + 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .requestSpeedChange( + 10.0, traffic_simulator::speed_change::Transition::AUTO, + traffic_simulator::speed_change::Constraint( + traffic_simulator::speed_change::Constraint::Type::NONE, 0.0), + false), + common::SemanticError); +} + +/** + * @note Test function behavior when route assigning is requested with lanelet pose + * - the goal is to test throwing error. + */ +TEST_F(MiscObjectEntityTest, requestAssignRoute_laneletPose) +{ + EXPECT_THROW( + traffic_simulator::entity::MiscObjectEntity( + entity_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), + 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .requestAssignRoute(std::vector{ + makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660)}), + common::SemanticError); +} + +/** + * @note Test function behavior when route assigning is requested with pose + * - the goal is to test throwing error. + */ +TEST_F(MiscObjectEntityTest, requestAssignRoute_pose) +{ + EXPECT_THROW( + traffic_simulator::entity::MiscObjectEntity( + entity_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), + 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .requestAssignRoute( + std::vector{geometry_msgs::build() + .position(makePoint(3759.34, 73791.38)) + .orientation(makeQuaternionFromYaw(0.0))}), + common::SemanticError); +} + +/** + * @note Test function behavior when position acquiring is requested with lanelet pose + * - the goal is to test throwing error. + */ +TEST_F(MiscObjectEntityTest, requestAcquirePosition_laneletPose) +{ + EXPECT_THROW( + traffic_simulator::entity::MiscObjectEntity( + entity_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), + 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .requestAcquirePosition(makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660)), + common::SemanticError); +} + +/** + * @note Test function behavior when position acquiring is requested with pose + * - the goal is to test throwing error. + */ +TEST_F(MiscObjectEntityTest, requestAcquirePosition_pose) +{ + EXPECT_THROW( + traffic_simulator::entity::MiscObjectEntity( + entity_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), + 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .requestAcquirePosition(geometry_msgs::build() + .position(makePoint(3759.34, 73791.38)) + .orientation(makeQuaternionFromYaw(0.0))), + common::SemanticError); +} + +/** + * @note Test function behavior when called with any argument - the goal is to test error throwing. + */ +TEST_F(MiscObjectEntityTest, getRouteLanelets) +{ + EXPECT_THROW( + traffic_simulator::entity::MiscObjectEntity( + entity_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), + 0.0, entity_name), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .getRouteLanelets(100.0), + common::SemanticError); +} \ No newline at end of file diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp new file mode 100644 index 00000000000..92c40578582 --- /dev/null +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -0,0 +1,133 @@ +// 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 TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ +#define TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ + +#include +#include +#include +#include + +#include "catalogs.hpp" +#include "expect_eq_macros.hpp" + +auto makeHdMapUtilsSharedPointer() -> std::shared_ptr +{ + std::string path = + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; + geographic_msgs::msg::GeoPoint origin; + origin.latitude = 35.9037067912303; + origin.longitude = 139.9337945139059; + return std::make_shared(path, origin); +} + +auto makeCanonicalizedLaneletPose( + std::shared_ptr hdmap_utils, const lanelet::Id id = 120659, + const double s = 0.0, const double offset = 0.0) + -> traffic_simulator::lanelet_pose::CanonicalizedLaneletPose +{ + return traffic_simulator::lanelet_pose::CanonicalizedLaneletPose( + traffic_simulator::helper::constructLaneletPose(id, s, offset), hdmap_utils); +} + +auto makeBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs::msg::BoundingBox +{ + traffic_simulator_msgs::msg::BoundingBox bbox; + bbox.center.x = 1.0; + bbox.center.y = center_y; + bbox.dimensions.x = 4.0; + bbox.dimensions.y = 2.0; + bbox.dimensions.z = 1.5; + return bbox; +} + +auto makeEntityStatus( + std::shared_ptr hdmap_utils, + traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, + traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, + const std::string name = "dummy_entity") -> traffic_simulator::EntityStatus +{ + traffic_simulator::EntityStatus entity_status; + entity_status.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + entity_status.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; + entity_status.time = 0.0; + entity_status.name = name; + entity_status.bounding_box = bbox; + geometry_msgs::msg::Twist twist; + entity_status.action_status = + traffic_simulator::helper::constructActionStatus(speed, 0.0, 0.0, 0.0); + entity_status.lanelet_pose_valid = true; + entity_status.lanelet_pose = static_cast(pose); + entity_status.pose = hdmap_utils->toMapPose(entity_status.lanelet_pose).pose; + return entity_status; +} + +auto makeEntityStatus( + std::shared_ptr /* hdmap_utils */, geometry_msgs::msg::Pose pose, + traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, + const std::string name = "dummy_entity") -> traffic_simulator::EntityStatus +{ + traffic_simulator::EntityStatus entity_status; + entity_status.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + entity_status.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; + entity_status.time = 0.0; + entity_status.name = name; + entity_status.bounding_box = bbox; + geometry_msgs::msg::Twist twist; + entity_status.action_status = + traffic_simulator::helper::constructActionStatus(speed, 0.0, 0.0, 0.0); + entity_status.lanelet_pose_valid = false; + entity_status.pose = pose; + return entity_status; +} + +auto makeCanonicalizedEntityStatus( + std::shared_ptr hdmap_utils, + traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, + traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, + const std::string name = "dummy_entity") + -> traffic_simulator::entity_status::CanonicalizedEntityStatus +{ + return traffic_simulator::entity_status::CanonicalizedEntityStatus( + makeEntityStatus(hdmap_utils, pose, bbox, speed, name), hdmap_utils); +} + +auto makeCanonicalizedEntityStatus( + std::shared_ptr hdmap_utils, geometry_msgs::msg::Pose pose, + traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, + const std::string name = "dummy_entity") + -> traffic_simulator::entity_status::CanonicalizedEntityStatus +{ + return traffic_simulator::entity_status::CanonicalizedEntityStatus( + makeEntityStatus(hdmap_utils, pose, bbox, speed, name), hdmap_utils); +} + +auto makePoint(const double x, const double y, const double z = 0.0) -> geometry_msgs::msg::Point +{ + geometry_msgs::msg::Point point; + point.x = x; + point.y = y; + point.z = z; + return point; +} + +auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion +{ + geometry_msgs::msg::Vector3 v; + v.z = yaw; + return quaternion_operation::convertEulerAngleToQuaternion(v); +} + +#endif // TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ \ No newline at end of file From f37be0d2835ae4d987246426f290b4ec04a71819 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 11 Jun 2024 14:02:34 +0200 Subject: [PATCH 055/118] make it work; some tests have been deleted --- ...st_entity_base_with_misc_object_entity.cpp | 385 ++---------------- 1 file changed, 37 insertions(+), 348 deletions(-) diff --git a/simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp index 94a27027bd1..f831eac5777 100644 --- a/simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp @@ -29,10 +29,10 @@ int main(int argc, char ** argv) return RUN_ALL_TESTS(); } -class EntityBaseTest : public testing::Test +class EntityBaseWithMiscObjectTest : public testing::Test { protected: - EntityBaseTest() + EntityBaseWithMiscObjectTest() : id(120659), hdmap_utils(makeHdMapUtilsSharedPointer()), pose(makeCanonicalizedLaneletPose(hdmap_utils, id)), @@ -55,7 +55,7 @@ class EntityBaseTest : public testing::Test /** * @note Test basic functionality; test whether the function does nothing. */ -TEST_F(EntityBaseTest, appendDebugMarker) +TEST_F(EntityBaseWithMiscObjectTest, appendDebugMarker) { visualization_msgs::msg::MarkerArray markers{}; @@ -83,7 +83,7 @@ TEST_F(EntityBaseTest, appendDebugMarker) /** * @note Test basic functionality; test whether the function throws an error. */ -TEST_F(EntityBaseTest, asFieldOperatorApplication) +TEST_F(EntityBaseWithMiscObjectTest, asFieldOperatorApplication) { EXPECT_THROW(dummy.asFieldOperatorApplication(), common::Error); } @@ -91,7 +91,7 @@ TEST_F(EntityBaseTest, asFieldOperatorApplication) /** * @note Test functionality used by other units; test correctness of 2d polygon calculations. */ -TEST_F(EntityBaseTest, get2DPolygon) +TEST_F(EntityBaseWithMiscObjectTest, get2DPolygon) { const auto polygon = dummy.get2DPolygon(); @@ -114,7 +114,7 @@ TEST_F(EntityBaseTest, get2DPolygon) /** * @note Test basic functionality; test whether the NPC logic is started correctly. */ -TEST_F(EntityBaseTest, startNpcLogic) +TEST_F(EntityBaseWithMiscObjectTest, startNpcLogic) { EXPECT_FALSE(dummy.isNpcLogicStarted()); dummy.startNpcLogic(0.0); @@ -125,7 +125,7 @@ TEST_F(EntityBaseTest, startNpcLogic) * @note Test basic functionality; test activating an out of range job with * an entity that has a positive speed and a speed range specified in the job = [0, 0] */ -TEST_F(EntityBaseTest, activateOutOfRangeJob_speed) +TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_speed) { double min_velocity = 0.0; double max_velocity = 0.0; @@ -139,7 +139,7 @@ TEST_F(EntityBaseTest, activateOutOfRangeJob_speed) min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); double current_time = 0.0; double step_time = 0.0; - EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); + EXPECT_NO_THROW(dummy.EntityBase::onUpdate(current_time, step_time)); EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); } @@ -148,7 +148,7 @@ TEST_F(EntityBaseTest, activateOutOfRangeJob_speed) * with an entity that has a positive acceleration * and an acceleration range specified in the job = [0, 0]. */ -TEST_F(EntityBaseTest, activateOutOfRangeJob_acceleration) +TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_acceleration) { double min_velocity = -100.0; double max_velocity = 100.0; @@ -162,7 +162,7 @@ TEST_F(EntityBaseTest, activateOutOfRangeJob_acceleration) min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); double current_time = 0.0; double step_time = 0.0; - EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); + EXPECT_NO_THROW(dummy.EntityBase::onUpdate(current_time, step_time)); EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); } @@ -171,7 +171,7 @@ TEST_F(EntityBaseTest, activateOutOfRangeJob_acceleration) * with an entity that has a positive jerk * and a jerk range specified in the job = [0, 0]. */ -TEST_F(EntityBaseTest, activateOutOfRangeJob_jerk) +TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_jerk) { double min_velocity = -100.0; double max_velocity = 100.0; @@ -185,164 +185,14 @@ TEST_F(EntityBaseTest, activateOutOfRangeJob_jerk) min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); double current_time = 0.0; double step_time = 0.0; - EXPECT_NO_THROW(dummy.onUpdate(current_time, step_time)); + EXPECT_NO_THROW(dummy.EntityBase::onUpdate(current_time, step_time)); EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); } -/** - * @note Test functionality used by other units; test update execution correctness - * with a PRE_UPDATE task type added to the task queue (can be done in one of - * the implemented pure virtual member functions) - the goal is to check whether - * the task has been executed and whether the status and speed planner have been updated. - */ -TEST_F(EntityBaseTest, onUpdate) -{ - bool first_cleanup = false; - bool first_update = false; - bool second_cleanup = false; - bool second_update = false; - - auto first_update_func = [&first_update](const double) { return first_update = true; }; - auto first_cleanup_func = [&first_cleanup]() { first_cleanup = true; }; - auto second_update_func = [&second_update](const double) { return second_update = true; }; - auto second_cleanup_func = [&second_cleanup]() { second_cleanup = true; }; - - auto type_first = traffic_simulator::job::Type::LINEAR_VELOCITY; - auto type_second = traffic_simulator::job::Type::LINEAR_ACCELERATION; - auto first_event = traffic_simulator::job::Event::PRE_UPDATE; - auto second_event = traffic_simulator::job::Event::POST_UPDATE; - auto is_exclusive = true; - - dummy.appendToJobList( - first_update_func, first_cleanup_func, type_first, is_exclusive, first_event); - dummy.appendToJobList( - second_update_func, second_cleanup_func, type_second, is_exclusive, second_event); - - double current_time = 0.0; - double step_time = 0.0; - dummy.onUpdate(current_time, step_time); - - EXPECT_TRUE(first_cleanup); - EXPECT_TRUE(first_update); - EXPECT_FALSE(second_cleanup); - EXPECT_FALSE(second_update); -} - -/** - * @note Test functionality used by other units; test post update execution - * correctness with a POST_UPDATE task type added to the task queue (can be done - * in one of the implemented pure virtual member functions) - * - the goal is to check whether the task has been executed. - */ -TEST_F(EntityBaseTest, onPostUpdate) -{ - bool first_cleanup = false; - bool first_update = false; - bool second_cleanup = false; - bool second_update = false; - - auto first_update_func = [&first_update](const double) { return first_update = true; }; - auto first_cleanup_func = [&first_cleanup]() { first_cleanup = true; }; - auto second_update_func = [&second_update](const double) { return second_update = true; }; - auto second_cleanup_func = [&second_cleanup]() { second_cleanup = true; }; - - auto type_first = traffic_simulator::job::Type::LINEAR_VELOCITY; - auto type_second = traffic_simulator::job::Type::LINEAR_ACCELERATION; - auto first_event = traffic_simulator::job::Event::PRE_UPDATE; - auto second_event = traffic_simulator::job::Event::POST_UPDATE; - auto is_exclusive = true; - - dummy.appendToJobList( - first_update_func, first_cleanup_func, type_first, is_exclusive, first_event); - dummy.appendToJobList( - second_update_func, second_cleanup_func, type_second, is_exclusive, second_event); - - double current_time = 0.0; - double step_time = 0.0; - dummy.onPostUpdate(current_time, step_time); - - EXPECT_FALSE(first_cleanup); - EXPECT_FALSE(first_update); - EXPECT_TRUE(second_cleanup); - EXPECT_TRUE(second_update); -} - -/** - * @note Test basic functionality; test dynamic constraints resetting correctness - * - the goal is to check whether the dynamic constraints of the object are set - * to the same values as the ones returned by getDefaultDynamicConstraints. - */ -TEST_F(EntityBaseTest, resetDynamicConstraints) -{ - auto default_constraints = dummy.getDefaultDynamicConstraints(); - dummy.resetDynamicConstraints(); - auto current_constraints = dummy.getDynamicConstraints(); - - EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); -} - -/** - * @note Test basic functionality; test wrapper function with absolute target. - */ -TEST_F(EntityBaseTest, requestLaneChange_absoluteTarget) -{ - traffic_simulator::lane_change::AbsoluteTarget target(id, 1.0); - - traffic_simulator::lane_change::TrajectoryShape trajectory_shape = - traffic_simulator::lane_change::TrajectoryShape::LINEAR; - - traffic_simulator::lane_change::Constraint constraint( - traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, - traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT); - - dummy_base->requestLaneChange(target, trajectory_shape, constraint); - auto result_param = dummy.getLaneChangeParameter(); - - EXPECT_LANE_CHANGE_ABSOLUTE_TARGET_EQ(result_param.target, target); - EXPECT_EQ(result_param.trajectory_shape, trajectory_shape); - EXPECT_LANE_CHANGE_CONSTRAINT_EQ(result_param.constraint, constraint); -} - -/** - * @note Test basic functionality; test wrapper function with valid relative target. - */ -TEST_F(EntityBaseTest, requestLaneChange_relativeTarget) -{ - const lanelet::Id target_id = 34468; - const std::string target_name = "target_name"; - auto target_pose = makeCanonicalizedLaneletPose(hdmap_utils, target_id, 5.0); - auto target_status = makeCanonicalizedEntityStatus(hdmap_utils, target_pose, bbox); - - const double target_offset = 1.0; - traffic_simulator::lane_change::RelativeTarget target( - target_name, traffic_simulator::lane_change::Direction::STRAIGHT, 3, target_offset); - - traffic_simulator::lane_change::TrajectoryShape trajectory_shape = - traffic_simulator::lane_change::TrajectoryShape::LINEAR; - - traffic_simulator::lane_change::Constraint constraint( - traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, - traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT); - - std::unordered_map other_status; - other_status.emplace(target_name, target_status); - - dummy_base->setOtherStatus(other_status); - - dummy_base->requestLaneChange(target, trajectory_shape, constraint); - auto result_param = dummy.getLaneChangeParameter(); - - traffic_simulator::lane_change::AbsoluteTarget ref_target(target_id, target_offset); - - EXPECT_LANE_CHANGE_ABSOLUTE_TARGET_EQ(result_param.target, ref_target); - EXPECT_EQ(result_param.trajectory_shape, trajectory_shape); - EXPECT_LANE_CHANGE_CONSTRAINT_EQ(result_param.constraint, constraint); -} - /** * @note Test basic functionality; test wrapper function with invalid relative target lanelet pose. */ -TEST_F(EntityBaseTest, requestLaneChange_relativeTargetLaneletPose) +TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetLaneletPose) { const std::string target_name = "target_name"; geometry_msgs::msg::Pose target_pose; @@ -372,7 +222,7 @@ TEST_F(EntityBaseTest, requestLaneChange_relativeTargetLaneletPose) /** * @note Test basic functionality; test wrapper function with invalid relative target name. */ -TEST_F(EntityBaseTest, requestLaneChange_relativeTargetName) +TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetName) { const lanelet::Id target_id = 34468; const std::string target_name = "target_name"; @@ -401,7 +251,7 @@ TEST_F(EntityBaseTest, requestLaneChange_relativeTargetName) * @note Test basic functionality; test wrapper function with invalid relative target lane change * - the goal is to request a lane change in the location where the lane change is impossible. */ -TEST_F(EntityBaseTest, requestLaneChange_relativeTargetInvalid) +TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetInvalid) { const lanelet::Id target_id = 34468; const std::string target_name = "target_name"; @@ -427,119 +277,10 @@ TEST_F(EntityBaseTest, requestLaneChange_relativeTargetInvalid) EXPECT_THROW(dummy_base->requestLaneChange(target, trajectory_shape, constraint), common::Error); } -/** - * @note Test basic functionality; test setting dynamic constraints correctness with a sample dynamic constraints. - */ -TEST_F(EntityBaseTest, setDynamicConstraints) -{ - traffic_simulator_msgs::msg::DynamicConstraints custom_constraints{}; - - custom_constraints.max_speed = 5.0; - custom_constraints.max_acceleration = 7.0; - custom_constraints.max_deceleration = 11.0; - custom_constraints.max_acceleration_rate = 13.0; - custom_constraints.max_deceleration_rate = 17.0; - dummy.setDynamicConstraints(custom_constraints); - auto result_constraints = dummy.getDynamicConstraints(); - - EXPECT_DYNAMIC_CONSTRAINTS_EQ(custom_constraints, result_constraints); -} - -/** - * @note Test basic functionality; test setting other Entities status correctness - * with a sample other entities status unordered map. - */ -TEST_F(EntityBaseTest, setOtherStatus) -{ - const std::string name0 = "other_entity0", name1 = "other_entity1"; - - std::unordered_map other_status; - { - auto pose = makeCanonicalizedLaneletPose(hdmap_utils, id, 5.0); - auto status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, 0.0, name0); - other_status.emplace(name0, status); - } - { - geometry_msgs::msg::Pose pose; - pose.position = makePoint(3810.0, 73745.0); // outside of road - auto status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, 0.0, name1); - other_status.emplace(name1, status); - } - - dummy_base->setOtherStatus(other_status); - - const auto & result_status = dummy.getOtherStatus(); - - EXPECT_EQ(other_status.size(), result_status.size()); - EXPECT_EQ( - static_cast(other_status.at(name0)), - static_cast(result_status.at(name0))); - EXPECT_EQ( - static_cast(other_status.at(name1)), - static_cast(result_status.at(name1))); -} - -/** - * @note Test basic functionality; test setting a status correctness with a sample status. - */ -TEST_F(EntityBaseTest, setStatus) -{ - dummy.setEntityType(traffic_simulator_msgs::msg::EntityType::VEHICLE); - - auto new_type = traffic_simulator_msgs::msg::EntityType::VEHICLE; - auto new_subtype = traffic_simulator_msgs::msg::EntitySubtype::CAR; - double new_time = 1.0; - const std::string new_name = "dummy_entity_new"; - traffic_simulator_msgs::msg::BoundingBox new_bounding_box; - new_bounding_box.center.x = 2.0; - new_bounding_box.dimensions.x = 3.0; - new_bounding_box.dimensions.y = 1.5; - traffic_simulator_msgs::msg::ActionStatus new_action_status; - new_action_status.twist.linear.x = 100.0; - new_action_status.linear_jerk = 20.0; - new_action_status.current_action = "new_current_status"; - geometry_msgs::msg::Pose new_pose; - new_pose.position = makePoint(3810.0, 73745.0); // outside of road - bool new_lanelet_pose_valid = false; - - traffic_simulator_msgs::msg::EntityStatus new_status; - { - new_status.type.type = new_type; - new_status.subtype.value = new_subtype; - new_status.time = new_time; - new_status.name = new_name; - new_status.bounding_box = new_bounding_box; - new_status.action_status = new_action_status; - new_status.pose = new_pose; - new_status.lanelet_pose_valid = new_lanelet_pose_valid; - } - - auto ref_status = static_cast(dummy_base->getStatus()); - { - ref_status.time = new_time; - ref_status.action_status = [&]() -> traffic_simulator_msgs::msg::ActionStatus { - auto tmp_action = ref_status.action_status.current_action; - auto ret_status = new_action_status; - ret_status.current_action = tmp_action; - return ret_status; - }(); - ref_status.pose = new_pose; - ref_status.lanelet_pose = traffic_simulator_msgs::msg::LaneletPose(); - ref_status.lanelet_pose_valid = new_lanelet_pose_valid; - } - - dummy_base->setStatus( - traffic_simulator::entity_status::CanonicalizedEntityStatus(new_status, hdmap_utils)); - - EXPECT_EQ( - static_cast(dummy_base->getStatus()), - static_cast(ref_status)); -} - /** * @note Test function behavior when called with any argument - the goal is to test error throwing. */ -TEST_F(EntityBaseTest, requestFollowTrajectory) +TEST_F(EntityBaseWithMiscObjectTest, requestFollowTrajectory) { auto ptr = std::make_shared(); EXPECT_THROW(dummy.requestFollowTrajectory(ptr), common::Error); @@ -548,7 +289,7 @@ TEST_F(EntityBaseTest, requestFollowTrajectory) /** * @note Test function behavior when called with any argument - the goal is to test error throwing. */ -TEST_F(EntityBaseTest, requestWalkStraight) +TEST_F(EntityBaseWithMiscObjectTest, requestWalkStraight) { EXPECT_THROW(dummy.requestWalkStraight(), common::Error); } @@ -557,9 +298,9 @@ TEST_F(EntityBaseTest, requestWalkStraight) * @note test basic functionality; test updating stand still duration * when NPC logic is started and velocity is greater than 0. */ -TEST_F(EntityBaseTest, updateStandStillDuration_startedMoving) +TEST_F(EntityBaseWithMiscObjectTest, updateStandStillDuration_startedMoving) { - dummy.startNpcLogic(); + dummy.startNpcLogic(0.0); dummy.setLinearVelocity(3.0); EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); @@ -569,7 +310,7 @@ TEST_F(EntityBaseTest, updateStandStillDuration_startedMoving) * @note Test basic functionality; test updating stand still duration * when NPC logic is not started. */ -TEST_F(EntityBaseTest, updateStandStillDuration_notStarted) +TEST_F(EntityBaseWithMiscObjectTest, updateStandStillDuration_notStarted) { dummy.setLinearVelocity(3.0); EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); @@ -582,11 +323,11 @@ TEST_F(EntityBaseTest, updateStandStillDuration_notStarted) * @note Test basic functionality; test updating traveled distance correctness * with NPC logic started and velocity greater than 0. */ -TEST_F(EntityBaseTest, updateTraveledDistance_startedMoving) +TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_startedMoving) { double velocity = 3.0; double step_time = 0.1; - dummy.startNpcLogic(); + dummy.startNpcLogic(0.0); dummy.setLinearVelocity(velocity); EXPECT_EQ(1.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); @@ -598,7 +339,7 @@ TEST_F(EntityBaseTest, updateTraveledDistance_startedMoving) /** * @note Test basic functionality; test updating traveled distance correctness with NPC not started. */ -TEST_F(EntityBaseTest, updateTraveledDistance_notStarted) +TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_notStarted) { double velocity = 3.0; double step_time = 0.1; @@ -613,7 +354,7 @@ TEST_F(EntityBaseTest, updateTraveledDistance_notStarted) * @note Test basic functionality; test stopping correctness - the goal * is to check whether the entity status is changed to stopped (no velocity etc.). */ -TEST_F(EntityBaseTest, stopAtCurrentPosition) +TEST_F(EntityBaseWithMiscObjectTest, stopAtCurrentPosition) { double velocity = 3.0; dummy.setLinearVelocity(velocity); @@ -630,7 +371,7 @@ TEST_F(EntityBaseTest, stopAtCurrentPosition) * with a matching distance smaller than a distance from an entity to the lanelet * (both crosswalk and road) and status_.type.type = PEDESTRIAN. */ -TEST(EntityBase, getLaneletPose_notOnRoadAndCrosswalkPedestrian) +TEST(EntityBaseWithMiscObject, getLaneletPose_notOnRoadAndCrosswalkPedestrian) { auto hdmap_utils = makeHdMapUtilsSharedPointer(); auto bbox = makeBoundingBox(); @@ -643,71 +384,19 @@ TEST(EntityBase, getLaneletPose_notOnRoadAndCrosswalkPedestrian) status_base.type.type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); - DummyEntity dummy("dummy_entity", status, hdmap_utils); - dummy.setEntityType(status_base.type.type); + auto dummy = traffic_simulator::entity::MiscObjectEntity( + "dummy_entity", status, hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}); auto lanelet_pose = dummy.getLaneletPose(5.0); EXPECT_FALSE(lanelet_pose); } -/** - * @note Test functionality used by other units; test lanelet pose obtaining - * with a matching distance greater than a distance from an entity to the lanelet - * (both crosswalk and road) and status_.type.type = PEDESTRIAN. - */ -TEST(EntityBase, getLaneletPose_onRoadAndCrosswalkPedestrian) -{ - auto hdmap_utils = makeHdMapUtilsSharedPointer(); - auto bbox = makeBoundingBox(); - - geometry_msgs::msg::Pose pose; - pose.position.x = 3766.1; - pose.position.y = 73738.2; - pose.orientation = makeQuaternionFromYaw((30.0) * M_PI / 180.0); - - auto status_base = makeEntityStatus(hdmap_utils, pose, bbox); - status_base.type.type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; - traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); - - DummyEntity dummy("dummy_entity", status, hdmap_utils); - dummy.setEntityType(status_base.type.type); - - auto lanelet_pose = dummy.getLaneletPose(1.0); - EXPECT_TRUE(lanelet_pose); -} - -/** - * @note Test functionality used by other units; test lanelet pose obtaining - * with a matching distance greater than a distance from an entity to the crosswalk lanelet, - * but smaller than to the road lanelet and status_.type.type = PEDESTRIAN. - */ -TEST(EntityBase, getLaneletPose_onCrosswalkNotOnRoadPedestrian) -{ - auto hdmap_utils = makeHdMapUtilsSharedPointer(); - auto bbox = makeBoundingBox(); - - geometry_msgs::msg::Pose pose; - pose.position.x = 3764.5; - pose.position.y = 73737.5; - pose.orientation = makeQuaternionFromYaw((30.0) * M_PI / 180.0); - - auto status_base = makeEntityStatus(hdmap_utils, pose, bbox); - status_base.type.type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; - traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); - - DummyEntity dummy("dummy_entity", status, hdmap_utils); - dummy.setEntityType(status_base.type.type); - - auto lanelet_pose = dummy.getLaneletPose(1.0); - EXPECT_TRUE(lanelet_pose); -} - /** * @note Test functionality used by other units; test lanelet pose obtaining * with a matching distance smaller than a distance from an entity to the lanelet * (both crosswalk and road) and status_.type.type != PEDESTRIAN. */ -TEST(EntityBase, getLaneletPose_notOnRoadAndCrosswalkNotPedestrian) +TEST(EntityBaseWithMiscObject, getLaneletPose_notOnRoadAndCrosswalkNotPedestrian) { auto hdmap_utils = makeHdMapUtilsSharedPointer(); auto bbox = makeBoundingBox(); @@ -720,8 +409,8 @@ TEST(EntityBase, getLaneletPose_notOnRoadAndCrosswalkNotPedestrian) status_base.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); - DummyEntity dummy("dummy_entity", status, hdmap_utils); - dummy.setEntityType(status_base.type.type); + auto dummy = traffic_simulator::entity::MiscObjectEntity( + "dummy_entity", status, hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}); auto lanelet_pose = dummy.getLaneletPose(5.0); EXPECT_FALSE(lanelet_pose); @@ -732,7 +421,7 @@ TEST(EntityBase, getLaneletPose_notOnRoadAndCrosswalkNotPedestrian) * with a matching distance greater than a distance from an entity to the lanelet * (both crosswalk and road) and status_.type.type != PEDESTRIAN. */ -TEST(EntityBase, getLaneletPose_onRoadAndCrosswalkNotPedestrian) +TEST(EntityBaseWithMiscObject, getLaneletPose_onRoadAndCrosswalkNotPedestrian) { auto hdmap_utils = makeHdMapUtilsSharedPointer(); auto bbox = makeBoundingBox(); @@ -746,8 +435,8 @@ TEST(EntityBase, getLaneletPose_onRoadAndCrosswalkNotPedestrian) status_base.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); - DummyEntity dummy("dummy_entity", status, hdmap_utils); - dummy.setEntityType(status_base.type.type); + auto dummy = traffic_simulator::entity::MiscObjectEntity( + "dummy_entity", status, hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}); auto lanelet_pose = dummy.getLaneletPose(1.0); EXPECT_TRUE(lanelet_pose); @@ -758,7 +447,7 @@ TEST(EntityBase, getLaneletPose_onRoadAndCrosswalkNotPedestrian) * with a matching distance greater than a distance from an entity to the crosswalk lanelet, * but smaller than to the road lanelet and status_.type.type != PEDESTRIAN. */ -TEST(EntityBase, getLaneletPose_onCrosswalkNotOnRoadNotPedestrian) +TEST(EntityBaseWithMiscObject, getLaneletPose_onCrosswalkNotOnRoadNotPedestrian) { auto hdmap_utils = makeHdMapUtilsSharedPointer(); auto bbox = makeBoundingBox(); @@ -772,8 +461,8 @@ TEST(EntityBase, getLaneletPose_onCrosswalkNotOnRoadNotPedestrian) status_base.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); - DummyEntity dummy("dummy_entity", status, hdmap_utils); - dummy.setEntityType(status_base.type.type); + auto dummy = traffic_simulator::entity::MiscObjectEntity( + "dummy_entity", status, hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}); auto lanelet_pose = dummy.getLaneletPose(1.0); EXPECT_FALSE(lanelet_pose); @@ -783,7 +472,7 @@ TEST(EntityBase, getLaneletPose_onCrosswalkNotOnRoadNotPedestrian) * @note Test functionality used by other units; test relative pose calculations * correctness with a transformation argument passed. */ -TEST_F(EntityBaseTest, getMapPoseFromRelativePose_relative) +TEST_F(EntityBaseWithMiscObjectTest, getMapPoseFromRelativePose_relative) { const double s = 5.0; constexpr double eps = 0.1; From f21000f15eebed04b0af4eab35b0c38c90d3d0ad Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 11 Jun 2024 15:30:52 +0200 Subject: [PATCH 056/118] remove variables used only once --- ...st_entity_base_with_misc_object_entity.cpp | 311 +++++++----------- .../src/entity/test_misc_object_entity.cpp | 33 +- .../test/src/hdmap_utils/test_hdmap_utils.cpp | 35 +- .../test/src/helper_functions.hpp | 83 +++-- 4 files changed, 186 insertions(+), 276 deletions(-) diff --git a/simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp index f831eac5777..63445af5146 100644 --- a/simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "../catalogs.hpp" #include "../expect_eq_macros.hpp" @@ -65,13 +66,14 @@ TEST_F(EntityBaseWithMiscObjectTest, appendDebugMarker) EXPECT_EQ(markers.markers.size(), markers_copy.markers.size()); } - visualization_msgs::msg::Marker marker{}; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.type = visualization_msgs::msg::Marker::ARROW; - marker.pose.position.x = 10.0; - marker.pose.position.y = 10.0; - - markers.markers.push_back(marker); + markers.markers.push_back([] { + auto marker = visualization_msgs::msg::Marker{}; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.pose.position.x = 10.0; + marker.pose.position.y = 10.0; + return marker; + }()); { auto markers_copy = markers; @@ -95,13 +97,9 @@ TEST_F(EntityBaseWithMiscObjectTest, get2DPolygon) { const auto polygon = dummy.get2DPolygon(); - std::vector ref_poly{}; - - ref_poly.push_back(makePoint(-1.0, -1.0)); - ref_poly.push_back(makePoint(-1.0, 1.0)); - ref_poly.push_back(makePoint(3.0, 1.0)); - ref_poly.push_back(makePoint(3.0, -1.0)); - ref_poly.push_back(ref_poly.front()); + std::vector ref_poly{ + makePoint(-1.0, -1.0), makePoint(-1.0, 1.0), makePoint(3.0, 1.0), makePoint(3.0, -1.0), + makePoint(-1.0, -1.0)}; EXPECT_EQ(polygon.size(), ref_poly.size()); EXPECT_POINT_EQ(polygon.at(0), ref_poly.at(0)); @@ -127,18 +125,12 @@ TEST_F(EntityBaseWithMiscObjectTest, startNpcLogic) */ TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_speed) { - double min_velocity = 0.0; - double max_velocity = 0.0; - double min_acceleration = -100.0; - double max_acceleration = 100.0; - double min_jerk = -100.0; - double max_jerk = 100.0; - double velocity = 1.0; + constexpr double velocity = 1.0; dummy.setLinearVelocity(velocity); - dummy.activateOutOfRangeJob( - min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); - double current_time = 0.0; - double step_time = 0.0; + dummy.activateOutOfRangeJob(0.0, 0.0, -100.0, 100.0, -100.0, 100.0); + + constexpr double current_time = 0.0; + constexpr double step_time = 0.0; EXPECT_NO_THROW(dummy.EntityBase::onUpdate(current_time, step_time)); EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); } @@ -150,18 +142,12 @@ TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_speed) */ TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_acceleration) { - double min_velocity = -100.0; - double max_velocity = 100.0; - double min_acceleration = 0.0; - double max_acceleration = 0.0; - double min_jerk = -100.0; - double max_jerk = 100.0; - double acceleration = 1.0; + constexpr double acceleration = 1.0; dummy.setLinearAcceleration(acceleration); - dummy.activateOutOfRangeJob( - min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); - double current_time = 0.0; - double step_time = 0.0; + dummy.activateOutOfRangeJob(-100.0, 100.0, 0.0, 0.0, -100.0, 100.0); + + constexpr double current_time = 0.0; + constexpr double step_time = 0.0; EXPECT_NO_THROW(dummy.EntityBase::onUpdate(current_time, step_time)); EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); } @@ -173,18 +159,12 @@ TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_acceleration) */ TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_jerk) { - double min_velocity = -100.0; - double max_velocity = 100.0; - double min_acceleration = -100.0; - double max_acceleration = 100.0; - double min_jerk = 0.0; - double max_jerk = 0.0; - double jerk = 1.0; + constexpr double jerk = 1.0; dummy.setLinearJerk(jerk); - dummy.activateOutOfRangeJob( - min_velocity, max_velocity, min_acceleration, max_acceleration, min_jerk, max_jerk); - double current_time = 0.0; - double step_time = 0.0; + dummy.activateOutOfRangeJob(-100.0, 100.0, -100.0, 100.0, 0.0, 0.0); + + constexpr double current_time = 0.0; + constexpr double step_time = 0.0; EXPECT_NO_THROW(dummy.EntityBase::onUpdate(current_time, step_time)); EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); } @@ -195,28 +175,23 @@ TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_jerk) TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetLaneletPose) { const std::string target_name = "target_name"; - geometry_msgs::msg::Pose target_pose; - target_pose.position = makePoint(3810.0, 73745.0); // outside of road - - auto target_status = makeCanonicalizedEntityStatus(hdmap_utils, target_pose, bbox); - const double target_offset = 1.0; - traffic_simulator::lane_change::RelativeTarget target( - target_name, traffic_simulator::lane_change::Direction::STRAIGHT, 3, target_offset); - - traffic_simulator::lane_change::TrajectoryShape trajectory_shape = - traffic_simulator::lane_change::TrajectoryShape::LINEAR; - - traffic_simulator::lane_change::Constraint constraint( - traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, - traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT); - - std::unordered_map other_status; - other_status.emplace(target_name, target_status); + auto other_status = + std::unordered_map{}; + other_status.emplace( + target_name, + makeCanonicalizedEntityStatus(hdmap_utils, makePose(makePoint(3810.0, 73745.0)), bbox)); dummy_base->setOtherStatus(other_status); - EXPECT_THROW(dummy_base->requestLaneChange(target, trajectory_shape, constraint);, common::Error); + EXPECT_THROW(dummy_base->requestLaneChange( + traffic_simulator::lane_change::RelativeTarget( + target_name, traffic_simulator::lane_change::Direction::STRAIGHT, 3, 1.0), + traffic_simulator::lane_change::TrajectoryShape::LINEAR, + traffic_simulator::lane_change::Constraint( + traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, + traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT)); + , common::Error); } /** @@ -224,27 +199,24 @@ TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetLaneletPose */ TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetName) { - const lanelet::Id target_id = 34468; const std::string target_name = "target_name"; - auto target_pose = makeCanonicalizedLaneletPose(hdmap_utils, target_id, 5.0); - auto target_status = makeCanonicalizedEntityStatus(hdmap_utils, target_pose, bbox); - - const double target_offset = 1.0; - traffic_simulator::lane_change::RelativeTarget target( - target_name + "_wrong", traffic_simulator::lane_change::Direction::STRAIGHT, 3, target_offset); - - traffic_simulator::lane_change::TrajectoryShape trajectory_shape = - traffic_simulator::lane_change::TrajectoryShape::LINEAR; - traffic_simulator::lane_change::Constraint constraint( - traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, - traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT); - - std::unordered_map other_status; - other_status.emplace(target_name, target_status); + auto other_status = + std::unordered_map{}; + other_status.emplace( + target_name, makeCanonicalizedEntityStatus( + hdmap_utils, makeCanonicalizedLaneletPose(hdmap_utils, 34468, 5.0), bbox)); dummy_base->setOtherStatus(other_status); - EXPECT_THROW(dummy_base->requestLaneChange(target, trajectory_shape, constraint);, common::Error); + EXPECT_THROW( + dummy_base->requestLaneChange( + traffic_simulator::lane_change::RelativeTarget( + target_name + "_wrong", traffic_simulator::lane_change::Direction::STRAIGHT, 3, 1.0), + traffic_simulator::lane_change::TrajectoryShape::LINEAR, + traffic_simulator::lane_change::Constraint( + traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, + traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT)), + common::Error); } /** @@ -253,28 +225,25 @@ TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetName) */ TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetInvalid) { - const lanelet::Id target_id = 34468; const std::string target_name = "target_name"; - auto target_pose = makeCanonicalizedLaneletPose(hdmap_utils, target_id, 5.0); - auto target_status = makeCanonicalizedEntityStatus(hdmap_utils, target_pose, bbox); - - const double target_offset = 1.0; - traffic_simulator::lane_change::RelativeTarget target( - target_name, traffic_simulator::lane_change::Direction::RIGHT, - std::numeric_limits::max(), target_offset); - - traffic_simulator::lane_change::TrajectoryShape trajectory_shape = - traffic_simulator::lane_change::TrajectoryShape::LINEAR; - traffic_simulator::lane_change::Constraint constraint( - traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, - traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT); - - std::unordered_map other_status; - other_status.emplace(target_name, target_status); + auto other_status = + std::unordered_map{}; + other_status.emplace( + target_name, makeCanonicalizedEntityStatus( + hdmap_utils, makeCanonicalizedLaneletPose(hdmap_utils, 34468, 5.0), bbox)); dummy_base->setOtherStatus(other_status); - EXPECT_THROW(dummy_base->requestLaneChange(target, trajectory_shape, constraint), common::Error); + EXPECT_THROW( + dummy_base->requestLaneChange( + traffic_simulator::lane_change::RelativeTarget( + target_name, traffic_simulator::lane_change::Direction::RIGHT, + std::numeric_limits::max(), 1.0), + traffic_simulator::lane_change::TrajectoryShape::LINEAR, + traffic_simulator::lane_change::Constraint( + traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, + traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT)), + common::Error); } /** @@ -282,8 +251,10 @@ TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetInvalid) */ TEST_F(EntityBaseWithMiscObjectTest, requestFollowTrajectory) { - auto ptr = std::make_shared(); - EXPECT_THROW(dummy.requestFollowTrajectory(ptr), common::Error); + EXPECT_THROW( + dummy.requestFollowTrajectory( + std::make_shared()), + common::Error); } /** @@ -325,8 +296,8 @@ TEST_F(EntityBaseWithMiscObjectTest, updateStandStillDuration_notStarted) */ TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_startedMoving) { - double velocity = 3.0; - double step_time = 0.1; + constexpr double velocity = 3.0; + constexpr double step_time = 0.1; dummy.startNpcLogic(0.0); dummy.setLinearVelocity(velocity); @@ -341,9 +312,8 @@ TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_startedMoving) */ TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_notStarted) { - double velocity = 3.0; - double step_time = 0.1; - dummy.setLinearVelocity(velocity); + constexpr double step_time = 0.1; + dummy.setLinearVelocity(3.0); EXPECT_EQ(0.0, dummy.updateTraveledDistance(step_time)); dummy.setLinearVelocity(0.0); @@ -356,39 +326,12 @@ TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_notStarted) */ TEST_F(EntityBaseWithMiscObjectTest, stopAtCurrentPosition) { - double velocity = 3.0; + constexpr double velocity = 3.0; dummy.setLinearVelocity(velocity); - auto curr_twist = dummy.getCurrentTwist(); - EXPECT_EQ(curr_twist.linear.x, velocity); + EXPECT_EQ(dummy.getCurrentTwist().linear.x, velocity); dummy.stopAtCurrentPosition(); - curr_twist = dummy.getCurrentTwist(); - EXPECT_EQ(curr_twist.linear.x, 0.0); -} - -/** - * @note Test functionality used by other units; test lanelet pose obtaining - * with a matching distance smaller than a distance from an entity to the lanelet - * (both crosswalk and road) and status_.type.type = PEDESTRIAN. - */ -TEST(EntityBaseWithMiscObject, getLaneletPose_notOnRoadAndCrosswalkPedestrian) -{ - auto hdmap_utils = makeHdMapUtilsSharedPointer(); - auto bbox = makeBoundingBox(); - - geometry_msgs::msg::Pose pose; - pose.position.x = 3810.0; - pose.position.y = 73745.0; - - auto status_base = makeEntityStatus(hdmap_utils, pose, bbox); - status_base.type.type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; - traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); - - auto dummy = traffic_simulator::entity::MiscObjectEntity( - "dummy_entity", status, hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}); - - auto lanelet_pose = dummy.getLaneletPose(5.0); - EXPECT_FALSE(lanelet_pose); + EXPECT_EQ(dummy.getCurrentTwist().linear.x, 0.0); } /** @@ -399,21 +342,17 @@ TEST(EntityBaseWithMiscObject, getLaneletPose_notOnRoadAndCrosswalkPedestrian) TEST(EntityBaseWithMiscObject, getLaneletPose_notOnRoadAndCrosswalkNotPedestrian) { auto hdmap_utils = makeHdMapUtilsSharedPointer(); - auto bbox = makeBoundingBox(); - - geometry_msgs::msg::Pose pose; - pose.position.x = 3810.0; - pose.position.y = 73745.0; - - auto status_base = makeEntityStatus(hdmap_utils, pose, bbox); - status_base.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; - traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); - auto dummy = traffic_simulator::entity::MiscObjectEntity( - "dummy_entity", status, hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}); - - auto lanelet_pose = dummy.getLaneletPose(5.0); - EXPECT_FALSE(lanelet_pose); + EXPECT_FALSE(traffic_simulator::entity::MiscObjectEntity( + "dummy_entity", + traffic_simulator::CanonicalizedEntityStatus( + makeEntityStatus( + hdmap_utils, makePose(makePoint(3810.0, 73745.0)), makeBoundingBox(), 0.0, + "dummy_entity", traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + hdmap_utils), + hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .getLaneletPose(5.0) + .has_value()); } /** @@ -424,22 +363,20 @@ TEST(EntityBaseWithMiscObject, getLaneletPose_notOnRoadAndCrosswalkNotPedestrian TEST(EntityBaseWithMiscObject, getLaneletPose_onRoadAndCrosswalkNotPedestrian) { auto hdmap_utils = makeHdMapUtilsSharedPointer(); - auto bbox = makeBoundingBox(); - - geometry_msgs::msg::Pose pose; - pose.position.x = 3766.1; - pose.position.y = 73738.2; - pose.orientation = makeQuaternionFromYaw((120.0) * M_PI / 180.0); - - auto status_base = makeEntityStatus(hdmap_utils, pose, bbox); - status_base.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; - traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); - auto dummy = traffic_simulator::entity::MiscObjectEntity( - "dummy_entity", status, hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}); - - auto lanelet_pose = dummy.getLaneletPose(1.0); - EXPECT_TRUE(lanelet_pose); + EXPECT_TRUE( + traffic_simulator::entity::MiscObjectEntity( + "dummy_entity", + traffic_simulator::CanonicalizedEntityStatus( + makeEntityStatus( + hdmap_utils, + makePose(makePoint(3766.1, 73738.2), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), + makeBoundingBox(), 0.0, "dummy_entity", + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + hdmap_utils), + hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .getLaneletPose(1.0) + .has_value()); } /** @@ -450,22 +387,20 @@ TEST(EntityBaseWithMiscObject, getLaneletPose_onRoadAndCrosswalkNotPedestrian) TEST(EntityBaseWithMiscObject, getLaneletPose_onCrosswalkNotOnRoadNotPedestrian) { auto hdmap_utils = makeHdMapUtilsSharedPointer(); - auto bbox = makeBoundingBox(); - - geometry_msgs::msg::Pose pose; - pose.position.x = 3764.5; - pose.position.y = 73737.5; - pose.orientation = makeQuaternionFromYaw((120.0) * M_PI / 180.0); - auto status_base = makeEntityStatus(hdmap_utils, pose, bbox); - status_base.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; - traffic_simulator::CanonicalizedEntityStatus status(status_base, hdmap_utils); - - auto dummy = traffic_simulator::entity::MiscObjectEntity( - "dummy_entity", status, hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}); - - auto lanelet_pose = dummy.getLaneletPose(1.0); - EXPECT_FALSE(lanelet_pose); + EXPECT_FALSE( + traffic_simulator::entity::MiscObjectEntity( + "dummy_entity", + traffic_simulator::CanonicalizedEntityStatus( + makeEntityStatus( + hdmap_utils, + makePose(makePoint(3764.5, 73737.5), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), + makeBoundingBox(), 0.0, "dummy_entity", + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + hdmap_utils), + hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .getLaneletPose(1.0) + .has_value()); } /** @@ -474,14 +409,8 @@ TEST(EntityBaseWithMiscObject, getLaneletPose_onCrosswalkNotOnRoadNotPedestrian) */ TEST_F(EntityBaseWithMiscObjectTest, getMapPoseFromRelativePose_relative) { - const double s = 5.0; - constexpr double eps = 0.1; - - geometry_msgs::msg::Pose relative_pose; - relative_pose.position.x = s; - - auto result_pose = dummy.getMapPoseFromRelativePose(relative_pose); - - auto ref_pose = makeCanonicalizedLaneletPose(hdmap_utils, id, s); - EXPECT_POSE_NEAR(result_pose, static_cast(ref_pose), eps); + constexpr double s = 5.0; + EXPECT_POSE_NEAR( + dummy.getMapPoseFromRelativePose(makePose(makePoint(s, 0.0))), + static_cast(makeCanonicalizedLaneletPose(hdmap_utils, id, s)), 0.1); } diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp index 319ae27c793..16e973e6102 100644 --- a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp @@ -44,7 +44,7 @@ TEST_F(MiscObjectEntityTest, getCurrentAction_npcNotStarted) { auto non_canonicalized_status = makeEntityStatus( hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), 0.0, - entity_name); + entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT); non_canonicalized_status.action_status.current_action = "current_action_name"; const auto blob = traffic_simulator::entity::MiscObjectEntity( @@ -67,7 +67,7 @@ TEST_F(MiscObjectEntityTest, requestSpeedChange_absolute) entity_name, makeCanonicalizedEntityStatus( hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name), + 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .requestSpeedChange(10.0, false), common::SemanticError); @@ -82,13 +82,18 @@ TEST_F(MiscObjectEntityTest, requestSpeedChange_relative) auto bbox = makeBoundingBox(); auto blob = traffic_simulator::entity::MiscObjectEntity( - entity_name, makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, 0.0, entity_name), + entity_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, pose, bbox, 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}); std::unordered_map others; others.emplace( - "bob_entity", makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, 17.0, "bob")); + "bob_entity", makeCanonicalizedEntityStatus( + hdmap_utils_ptr, pose, bbox, 17.0, "bob", + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT)); blob.setOtherStatus(others); EXPECT_THROW( @@ -110,7 +115,7 @@ TEST_F(MiscObjectEntityTest, requestSpeedChange_absoluteTransition) entity_name, makeCanonicalizedEntityStatus( hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name), + 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .requestSpeedChange( 10.0, traffic_simulator::speed_change::Transition::AUTO, @@ -131,7 +136,7 @@ TEST_F(MiscObjectEntityTest, requestAssignRoute_laneletPose) entity_name, makeCanonicalizedEntityStatus( hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name), + 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .requestAssignRoute(std::vector{ makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660)}), @@ -149,12 +154,10 @@ TEST_F(MiscObjectEntityTest, requestAssignRoute_pose) entity_name, makeCanonicalizedEntityStatus( hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name), + 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .requestAssignRoute( - std::vector{geometry_msgs::build() - .position(makePoint(3759.34, 73791.38)) - .orientation(makeQuaternionFromYaw(0.0))}), + std::vector{makePose(makePoint(3759.34, 73791.38))}), common::SemanticError); } @@ -169,7 +172,7 @@ TEST_F(MiscObjectEntityTest, requestAcquirePosition_laneletPose) entity_name, makeCanonicalizedEntityStatus( hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name), + 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .requestAcquirePosition(makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660)), common::SemanticError); @@ -186,11 +189,9 @@ TEST_F(MiscObjectEntityTest, requestAcquirePosition_pose) entity_name, makeCanonicalizedEntityStatus( hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name), + 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) - .requestAcquirePosition(geometry_msgs::build() - .position(makePoint(3759.34, 73791.38)) - .orientation(makeQuaternionFromYaw(0.0))), + .requestAcquirePosition(makePose(makePoint(3759.34, 73791.38))), common::SemanticError); } @@ -204,7 +205,7 @@ TEST_F(MiscObjectEntityTest, getRouteLanelets) entity_name, makeCanonicalizedEntityStatus( hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name), + 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getRouteLanelets(100.0), common::SemanticError); diff --git a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp index 834a0576386..77290fc9d05 100644 --- a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp +++ b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp @@ -21,40 +21,7 @@ #include #include "../expect_eq_macros.hpp" - -auto makePoint(const double x, const double y, const double z = 0.0) -> geometry_msgs::msg::Point -{ - return geometry_msgs::build().x(x).y(y).z(z); -} - -auto makeBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs::msg::BoundingBox -{ - return traffic_simulator_msgs::build() - .center(makePoint(1.0, center_y)) - .dimensions(geometry_msgs::build().x(4.0).y(2.0).z(1.5)); -} - -auto makePose( - geometry_msgs::msg::Point position, - geometry_msgs::msg::Quaternion orientation = geometry_msgs::msg::Quaternion()) - -> geometry_msgs::msg::Pose -{ - return geometry_msgs::build().position(position).orientation( - orientation); -} - -auto makeSmallBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs::msg::BoundingBox -{ - return traffic_simulator_msgs::build() - .center(makePoint(0.0, center_y)) - .dimensions(geometry_msgs::build().x(1.0).y(1.0).z(1.0)); -} - -auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion -{ - return quaternion_operation::convertEulerAngleToQuaternion( - geometry_msgs::build().x(0.0).y(0.0).z(yaw)); -} +#include "../helper_functions.hpp" int main(int argc, char ** argv) { diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index 92c40578582..eadf7a79d01 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -23,6 +23,40 @@ #include "catalogs.hpp" #include "expect_eq_macros.hpp" +auto makePoint(const double x, const double y, const double z = 0.0) -> geometry_msgs::msg::Point +{ + return geometry_msgs::build().x(x).y(y).z(z); +} + +auto makeBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs::msg::BoundingBox +{ + return traffic_simulator_msgs::build() + .center(makePoint(1.0, center_y)) + .dimensions(geometry_msgs::build().x(4.0).y(2.0).z(1.5)); +} + +auto makeSmallBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs::msg::BoundingBox +{ + return traffic_simulator_msgs::build() + .center(makePoint(0.0, center_y)) + .dimensions(geometry_msgs::build().x(1.0).y(1.0).z(1.0)); +} + +auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion +{ + return quaternion_operation::convertEulerAngleToQuaternion( + geometry_msgs::build().x(0.0).y(0.0).z(yaw)); +} + +auto makePose( + geometry_msgs::msg::Point position, + geometry_msgs::msg::Quaternion orientation = geometry_msgs::msg::Quaternion()) + -> geometry_msgs::msg::Pose +{ + return geometry_msgs::build().position(position).orientation( + orientation); +} + auto makeHdMapUtilsSharedPointer() -> std::shared_ptr { std::string path = @@ -42,25 +76,16 @@ auto makeCanonicalizedLaneletPose( traffic_simulator::helper::constructLaneletPose(id, s, offset), hdmap_utils); } -auto makeBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs::msg::BoundingBox -{ - traffic_simulator_msgs::msg::BoundingBox bbox; - bbox.center.x = 1.0; - bbox.center.y = center_y; - bbox.dimensions.x = 4.0; - bbox.dimensions.y = 2.0; - bbox.dimensions.z = 1.5; - return bbox; -} - auto makeEntityStatus( std::shared_ptr hdmap_utils, traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, - const std::string name = "dummy_entity") -> traffic_simulator::EntityStatus + const std::string name = "dummy_entity", + const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) + -> traffic_simulator::EntityStatus { traffic_simulator::EntityStatus entity_status; - entity_status.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + entity_status.type.type = type; entity_status.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; entity_status.time = 0.0; entity_status.name = name; @@ -77,10 +102,12 @@ auto makeEntityStatus( auto makeEntityStatus( std::shared_ptr /* hdmap_utils */, geometry_msgs::msg::Pose pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, - const std::string name = "dummy_entity") -> traffic_simulator::EntityStatus + const std::string name = "dummy_entity", + const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) + -> traffic_simulator::EntityStatus { traffic_simulator::EntityStatus entity_status; - entity_status.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + entity_status.type.type = type; entity_status.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; entity_status.time = 0.0; entity_status.name = name; @@ -97,37 +124,23 @@ auto makeCanonicalizedEntityStatus( std::shared_ptr hdmap_utils, traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, - const std::string name = "dummy_entity") + const std::string name = "dummy_entity", + const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::entity_status::CanonicalizedEntityStatus { return traffic_simulator::entity_status::CanonicalizedEntityStatus( - makeEntityStatus(hdmap_utils, pose, bbox, speed, name), hdmap_utils); + makeEntityStatus(hdmap_utils, pose, bbox, speed, name, type), hdmap_utils); } auto makeCanonicalizedEntityStatus( std::shared_ptr hdmap_utils, geometry_msgs::msg::Pose pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, - const std::string name = "dummy_entity") + const std::string name = "dummy_entity", + const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::entity_status::CanonicalizedEntityStatus { return traffic_simulator::entity_status::CanonicalizedEntityStatus( - makeEntityStatus(hdmap_utils, pose, bbox, speed, name), hdmap_utils); -} - -auto makePoint(const double x, const double y, const double z = 0.0) -> geometry_msgs::msg::Point -{ - geometry_msgs::msg::Point point; - point.x = x; - point.y = y; - point.z = z; - return point; -} - -auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion -{ - geometry_msgs::msg::Vector3 v; - v.z = yaw; - return quaternion_operation::convertEulerAngleToQuaternion(v); + makeEntityStatus(hdmap_utils, pose, bbox, speed, name, type), hdmap_utils); } #endif // TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ \ No newline at end of file From 7f8c4fe6f4f4d5564c356b68569398f0cff47e2a Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 11 Jun 2024 16:00:46 +0200 Subject: [PATCH 057/118] use builder --- .../test/src/helper_functions.hpp | 63 +++++++++---------- 1 file changed, 31 insertions(+), 32 deletions(-) diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index eadf7a79d01..123a8354c4f 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include "catalogs.hpp" #include "expect_eq_macros.hpp" @@ -59,12 +61,12 @@ auto makePose( auto makeHdMapUtilsSharedPointer() -> std::shared_ptr { - std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; - geographic_msgs::msg::GeoPoint origin; - origin.latitude = 35.9037067912303; - origin.longitude = 139.9337945139059; - return std::make_shared(path, origin); + return std::make_shared( + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.9037067912303) + .longitude(139.9337945139059) + .altitude(0.0)); } auto makeCanonicalizedLaneletPose( @@ -84,19 +86,17 @@ auto makeEntityStatus( const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::EntityStatus { - traffic_simulator::EntityStatus entity_status; - entity_status.type.type = type; - entity_status.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; - entity_status.time = 0.0; - entity_status.name = name; - entity_status.bounding_box = bbox; - geometry_msgs::msg::Twist twist; - entity_status.action_status = - traffic_simulator::helper::constructActionStatus(speed, 0.0, 0.0, 0.0); - entity_status.lanelet_pose_valid = true; - entity_status.lanelet_pose = static_cast(pose); - entity_status.pose = hdmap_utils->toMapPose(entity_status.lanelet_pose).pose; - return entity_status; + return traffic_simulator_msgs::build() + .type(traffic_simulator_msgs::build().type(type)) + .subtype(traffic_simulator_msgs::build().value( + traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN)) + .time(0.0) + .name(name) + .bounding_box(bbox) + .action_status(traffic_simulator::helper::constructActionStatus(speed, 0.0, 0.0, 0.0)) + .pose(hdmap_utils->toMapPose(static_cast(pose)).pose) + .lanelet_pose(static_cast(pose)) + .lanelet_pose_valid(true); } auto makeEntityStatus( @@ -106,18 +106,17 @@ auto makeEntityStatus( const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::EntityStatus { - traffic_simulator::EntityStatus entity_status; - entity_status.type.type = type; - entity_status.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; - entity_status.time = 0.0; - entity_status.name = name; - entity_status.bounding_box = bbox; - geometry_msgs::msg::Twist twist; - entity_status.action_status = - traffic_simulator::helper::constructActionStatus(speed, 0.0, 0.0, 0.0); - entity_status.lanelet_pose_valid = false; - entity_status.pose = pose; - return entity_status; + return traffic_simulator_msgs::build() + .type(traffic_simulator_msgs::build().type(type)) + .subtype(traffic_simulator_msgs::build().value( + traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN)) + .time(0.0) + .name(name) + .bounding_box(bbox) + .action_status(traffic_simulator::helper::constructActionStatus(speed, 0.0, 0.0, 0.0)) + .pose(pose) + .lanelet_pose(traffic_simulator_msgs::msg::LaneletPose{}) + .lanelet_pose_valid(false); } auto makeCanonicalizedEntityStatus( @@ -143,4 +142,4 @@ auto makeCanonicalizedEntityStatus( makeEntityStatus(hdmap_utils, pose, bbox, speed, name, type), hdmap_utils); } -#endif // TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ \ No newline at end of file +#endif // TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ From 1d7d8dcdb927416d2206381c2f3f55d657e5c999 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 11 Jun 2024 16:05:57 +0200 Subject: [PATCH 058/118] rename file definition --- simulation/traffic_simulator/test/src/helper_functions.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index 123a8354c4f..d5b58c8911f 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ -#define TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ +#ifndef TRAFFIC_SIMULATOR__TEST__HELPER_FUNCTIONS_HPP_ +#define TRAFFIC_SIMULATOR__TEST__HELPER_FUNCTIONS_HPP_ #include #include From 2809b7fb63959d4e12c9d76f4aa03e5ddecfd466 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 11 Jun 2024 16:19:40 +0200 Subject: [PATCH 059/118] add newline --- .../test/src/entity/test_misc_object_entity.cpp | 2 +- simulation/traffic_simulator/test/src/entity/txt | 0 2 files changed, 1 insertion(+), 1 deletion(-) create mode 100644 simulation/traffic_simulator/test/src/entity/txt diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp index 16e973e6102..ccd66f8d3bc 100644 --- a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp @@ -209,4 +209,4 @@ TEST_F(MiscObjectEntityTest, getRouteLanelets) hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getRouteLanelets(100.0), common::SemanticError); -} \ No newline at end of file +} diff --git a/simulation/traffic_simulator/test/src/entity/txt b/simulation/traffic_simulator/test/src/entity/txt new file mode 100644 index 00000000000..e69de29bb2d From 15481f4daa8ebb89ff627b923417ede12c437680 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 11 Jun 2024 16:20:37 +0200 Subject: [PATCH 060/118] clean up --- simulation/traffic_simulator/test/src/entity/txt | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 simulation/traffic_simulator/test/src/entity/txt diff --git a/simulation/traffic_simulator/test/src/entity/txt b/simulation/traffic_simulator/test/src/entity/txt deleted file mode 100644 index e69de29bb2d..00000000000 From 8b6d6d033f98f242d7b8db5f697429dd65afe41d Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Wed, 12 Jun 2024 17:42:39 +0900 Subject: [PATCH 061/118] fix --- .../src/traffic_source/define_traffic_source_delay.cpp | 1 + .../src/traffic_source/define_traffic_source_high_rate.cpp | 1 + .../src/traffic_source/define_traffic_source_large.cpp | 1 + .../src/traffic_source/define_traffic_source_mixed.cpp | 1 + .../src/traffic_source/define_traffic_source_multiple.cpp | 1 + .../src/traffic_source/define_traffic_source_outside_lane.cpp | 1 + .../src/traffic_source/define_traffic_source_pedestrian.cpp | 1 + .../src/traffic_source/define_traffic_source_vehicle.cpp | 1 + 8 files changed, 8 insertions(+) diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp index 1f9f6966f8f..431851f40f7 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace cpp_mock_scenarios { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp index 0cc96e172d7..5f2d5df8e2e 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace cpp_mock_scenarios { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp index 4f1fcc8b67d..49577ab9c9f 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace cpp_mock_scenarios { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp index b1169c8aef8..65e1aebc091 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace cpp_mock_scenarios { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp index 1c0cb7de4a1..94e993a6b5c 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace cpp_mock_scenarios { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp index aa77e88828f..d7b42cf1e8a 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace cpp_mock_scenarios { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp index 13237b0810b..300624a442a 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace cpp_mock_scenarios { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp index d8ecac1f33d..5e1ebfd69f7 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace cpp_mock_scenarios { From c6a10ea0d4d41b0fe36daa46e08707ea8914f52c Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 12 Jun 2024 12:31:11 +0200 Subject: [PATCH 062/118] merge tests into a singular file --- .../test/src/entity/CMakeLists.txt | 3 - ...st_entity_base_with_misc_object_entity.cpp | 416 ------------------ .../src/entity/test_misc_object_entity.cpp | 386 ++++++++++++++++ 3 files changed, 386 insertions(+), 419 deletions(-) delete mode 100644 simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp diff --git a/simulation/traffic_simulator/test/src/entity/CMakeLists.txt b/simulation/traffic_simulator/test/src/entity/CMakeLists.txt index 68de39065b2..0dcb3f4ca29 100644 --- a/simulation/traffic_simulator/test/src/entity/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/entity/CMakeLists.txt @@ -3,6 +3,3 @@ target_link_libraries(test_vehicle_entity traffic_simulator) ament_add_gtest(test_misc_object_entity test_misc_object_entity.cpp) target_link_libraries(test_misc_object_entity traffic_simulator) - -ament_add_gtest(test_entity_base_with_misc_object_entity test_entity_base_with_misc_object_entity.cpp) -target_link_libraries(test_entity_base_with_misc_object_entity traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp deleted file mode 100644 index 63445af5146..00000000000 --- a/simulation/traffic_simulator/test/src/entity/test_entity_base_with_misc_object_entity.cpp +++ /dev/null @@ -1,416 +0,0 @@ -// 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 - -#include -#include -#include -#include -#include - -#include "../catalogs.hpp" -#include "../expect_eq_macros.hpp" -#include "../helper_functions.hpp" - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -class EntityBaseWithMiscObjectTest : public testing::Test -{ -protected: - EntityBaseWithMiscObjectTest() - : id(120659), - hdmap_utils(makeHdMapUtilsSharedPointer()), - pose(makeCanonicalizedLaneletPose(hdmap_utils, id)), - bbox(makeBoundingBox()), - status(makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox)), - dummy("dummy_entity", status, hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}), - dummy_base(&dummy) - { - } - - const lanelet::Id id; - std::shared_ptr hdmap_utils; - traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose; - traffic_simulator_msgs::msg::BoundingBox bbox; - traffic_simulator::entity_status::CanonicalizedEntityStatus status; - traffic_simulator::entity::MiscObjectEntity dummy; - traffic_simulator::entity::EntityBase * dummy_base; -}; - -/** - * @note Test basic functionality; test whether the function does nothing. - */ -TEST_F(EntityBaseWithMiscObjectTest, appendDebugMarker) -{ - visualization_msgs::msg::MarkerArray markers{}; - - { - auto markers_copy = markers; - dummy.appendDebugMarker(markers); - EXPECT_EQ(markers.markers.size(), markers_copy.markers.size()); - } - - markers.markers.push_back([] { - auto marker = visualization_msgs::msg::Marker{}; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.type = visualization_msgs::msg::Marker::ARROW; - marker.pose.position.x = 10.0; - marker.pose.position.y = 10.0; - return marker; - }()); - - { - auto markers_copy = markers; - dummy.appendDebugMarker(markers); - EXPECT_EQ(markers.markers.size(), markers_copy.markers.size()); - } -} - -/** - * @note Test basic functionality; test whether the function throws an error. - */ -TEST_F(EntityBaseWithMiscObjectTest, asFieldOperatorApplication) -{ - EXPECT_THROW(dummy.asFieldOperatorApplication(), common::Error); -} - -/** - * @note Test functionality used by other units; test correctness of 2d polygon calculations. - */ -TEST_F(EntityBaseWithMiscObjectTest, get2DPolygon) -{ - const auto polygon = dummy.get2DPolygon(); - - std::vector ref_poly{ - makePoint(-1.0, -1.0), makePoint(-1.0, 1.0), makePoint(3.0, 1.0), makePoint(3.0, -1.0), - makePoint(-1.0, -1.0)}; - - EXPECT_EQ(polygon.size(), ref_poly.size()); - EXPECT_POINT_EQ(polygon.at(0), ref_poly.at(0)); - EXPECT_POINT_EQ(polygon.at(1), ref_poly.at(1)); - EXPECT_POINT_EQ(polygon.at(2), ref_poly.at(2)); - EXPECT_POINT_EQ(polygon.at(3), ref_poly.at(3)); - EXPECT_POINT_EQ(polygon.at(4), ref_poly.at(4)); -} - -/** - * @note Test basic functionality; test whether the NPC logic is started correctly. - */ -TEST_F(EntityBaseWithMiscObjectTest, startNpcLogic) -{ - EXPECT_FALSE(dummy.isNpcLogicStarted()); - dummy.startNpcLogic(0.0); - EXPECT_TRUE(dummy.isNpcLogicStarted()); -} - -/** - * @note Test basic functionality; test activating an out of range job with - * an entity that has a positive speed and a speed range specified in the job = [0, 0] - */ -TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_speed) -{ - constexpr double velocity = 1.0; - dummy.setLinearVelocity(velocity); - dummy.activateOutOfRangeJob(0.0, 0.0, -100.0, 100.0, -100.0, 100.0); - - constexpr double current_time = 0.0; - constexpr double step_time = 0.0; - EXPECT_NO_THROW(dummy.EntityBase::onUpdate(current_time, step_time)); - EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); -} - -/** - * @note Test basic functionality; test activating an out of range job - * with an entity that has a positive acceleration - * and an acceleration range specified in the job = [0, 0]. - */ -TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_acceleration) -{ - constexpr double acceleration = 1.0; - dummy.setLinearAcceleration(acceleration); - dummy.activateOutOfRangeJob(-100.0, 100.0, 0.0, 0.0, -100.0, 100.0); - - constexpr double current_time = 0.0; - constexpr double step_time = 0.0; - EXPECT_NO_THROW(dummy.EntityBase::onUpdate(current_time, step_time)); - EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); -} - -/** - * @note Test basic functionality; test activating an out of range job - * with an entity that has a positive jerk - * and a jerk range specified in the job = [0, 0]. - */ -TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_jerk) -{ - constexpr double jerk = 1.0; - dummy.setLinearJerk(jerk); - dummy.activateOutOfRangeJob(-100.0, 100.0, -100.0, 100.0, 0.0, 0.0); - - constexpr double current_time = 0.0; - constexpr double step_time = 0.0; - EXPECT_NO_THROW(dummy.EntityBase::onUpdate(current_time, step_time)); - EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); -} - -/** - * @note Test basic functionality; test wrapper function with invalid relative target lanelet pose. - */ -TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetLaneletPose) -{ - const std::string target_name = "target_name"; - - auto other_status = - std::unordered_map{}; - other_status.emplace( - target_name, - makeCanonicalizedEntityStatus(hdmap_utils, makePose(makePoint(3810.0, 73745.0)), bbox)); - - dummy_base->setOtherStatus(other_status); - - EXPECT_THROW(dummy_base->requestLaneChange( - traffic_simulator::lane_change::RelativeTarget( - target_name, traffic_simulator::lane_change::Direction::STRAIGHT, 3, 1.0), - traffic_simulator::lane_change::TrajectoryShape::LINEAR, - traffic_simulator::lane_change::Constraint( - traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, - traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT)); - , common::Error); -} - -/** - * @note Test basic functionality; test wrapper function with invalid relative target name. - */ -TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetName) -{ - const std::string target_name = "target_name"; - - auto other_status = - std::unordered_map{}; - other_status.emplace( - target_name, makeCanonicalizedEntityStatus( - hdmap_utils, makeCanonicalizedLaneletPose(hdmap_utils, 34468, 5.0), bbox)); - - dummy_base->setOtherStatus(other_status); - EXPECT_THROW( - dummy_base->requestLaneChange( - traffic_simulator::lane_change::RelativeTarget( - target_name + "_wrong", traffic_simulator::lane_change::Direction::STRAIGHT, 3, 1.0), - traffic_simulator::lane_change::TrajectoryShape::LINEAR, - traffic_simulator::lane_change::Constraint( - traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, - traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT)), - common::Error); -} - -/** - * @note Test basic functionality; test wrapper function with invalid relative target lane change - * - the goal is to request a lane change in the location where the lane change is impossible. - */ -TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetInvalid) -{ - const std::string target_name = "target_name"; - - auto other_status = - std::unordered_map{}; - other_status.emplace( - target_name, makeCanonicalizedEntityStatus( - hdmap_utils, makeCanonicalizedLaneletPose(hdmap_utils, 34468, 5.0), bbox)); - - dummy_base->setOtherStatus(other_status); - EXPECT_THROW( - dummy_base->requestLaneChange( - traffic_simulator::lane_change::RelativeTarget( - target_name, traffic_simulator::lane_change::Direction::RIGHT, - std::numeric_limits::max(), 1.0), - traffic_simulator::lane_change::TrajectoryShape::LINEAR, - traffic_simulator::lane_change::Constraint( - traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, - traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT)), - common::Error); -} - -/** - * @note Test function behavior when called with any argument - the goal is to test error throwing. - */ -TEST_F(EntityBaseWithMiscObjectTest, requestFollowTrajectory) -{ - EXPECT_THROW( - dummy.requestFollowTrajectory( - std::make_shared()), - common::Error); -} - -/** - * @note Test function behavior when called with any argument - the goal is to test error throwing. - */ -TEST_F(EntityBaseWithMiscObjectTest, requestWalkStraight) -{ - EXPECT_THROW(dummy.requestWalkStraight(), common::Error); -} - -/** - * @note test basic functionality; test updating stand still duration - * when NPC logic is started and velocity is greater than 0. - */ -TEST_F(EntityBaseWithMiscObjectTest, updateStandStillDuration_startedMoving) -{ - dummy.startNpcLogic(0.0); - dummy.setLinearVelocity(3.0); - - EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); -} - -/** - * @note Test basic functionality; test updating stand still duration - * when NPC logic is not started. - */ -TEST_F(EntityBaseWithMiscObjectTest, updateStandStillDuration_notStarted) -{ - dummy.setLinearVelocity(3.0); - EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); - - dummy.setLinearVelocity(0.0); - EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); -} - -/** - * @note Test basic functionality; test updating traveled distance correctness - * with NPC logic started and velocity greater than 0. - */ -TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_startedMoving) -{ - constexpr double velocity = 3.0; - constexpr double step_time = 0.1; - dummy.startNpcLogic(0.0); - dummy.setLinearVelocity(velocity); - - EXPECT_EQ(1.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); - EXPECT_EQ(2.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); - EXPECT_EQ(3.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); - EXPECT_EQ(4.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); -} - -/** - * @note Test basic functionality; test updating traveled distance correctness with NPC not started. - */ -TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_notStarted) -{ - constexpr double step_time = 0.1; - dummy.setLinearVelocity(3.0); - EXPECT_EQ(0.0, dummy.updateTraveledDistance(step_time)); - - dummy.setLinearVelocity(0.0); - EXPECT_EQ(0.0, dummy.updateTraveledDistance(step_time)); -} - -/** - * @note Test basic functionality; test stopping correctness - the goal - * is to check whether the entity status is changed to stopped (no velocity etc.). - */ -TEST_F(EntityBaseWithMiscObjectTest, stopAtCurrentPosition) -{ - constexpr double velocity = 3.0; - dummy.setLinearVelocity(velocity); - EXPECT_EQ(dummy.getCurrentTwist().linear.x, velocity); - - dummy.stopAtCurrentPosition(); - EXPECT_EQ(dummy.getCurrentTwist().linear.x, 0.0); -} - -/** - * @note Test functionality used by other units; test lanelet pose obtaining - * with a matching distance smaller than a distance from an entity to the lanelet - * (both crosswalk and road) and status_.type.type != PEDESTRIAN. - */ -TEST(EntityBaseWithMiscObject, getLaneletPose_notOnRoadAndCrosswalkNotPedestrian) -{ - auto hdmap_utils = makeHdMapUtilsSharedPointer(); - - EXPECT_FALSE(traffic_simulator::entity::MiscObjectEntity( - "dummy_entity", - traffic_simulator::CanonicalizedEntityStatus( - makeEntityStatus( - hdmap_utils, makePose(makePoint(3810.0, 73745.0)), makeBoundingBox(), 0.0, - "dummy_entity", traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), - hdmap_utils), - hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}) - .getLaneletPose(5.0) - .has_value()); -} - -/** - * @note Test functionality used by other units; test lanelet pose obtaining - * with a matching distance greater than a distance from an entity to the lanelet - * (both crosswalk and road) and status_.type.type != PEDESTRIAN. - */ -TEST(EntityBaseWithMiscObject, getLaneletPose_onRoadAndCrosswalkNotPedestrian) -{ - auto hdmap_utils = makeHdMapUtilsSharedPointer(); - - EXPECT_TRUE( - traffic_simulator::entity::MiscObjectEntity( - "dummy_entity", - traffic_simulator::CanonicalizedEntityStatus( - makeEntityStatus( - hdmap_utils, - makePose(makePoint(3766.1, 73738.2), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), - makeBoundingBox(), 0.0, "dummy_entity", - traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), - hdmap_utils), - hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}) - .getLaneletPose(1.0) - .has_value()); -} - -/** - * @note Test functionality used by other units; test lanelet pose obtaining - * with a matching distance greater than a distance from an entity to the crosswalk lanelet, - * but smaller than to the road lanelet and status_.type.type != PEDESTRIAN. - */ -TEST(EntityBaseWithMiscObject, getLaneletPose_onCrosswalkNotOnRoadNotPedestrian) -{ - auto hdmap_utils = makeHdMapUtilsSharedPointer(); - - EXPECT_FALSE( - traffic_simulator::entity::MiscObjectEntity( - "dummy_entity", - traffic_simulator::CanonicalizedEntityStatus( - makeEntityStatus( - hdmap_utils, - makePose(makePoint(3764.5, 73737.5), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), - makeBoundingBox(), 0.0, "dummy_entity", - traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), - hdmap_utils), - hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}) - .getLaneletPose(1.0) - .has_value()); -} - -/** - * @note Test functionality used by other units; test relative pose calculations - * correctness with a transformation argument passed. - */ -TEST_F(EntityBaseWithMiscObjectTest, getMapPoseFromRelativePose_relative) -{ - constexpr double s = 5.0; - EXPECT_POSE_NEAR( - dummy.getMapPoseFromRelativePose(makePose(makePoint(s, 0.0))), - static_cast(makeCanonicalizedLaneletPose(hdmap_utils, id, s)), 0.1); -} diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp index ccd66f8d3bc..92bf4ace4c6 100644 --- a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp @@ -37,6 +37,30 @@ class MiscObjectEntityTest : public testing::Test std::shared_ptr hdmap_utils_ptr; const std::string entity_name; }; + +class EntityBaseWithMiscObjectTest : public testing::Test +{ +protected: + EntityBaseWithMiscObjectTest() + : id(120659), + hdmap_utils(makeHdMapUtilsSharedPointer()), + pose(makeCanonicalizedLaneletPose(hdmap_utils, id)), + bbox(makeBoundingBox()), + status(makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox)), + dummy("dummy_entity", status, hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}), + dummy_base(&dummy) + { + } + + const lanelet::Id id; + std::shared_ptr hdmap_utils; + traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose; + traffic_simulator_msgs::msg::BoundingBox bbox; + traffic_simulator::entity_status::CanonicalizedEntityStatus status; + traffic_simulator::entity::MiscObjectEntity dummy; + traffic_simulator::entity::EntityBase * dummy_base; +}; + /** * @note Test basic functionality. Test current action obtaining when NPC logic is not started. */ @@ -210,3 +234,365 @@ TEST_F(MiscObjectEntityTest, getRouteLanelets) .getRouteLanelets(100.0), common::SemanticError); } + +/** + * @note Test basic functionality; test whether the function does nothing. + */ +TEST_F(EntityBaseWithMiscObjectTest, appendDebugMarker) +{ + visualization_msgs::msg::MarkerArray markers{}; + + { + auto markers_copy = markers; + dummy.appendDebugMarker(markers); + EXPECT_EQ(markers.markers.size(), markers_copy.markers.size()); + } + + markers.markers.push_back([] { + auto marker = visualization_msgs::msg::Marker{}; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.pose.position.x = 10.0; + marker.pose.position.y = 10.0; + return marker; + }()); + + { + auto markers_copy = markers; + dummy.appendDebugMarker(markers); + EXPECT_EQ(markers.markers.size(), markers_copy.markers.size()); + } +} + +/** + * @note Test basic functionality; test whether the function throws an error. + */ +TEST_F(EntityBaseWithMiscObjectTest, asFieldOperatorApplication) +{ + EXPECT_THROW(dummy.asFieldOperatorApplication(), common::Error); +} + +/** + * @note Test functionality used by other units; test correctness of 2d polygon calculations. + */ +TEST_F(EntityBaseWithMiscObjectTest, get2DPolygon) +{ + const auto polygon = dummy.get2DPolygon(); + + std::vector ref_poly{ + makePoint(-1.0, -1.0), makePoint(-1.0, 1.0), makePoint(3.0, 1.0), makePoint(3.0, -1.0), + makePoint(-1.0, -1.0)}; + + EXPECT_EQ(polygon.size(), ref_poly.size()); + EXPECT_POINT_EQ(polygon.at(0), ref_poly.at(0)); + EXPECT_POINT_EQ(polygon.at(1), ref_poly.at(1)); + EXPECT_POINT_EQ(polygon.at(2), ref_poly.at(2)); + EXPECT_POINT_EQ(polygon.at(3), ref_poly.at(3)); + EXPECT_POINT_EQ(polygon.at(4), ref_poly.at(4)); +} + +/** + * @note Test basic functionality; test whether the NPC logic is started correctly. + */ +TEST_F(EntityBaseWithMiscObjectTest, startNpcLogic) +{ + EXPECT_FALSE(dummy.isNpcLogicStarted()); + dummy.startNpcLogic(0.0); + EXPECT_TRUE(dummy.isNpcLogicStarted()); +} + +/** + * @note Test basic functionality; test activating an out of range job with + * an entity that has a positive speed and a speed range specified in the job = [0, 0] + */ +TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_speed) +{ + constexpr double velocity = 1.0; + dummy.setLinearVelocity(velocity); + dummy.activateOutOfRangeJob(0.0, 0.0, -100.0, 100.0, -100.0, 100.0); + + constexpr double current_time = 0.0; + constexpr double step_time = 0.0; + EXPECT_NO_THROW(dummy.EntityBase::onUpdate(current_time, step_time)); + EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); +} + +/** + * @note Test basic functionality; test activating an out of range job + * with an entity that has a positive acceleration + * and an acceleration range specified in the job = [0, 0]. + */ +TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_acceleration) +{ + constexpr double acceleration = 1.0; + dummy.setLinearAcceleration(acceleration); + dummy.activateOutOfRangeJob(-100.0, 100.0, 0.0, 0.0, -100.0, 100.0); + + constexpr double current_time = 0.0; + constexpr double step_time = 0.0; + EXPECT_NO_THROW(dummy.EntityBase::onUpdate(current_time, step_time)); + EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); +} + +/** + * @note Test basic functionality; test activating an out of range job + * with an entity that has a positive jerk + * and a jerk range specified in the job = [0, 0]. + */ +TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_jerk) +{ + constexpr double jerk = 1.0; + dummy.setLinearJerk(jerk); + dummy.activateOutOfRangeJob(-100.0, 100.0, -100.0, 100.0, 0.0, 0.0); + + constexpr double current_time = 0.0; + constexpr double step_time = 0.0; + EXPECT_NO_THROW(dummy.EntityBase::onUpdate(current_time, step_time)); + EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); +} + +/** + * @note Test basic functionality; test wrapper function with invalid relative target lanelet pose. + */ +TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetLaneletPose) +{ + const std::string target_name = "target_name"; + + auto other_status = + std::unordered_map{}; + other_status.emplace( + target_name, + makeCanonicalizedEntityStatus(hdmap_utils, makePose(makePoint(3810.0, 73745.0)), bbox)); + + dummy_base->setOtherStatus(other_status); + + EXPECT_THROW(dummy_base->requestLaneChange( + traffic_simulator::lane_change::RelativeTarget( + target_name, traffic_simulator::lane_change::Direction::STRAIGHT, 3, 1.0), + traffic_simulator::lane_change::TrajectoryShape::LINEAR, + traffic_simulator::lane_change::Constraint( + traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, + traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT)); + , common::Error); +} + +/** + * @note Test basic functionality; test wrapper function with invalid relative target name. + */ +TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetName) +{ + const std::string target_name = "target_name"; + + auto other_status = + std::unordered_map{}; + other_status.emplace( + target_name, makeCanonicalizedEntityStatus( + hdmap_utils, makeCanonicalizedLaneletPose(hdmap_utils, 34468, 5.0), bbox)); + + dummy_base->setOtherStatus(other_status); + EXPECT_THROW( + dummy_base->requestLaneChange( + traffic_simulator::lane_change::RelativeTarget( + target_name + "_wrong", traffic_simulator::lane_change::Direction::STRAIGHT, 3, 1.0), + traffic_simulator::lane_change::TrajectoryShape::LINEAR, + traffic_simulator::lane_change::Constraint( + traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, + traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT)), + common::Error); +} + +/** + * @note Test basic functionality; test wrapper function with invalid relative target lane change + * - the goal is to request a lane change in the location where the lane change is impossible. + */ +TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetInvalid) +{ + const std::string target_name = "target_name"; + + auto other_status = + std::unordered_map{}; + other_status.emplace( + target_name, makeCanonicalizedEntityStatus( + hdmap_utils, makeCanonicalizedLaneletPose(hdmap_utils, 34468, 5.0), bbox)); + + dummy_base->setOtherStatus(other_status); + EXPECT_THROW( + dummy_base->requestLaneChange( + traffic_simulator::lane_change::RelativeTarget( + target_name, traffic_simulator::lane_change::Direction::RIGHT, + std::numeric_limits::max(), 1.0), + traffic_simulator::lane_change::TrajectoryShape::LINEAR, + traffic_simulator::lane_change::Constraint( + traffic_simulator::lane_change::Constraint::Type::TIME, 30.0, + traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT)), + common::Error); +} + +/** + * @note Test function behavior when called with any argument - the goal is to test error throwing. + */ +TEST_F(EntityBaseWithMiscObjectTest, requestFollowTrajectory) +{ + EXPECT_THROW( + dummy.requestFollowTrajectory( + std::make_shared()), + common::Error); +} + +/** + * @note Test function behavior when called with any argument - the goal is to test error throwing. + */ +TEST_F(EntityBaseWithMiscObjectTest, requestWalkStraight) +{ + EXPECT_THROW(dummy.requestWalkStraight(), common::Error); +} + +/** + * @note test basic functionality; test updating stand still duration + * when NPC logic is started and velocity is greater than 0. + */ +TEST_F(EntityBaseWithMiscObjectTest, updateStandStillDuration_startedMoving) +{ + dummy.startNpcLogic(0.0); + dummy.setLinearVelocity(3.0); + + EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); +} + +/** + * @note Test basic functionality; test updating stand still duration + * when NPC logic is not started. + */ +TEST_F(EntityBaseWithMiscObjectTest, updateStandStillDuration_notStarted) +{ + dummy.setLinearVelocity(3.0); + EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); + + dummy.setLinearVelocity(0.0); + EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); +} + +/** + * @note Test basic functionality; test updating traveled distance correctness + * with NPC logic started and velocity greater than 0. + */ +TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_startedMoving) +{ + constexpr double velocity = 3.0; + constexpr double step_time = 0.1; + dummy.startNpcLogic(0.0); + dummy.setLinearVelocity(velocity); + + EXPECT_EQ(1.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); + EXPECT_EQ(2.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); + EXPECT_EQ(3.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); + EXPECT_EQ(4.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); +} + +/** + * @note Test basic functionality; test updating traveled distance correctness with NPC not started. + */ +TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_notStarted) +{ + constexpr double step_time = 0.1; + dummy.setLinearVelocity(3.0); + EXPECT_EQ(0.0, dummy.updateTraveledDistance(step_time)); + + dummy.setLinearVelocity(0.0); + EXPECT_EQ(0.0, dummy.updateTraveledDistance(step_time)); +} + +/** + * @note Test basic functionality; test stopping correctness - the goal + * is to check whether the entity status is changed to stopped (no velocity etc.). + */ +TEST_F(EntityBaseWithMiscObjectTest, stopAtCurrentPosition) +{ + constexpr double velocity = 3.0; + dummy.setLinearVelocity(velocity); + EXPECT_EQ(dummy.getCurrentTwist().linear.x, velocity); + + dummy.stopAtCurrentPosition(); + EXPECT_EQ(dummy.getCurrentTwist().linear.x, 0.0); +} + +/** + * @note Test functionality used by other units; test lanelet pose obtaining + * with a matching distance smaller than a distance from an entity to the lanelet + * (both crosswalk and road) and status_.type.type != PEDESTRIAN. + */ +TEST(EntityBaseWithMiscObject, getLaneletPose_notOnRoadAndCrosswalkNotPedestrian) +{ + auto hdmap_utils = makeHdMapUtilsSharedPointer(); + + EXPECT_FALSE(traffic_simulator::entity::MiscObjectEntity( + "dummy_entity", + traffic_simulator::CanonicalizedEntityStatus( + makeEntityStatus( + hdmap_utils, makePose(makePoint(3810.0, 73745.0)), makeBoundingBox(), 0.0, + "dummy_entity", traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + hdmap_utils), + hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .getLaneletPose(5.0) + .has_value()); +} + +/** + * @note Test functionality used by other units; test lanelet pose obtaining + * with a matching distance greater than a distance from an entity to the lanelet + * (both crosswalk and road) and status_.type.type != PEDESTRIAN. + */ +TEST(EntityBaseWithMiscObject, getLaneletPose_onRoadAndCrosswalkNotPedestrian) +{ + auto hdmap_utils = makeHdMapUtilsSharedPointer(); + + EXPECT_TRUE( + traffic_simulator::entity::MiscObjectEntity( + "dummy_entity", + traffic_simulator::CanonicalizedEntityStatus( + makeEntityStatus( + hdmap_utils, + makePose(makePoint(3766.1, 73738.2), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), + makeBoundingBox(), 0.0, "dummy_entity", + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + hdmap_utils), + hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .getLaneletPose(1.0) + .has_value()); +} + +/** + * @note Test functionality used by other units; test lanelet pose obtaining + * with a matching distance greater than a distance from an entity to the crosswalk lanelet, + * but smaller than to the road lanelet and status_.type.type != PEDESTRIAN. + */ +TEST(EntityBaseWithMiscObject, getLaneletPose_onCrosswalkNotOnRoadNotPedestrian) +{ + auto hdmap_utils = makeHdMapUtilsSharedPointer(); + + EXPECT_FALSE( + traffic_simulator::entity::MiscObjectEntity( + "dummy_entity", + traffic_simulator::CanonicalizedEntityStatus( + makeEntityStatus( + hdmap_utils, + makePose(makePoint(3764.5, 73737.5), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), + makeBoundingBox(), 0.0, "dummy_entity", + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + hdmap_utils), + hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}) + .getLaneletPose(1.0) + .has_value()); +} + +/** + * @note Test functionality used by other units; test relative pose calculations + * correctness with a transformation argument passed. + */ +TEST_F(EntityBaseWithMiscObjectTest, getMapPoseFromRelativePose_relative) +{ + constexpr double s = 5.0; + EXPECT_POSE_NEAR( + dummy.getMapPoseFromRelativePose(makePose(makePoint(s, 0.0))), + static_cast(makeCanonicalizedLaneletPose(hdmap_utils, id, s)), 0.1); +} From d3395babe06decca62eda90fd2dc4b5660af9384 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 12 Jun 2024 12:45:59 +0200 Subject: [PATCH 063/118] use better naming --- .../src/entity/test_misc_object_entity.cpp | 133 +++++++++--------- .../test/src/helper_functions.hpp | 8 +- 2 files changed, 73 insertions(+), 68 deletions(-) diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp index 92bf4ace4c6..a62292c856b 100644 --- a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp @@ -32,7 +32,10 @@ int main(int argc, char ** argv) class MiscObjectEntityTest : public testing::Test { protected: - MiscObjectEntityTest() : hdmap_utils_ptr(makeHdMapUtilsSharedPointer()), entity_name("blob") {} + MiscObjectEntityTest() + : hdmap_utils_ptr(makeHdMapUtilsSharedPointer()), entity_name("misc_object_entity") + { + } std::shared_ptr hdmap_utils_ptr; const std::string entity_name; @@ -47,8 +50,10 @@ class EntityBaseWithMiscObjectTest : public testing::Test pose(makeCanonicalizedLaneletPose(hdmap_utils, id)), bbox(makeBoundingBox()), status(makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox)), - dummy("dummy_entity", status, hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}), - dummy_base(&dummy) + misc_object( + "default_entity_name", status, hdmap_utils, + traffic_simulator_msgs::msg::MiscObjectParameters{}), + entity_base(&misc_object) { } @@ -57,8 +62,8 @@ class EntityBaseWithMiscObjectTest : public testing::Test traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose; traffic_simulator_msgs::msg::BoundingBox bbox; traffic_simulator::entity_status::CanonicalizedEntityStatus status; - traffic_simulator::entity::MiscObjectEntity dummy; - traffic_simulator::entity::EntityBase * dummy_base; + traffic_simulator::entity::MiscObjectEntity misc_object; + traffic_simulator::entity::EntityBase * entity_base; }; /** @@ -115,15 +120,15 @@ TEST_F(MiscObjectEntityTest, requestSpeedChange_relative) std::unordered_map others; others.emplace( - "bob_entity", makeCanonicalizedEntityStatus( - hdmap_utils_ptr, pose, bbox, 17.0, "bob", - traffic_simulator_msgs::msg::EntityType::MISC_OBJECT)); + "other_entity", makeCanonicalizedEntityStatus( + hdmap_utils_ptr, pose, bbox, 17.0, "other_entity_name", + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT)); blob.setOtherStatus(others); EXPECT_THROW( blob.requestSpeedChange( traffic_simulator::speed_change::RelativeTargetSpeed( - "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, 3.0), + "other_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, 3.0), true), common::SemanticError); } @@ -244,7 +249,7 @@ TEST_F(EntityBaseWithMiscObjectTest, appendDebugMarker) { auto markers_copy = markers; - dummy.appendDebugMarker(markers); + misc_object.appendDebugMarker(markers); EXPECT_EQ(markers.markers.size(), markers_copy.markers.size()); } @@ -259,7 +264,7 @@ TEST_F(EntityBaseWithMiscObjectTest, appendDebugMarker) { auto markers_copy = markers; - dummy.appendDebugMarker(markers); + misc_object.appendDebugMarker(markers); EXPECT_EQ(markers.markers.size(), markers_copy.markers.size()); } } @@ -269,7 +274,7 @@ TEST_F(EntityBaseWithMiscObjectTest, appendDebugMarker) */ TEST_F(EntityBaseWithMiscObjectTest, asFieldOperatorApplication) { - EXPECT_THROW(dummy.asFieldOperatorApplication(), common::Error); + EXPECT_THROW(misc_object.asFieldOperatorApplication(), common::Error); } /** @@ -277,7 +282,7 @@ TEST_F(EntityBaseWithMiscObjectTest, asFieldOperatorApplication) */ TEST_F(EntityBaseWithMiscObjectTest, get2DPolygon) { - const auto polygon = dummy.get2DPolygon(); + const auto polygon = misc_object.get2DPolygon(); std::vector ref_poly{ makePoint(-1.0, -1.0), makePoint(-1.0, 1.0), makePoint(3.0, 1.0), makePoint(3.0, -1.0), @@ -296,9 +301,9 @@ TEST_F(EntityBaseWithMiscObjectTest, get2DPolygon) */ TEST_F(EntityBaseWithMiscObjectTest, startNpcLogic) { - EXPECT_FALSE(dummy.isNpcLogicStarted()); - dummy.startNpcLogic(0.0); - EXPECT_TRUE(dummy.isNpcLogicStarted()); + EXPECT_FALSE(misc_object.isNpcLogicStarted()); + misc_object.startNpcLogic(0.0); + EXPECT_TRUE(misc_object.isNpcLogicStarted()); } /** @@ -308,13 +313,13 @@ TEST_F(EntityBaseWithMiscObjectTest, startNpcLogic) TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_speed) { constexpr double velocity = 1.0; - dummy.setLinearVelocity(velocity); - dummy.activateOutOfRangeJob(0.0, 0.0, -100.0, 100.0, -100.0, 100.0); + misc_object.setLinearVelocity(velocity); + misc_object.activateOutOfRangeJob(0.0, 0.0, -100.0, 100.0, -100.0, 100.0); constexpr double current_time = 0.0; constexpr double step_time = 0.0; - EXPECT_NO_THROW(dummy.EntityBase::onUpdate(current_time, step_time)); - EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); + EXPECT_NO_THROW(misc_object.EntityBase::onUpdate(current_time, step_time)); + EXPECT_THROW(misc_object.onPostUpdate(current_time, step_time), common::Error); } /** @@ -325,13 +330,13 @@ TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_speed) TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_acceleration) { constexpr double acceleration = 1.0; - dummy.setLinearAcceleration(acceleration); - dummy.activateOutOfRangeJob(-100.0, 100.0, 0.0, 0.0, -100.0, 100.0); + misc_object.setLinearAcceleration(acceleration); + misc_object.activateOutOfRangeJob(-100.0, 100.0, 0.0, 0.0, -100.0, 100.0); constexpr double current_time = 0.0; constexpr double step_time = 0.0; - EXPECT_NO_THROW(dummy.EntityBase::onUpdate(current_time, step_time)); - EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); + EXPECT_NO_THROW(misc_object.EntityBase::onUpdate(current_time, step_time)); + EXPECT_THROW(misc_object.onPostUpdate(current_time, step_time), common::Error); } /** @@ -342,13 +347,13 @@ TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_acceleration) TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_jerk) { constexpr double jerk = 1.0; - dummy.setLinearJerk(jerk); - dummy.activateOutOfRangeJob(-100.0, 100.0, -100.0, 100.0, 0.0, 0.0); + misc_object.setLinearJerk(jerk); + misc_object.activateOutOfRangeJob(-100.0, 100.0, -100.0, 100.0, 0.0, 0.0); constexpr double current_time = 0.0; constexpr double step_time = 0.0; - EXPECT_NO_THROW(dummy.EntityBase::onUpdate(current_time, step_time)); - EXPECT_THROW(dummy.onPostUpdate(current_time, step_time), common::Error); + EXPECT_NO_THROW(misc_object.EntityBase::onUpdate(current_time, step_time)); + EXPECT_THROW(misc_object.onPostUpdate(current_time, step_time), common::Error); } /** @@ -364,9 +369,9 @@ TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetLaneletPose target_name, makeCanonicalizedEntityStatus(hdmap_utils, makePose(makePoint(3810.0, 73745.0)), bbox)); - dummy_base->setOtherStatus(other_status); + entity_base->setOtherStatus(other_status); - EXPECT_THROW(dummy_base->requestLaneChange( + EXPECT_THROW(entity_base->requestLaneChange( traffic_simulator::lane_change::RelativeTarget( target_name, traffic_simulator::lane_change::Direction::STRAIGHT, 3, 1.0), traffic_simulator::lane_change::TrajectoryShape::LINEAR, @@ -389,9 +394,9 @@ TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetName) target_name, makeCanonicalizedEntityStatus( hdmap_utils, makeCanonicalizedLaneletPose(hdmap_utils, 34468, 5.0), bbox)); - dummy_base->setOtherStatus(other_status); + entity_base->setOtherStatus(other_status); EXPECT_THROW( - dummy_base->requestLaneChange( + entity_base->requestLaneChange( traffic_simulator::lane_change::RelativeTarget( target_name + "_wrong", traffic_simulator::lane_change::Direction::STRAIGHT, 3, 1.0), traffic_simulator::lane_change::TrajectoryShape::LINEAR, @@ -415,9 +420,9 @@ TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetInvalid) target_name, makeCanonicalizedEntityStatus( hdmap_utils, makeCanonicalizedLaneletPose(hdmap_utils, 34468, 5.0), bbox)); - dummy_base->setOtherStatus(other_status); + entity_base->setOtherStatus(other_status); EXPECT_THROW( - dummy_base->requestLaneChange( + entity_base->requestLaneChange( traffic_simulator::lane_change::RelativeTarget( target_name, traffic_simulator::lane_change::Direction::RIGHT, std::numeric_limits::max(), 1.0), @@ -434,7 +439,7 @@ TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetInvalid) TEST_F(EntityBaseWithMiscObjectTest, requestFollowTrajectory) { EXPECT_THROW( - dummy.requestFollowTrajectory( + misc_object.requestFollowTrajectory( std::make_shared()), common::Error); } @@ -444,7 +449,7 @@ TEST_F(EntityBaseWithMiscObjectTest, requestFollowTrajectory) */ TEST_F(EntityBaseWithMiscObjectTest, requestWalkStraight) { - EXPECT_THROW(dummy.requestWalkStraight(), common::Error); + EXPECT_THROW(misc_object.requestWalkStraight(), common::Error); } /** @@ -453,10 +458,10 @@ TEST_F(EntityBaseWithMiscObjectTest, requestWalkStraight) */ TEST_F(EntityBaseWithMiscObjectTest, updateStandStillDuration_startedMoving) { - dummy.startNpcLogic(0.0); - dummy.setLinearVelocity(3.0); + misc_object.startNpcLogic(0.0); + misc_object.setLinearVelocity(3.0); - EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); + EXPECT_EQ(0.0, misc_object.updateStandStillDuration(0.1)); } /** @@ -465,11 +470,11 @@ TEST_F(EntityBaseWithMiscObjectTest, updateStandStillDuration_startedMoving) */ TEST_F(EntityBaseWithMiscObjectTest, updateStandStillDuration_notStarted) { - dummy.setLinearVelocity(3.0); - EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); + misc_object.setLinearVelocity(3.0); + EXPECT_EQ(0.0, misc_object.updateStandStillDuration(0.1)); - dummy.setLinearVelocity(0.0); - EXPECT_EQ(0.0, dummy.updateStandStillDuration(0.1)); + misc_object.setLinearVelocity(0.0); + EXPECT_EQ(0.0, misc_object.updateStandStillDuration(0.1)); } /** @@ -480,13 +485,13 @@ TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_startedMoving) { constexpr double velocity = 3.0; constexpr double step_time = 0.1; - dummy.startNpcLogic(0.0); - dummy.setLinearVelocity(velocity); + misc_object.startNpcLogic(0.0); + misc_object.setLinearVelocity(velocity); - EXPECT_EQ(1.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); - EXPECT_EQ(2.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); - EXPECT_EQ(3.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); - EXPECT_EQ(4.0 * step_time * velocity, dummy.updateTraveledDistance(step_time)); + EXPECT_EQ(1.0 * step_time * velocity, misc_object.updateTraveledDistance(step_time)); + EXPECT_EQ(2.0 * step_time * velocity, misc_object.updateTraveledDistance(step_time)); + EXPECT_EQ(3.0 * step_time * velocity, misc_object.updateTraveledDistance(step_time)); + EXPECT_EQ(4.0 * step_time * velocity, misc_object.updateTraveledDistance(step_time)); } /** @@ -495,11 +500,11 @@ TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_startedMoving) TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_notStarted) { constexpr double step_time = 0.1; - dummy.setLinearVelocity(3.0); - EXPECT_EQ(0.0, dummy.updateTraveledDistance(step_time)); + misc_object.setLinearVelocity(3.0); + EXPECT_EQ(0.0, misc_object.updateTraveledDistance(step_time)); - dummy.setLinearVelocity(0.0); - EXPECT_EQ(0.0, dummy.updateTraveledDistance(step_time)); + misc_object.setLinearVelocity(0.0); + EXPECT_EQ(0.0, misc_object.updateTraveledDistance(step_time)); } /** @@ -509,11 +514,11 @@ TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_notStarted) TEST_F(EntityBaseWithMiscObjectTest, stopAtCurrentPosition) { constexpr double velocity = 3.0; - dummy.setLinearVelocity(velocity); - EXPECT_EQ(dummy.getCurrentTwist().linear.x, velocity); + misc_object.setLinearVelocity(velocity); + EXPECT_EQ(misc_object.getCurrentTwist().linear.x, velocity); - dummy.stopAtCurrentPosition(); - EXPECT_EQ(dummy.getCurrentTwist().linear.x, 0.0); + misc_object.stopAtCurrentPosition(); + EXPECT_EQ(misc_object.getCurrentTwist().linear.x, 0.0); } /** @@ -526,11 +531,11 @@ TEST(EntityBaseWithMiscObject, getLaneletPose_notOnRoadAndCrosswalkNotPedestrian auto hdmap_utils = makeHdMapUtilsSharedPointer(); EXPECT_FALSE(traffic_simulator::entity::MiscObjectEntity( - "dummy_entity", + "misc_object_entity", traffic_simulator::CanonicalizedEntityStatus( makeEntityStatus( hdmap_utils, makePose(makePoint(3810.0, 73745.0)), makeBoundingBox(), 0.0, - "dummy_entity", traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + "misc_object_entity", traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils), hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getLaneletPose(5.0) @@ -548,12 +553,12 @@ TEST(EntityBaseWithMiscObject, getLaneletPose_onRoadAndCrosswalkNotPedestrian) EXPECT_TRUE( traffic_simulator::entity::MiscObjectEntity( - "dummy_entity", + "misc_object_entity", traffic_simulator::CanonicalizedEntityStatus( makeEntityStatus( hdmap_utils, makePose(makePoint(3766.1, 73738.2), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), - makeBoundingBox(), 0.0, "dummy_entity", + makeBoundingBox(), 0.0, "misc_object_entity", traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils), hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}) @@ -572,12 +577,12 @@ TEST(EntityBaseWithMiscObject, getLaneletPose_onCrosswalkNotOnRoadNotPedestrian) EXPECT_FALSE( traffic_simulator::entity::MiscObjectEntity( - "dummy_entity", + "misc_object_entity", traffic_simulator::CanonicalizedEntityStatus( makeEntityStatus( hdmap_utils, makePose(makePoint(3764.5, 73737.5), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), - makeBoundingBox(), 0.0, "dummy_entity", + makeBoundingBox(), 0.0, "misc_object_entity", traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils), hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}) @@ -593,6 +598,6 @@ TEST_F(EntityBaseWithMiscObjectTest, getMapPoseFromRelativePose_relative) { constexpr double s = 5.0; EXPECT_POSE_NEAR( - dummy.getMapPoseFromRelativePose(makePose(makePoint(s, 0.0))), + misc_object.getMapPoseFromRelativePose(makePose(makePoint(s, 0.0))), static_cast(makeCanonicalizedLaneletPose(hdmap_utils, id, s)), 0.1); } diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index d5b58c8911f..7b786a006e9 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -82,7 +82,7 @@ auto makeEntityStatus( std::shared_ptr hdmap_utils, traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, - const std::string name = "dummy_entity", + const std::string name = "default_entity_name", const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::EntityStatus { @@ -102,7 +102,7 @@ auto makeEntityStatus( auto makeEntityStatus( std::shared_ptr /* hdmap_utils */, geometry_msgs::msg::Pose pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, - const std::string name = "dummy_entity", + const std::string name = "default_entity_name", const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::EntityStatus { @@ -123,7 +123,7 @@ auto makeCanonicalizedEntityStatus( std::shared_ptr hdmap_utils, traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, - const std::string name = "dummy_entity", + const std::string name = "default_entity_name", const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::entity_status::CanonicalizedEntityStatus { @@ -134,7 +134,7 @@ auto makeCanonicalizedEntityStatus( auto makeCanonicalizedEntityStatus( std::shared_ptr hdmap_utils, geometry_msgs::msg::Pose pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, - const std::string name = "dummy_entity", + const std::string name = "default_entity_name", const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::entity_status::CanonicalizedEntityStatus { From 63b3c343c93b0eab4f1300c41d26a49652a71f76 Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Wed, 12 Jun 2024 20:21:55 +0900 Subject: [PATCH 064/118] change format --- .../src/traffic_source/define_traffic_source_delay.cpp | 2 +- .../src/traffic_source/define_traffic_source_high_rate.cpp | 2 +- .../src/traffic_source/define_traffic_source_large.cpp | 2 +- .../src/traffic_source/define_traffic_source_mixed.cpp | 2 +- .../src/traffic_source/define_traffic_source_multiple.cpp | 2 +- .../src/traffic_source/define_traffic_source_outside_lane.cpp | 2 +- .../src/traffic_source/define_traffic_source_pedestrian.cpp | 2 +- .../src/traffic_source/define_traffic_source_vehicle.cpp | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp index 431851f40f7..4fbaf8e04d1 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -22,7 +23,6 @@ #include #include #include -#include namespace cpp_mock_scenarios { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp index 5f2d5df8e2e..f717cb95409 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -22,7 +23,6 @@ #include #include #include -#include namespace cpp_mock_scenarios { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp index 49577ab9c9f..25b324894ee 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -22,7 +23,6 @@ #include #include #include -#include namespace cpp_mock_scenarios { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp index 65e1aebc091..97d8c75e2a2 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -22,7 +23,6 @@ #include #include #include -#include namespace cpp_mock_scenarios { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp index 94e993a6b5c..01e5a634256 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -22,7 +23,6 @@ #include #include #include -#include namespace cpp_mock_scenarios { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp index d7b42cf1e8a..37fd9d2af2a 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -22,7 +23,6 @@ #include #include #include -#include namespace cpp_mock_scenarios { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp index 300624a442a..870b790cf1b 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -22,7 +23,6 @@ #include #include #include -#include namespace cpp_mock_scenarios { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp index 5e1ebfd69f7..a926ab8b51e 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -22,7 +23,6 @@ #include #include #include -#include namespace cpp_mock_scenarios { From 9f9d4af38e72838e790492ac8a36fade8ea26cbc Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Wed, 12 Jun 2024 21:00:19 +0900 Subject: [PATCH 065/118] Only publsih tf onece. --- simulation/traffic_simulator/src/entity/entity_manager.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 571adc2dedf..6d4ba9cdca7 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -39,6 +39,10 @@ namespace entity { void EntityManager::broadcastEntityTransform() { + static bool isSend; + if(isSend){return;} + else{isSend = true;} + using math::geometry::operator/; using math::geometry::operator*; using math::geometry::operator+=; From 9ffa38fefb4d83074f7be3dce4824c22195aecfc Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Wed, 12 Jun 2024 21:01:27 +0900 Subject: [PATCH 066/118] fix format --- simulation/traffic_simulator/src/entity/entity_manager.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 6d4ba9cdca7..d834314b489 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -40,8 +40,11 @@ namespace entity void EntityManager::broadcastEntityTransform() { static bool isSend; - if(isSend){return;} - else{isSend = true;} + if (isSend) { + return; + } else { + isSend = true; + } using math::geometry::operator/; using math::geometry::operator*; From 8f047eee4d4183a00ba0fe81863c20d60cef65e2 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Wed, 12 Jun 2024 14:57:11 +0200 Subject: [PATCH 067/118] test: [RJD-937] to Implement Unit tests on simple_sensor_simulator - Added unit tests to Box and Vertex classes - Added unit tests to Raycaster class - Added set of macros used in unit tests --- .../simple_sensor_simulator/CMakeLists.txt | 3 + .../test/CMakeLists.txt | 14 ++ .../test/src/lidar/test_raycaster.cpp | 152 ++++++++++++++++++ .../test/src/lidar/test_raycaster.hpp | 77 +++++++++ .../test/src/primitives/test_box.cpp | 70 ++++++++ .../test/src/primitives/test_vertex.cpp | 123 ++++++++++++++ .../test/src/utils/expect_eq_macros.hpp | 68 ++++++++ 7 files changed, 507 insertions(+) create mode 100644 simulation/simple_sensor_simulator/test/CMakeLists.txt create mode 100644 simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.hpp create mode 100644 simulation/simple_sensor_simulator/test/src/primitives/test_box.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/primitives/test_vertex.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt index df6f898805f..2cfa16ebc5c 100644 --- a/simulation/simple_sensor_simulator/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/CMakeLists.txt @@ -84,7 +84,10 @@ EXPORT export_simple_sensor_simulator_component if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) ament_lint_auto_find_test_dependencies() + + add_subdirectory(test) endif() ament_auto_package() diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt new file mode 100644 index 00000000000..138711416a0 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/CMakeLists.txt @@ -0,0 +1,14 @@ +find_package(Protobuf REQUIRED) +include_directories(${Protobuf_INCLUDE_DIRS}) + +ament_add_gtest(test_box src/primitives/test_box.cpp) +target_link_libraries(test_box simple_sensor_simulator_component) + +ament_add_gtest(test_vertex src/primitives/test_vertex.cpp) +target_link_libraries(test_vertex simple_sensor_simulator_component) + +ament_add_gtest(test_raycaster src/lidar/test_raycaster.cpp) +target_link_libraries(test_raycaster simple_sensor_simulator_component ${Protobuf_LIBRARIES}) + +ament_add_gtest(test_grid_traversal src/lidar/test_grid_traversal.cpp) +target_link_libraries(test_grid_traversal simple_sensor_simulator_component) \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.cpp b/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.cpp new file mode 100644 index 00000000000..57a7308a6e8 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.cpp @@ -0,0 +1,152 @@ +// 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_raycaster.hpp" + +/** + * @note Test basic functionality + * + * Test adding a primitive to scene correctness with a Box. + */ +TEST_F(RaycasterTest, addPrimitive_box) +{ + EXPECT_NO_THROW(raycaster_->addPrimitive( + box_name_, box_depth_, box_width_, box_height_, box_pose_)); +} + +/** + * @note Test function behavior when adding two Boxes with identical names to a scene. + */ +TEST_F(RaycasterTest, addPrimitive_twoIdenticalNames) +{ + raycaster_->addPrimitive( + box_name_, box_depth_, box_width_, box_height_, box_pose_); + + EXPECT_THROW( + raycaster_->addPrimitive( + box_name_, box_depth_, box_width_, box_height_, box_pose_), + std::runtime_error); +} + +/** + * @note Test basic functionality + * + * Test raycasting correctness with an empty scene. + */ +TEST_F(RaycasterTest, raycast_empty) +{ + auto cloud = raycaster_->raycast(frame_id_, stamp_, origin_); + + EXPECT_EQ(cloud.width, 0); + EXPECT_EQ(cloud.header.frame_id, frame_id_); + EXPECT_EQ(cloud.header.stamp, stamp_); +} + +/** + * @note Test basic functionality + * + * Test raycasting correctness with one box on the scene. + */ +TEST_F(RaycasterTest, raycast_box) +{ + raycaster_->addPrimitive( + box_name_, box_depth_, box_width_, box_height_, box_pose_); + + auto cloud = raycaster_->raycast(frame_id_, stamp_, origin_); + + EXPECT_GT(cloud.width, 0); + EXPECT_EQ(cloud.header.frame_id, frame_id_); + EXPECT_EQ(cloud.header.stamp, stamp_); +} + +/** + * @note Test basic functionality + * + * Test setting ray directions with a lidar configuration that has one ray which intersects with the + * only box on the scene. + */ +TEST_F(RaycasterTest, setDirection_oneBox) +{ + raycaster_->addPrimitive( + box_name_, box_depth_, box_width_, box_height_, box_pose_); + + simulation_api_schema::LidarConfiguration config; + config.add_vertical_angles(0.0); // Only one vertical angle for a horizontal ring + config.set_horizontal_resolution(degToRad(1.0)); // Set horizontal resolution to 1 degree + + raycaster_->setDirection(config); + + auto cloud = raycaster_->raycast(frame_id_, stamp_, origin_); + + EXPECT_GT(cloud.width, 0); + EXPECT_EQ(cloud.header.frame_id, frame_id_); + EXPECT_EQ(cloud.header.stamp, stamp_); +} + +/** + * @note Test basic functionality + * + * Test setting ray directions with a lidar configuration that has a ring of horizontal rays which + * intersect with boxes positioned on the ring so that they intersect with the rays. + */ +TEST_F(RaycasterTest, setDirection_manyBoxes) +{ + constexpr double radius = 5.0; + constexpr int num_boxes = 10; + constexpr double angle_increment = 2 * M_PI / num_boxes; + + for (int i = 0; i < num_boxes; ++i) { + const double angle = i * angle_increment; + const auto box_pose = + geometry_msgs::build() + .position(geometry_msgs::build() + .x(radius * cos(angle)) + .y(radius * sin(angle)) + .z(0)) + .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); + + std::string name = "box" + std::to_string(i); + raycaster_->addPrimitive(name, box_depth_, box_width_, box_height_, box_pose); + } + + simulation_api_schema::LidarConfiguration config; + config.add_vertical_angles(0.0); // Only one vertical angle for a horizontal ring + config.set_horizontal_resolution(degToRad(5)); + + raycaster_->setDirection(config); + + auto cloud = raycaster_->raycast(frame_id_, stamp_, origin_); + + EXPECT_GT(cloud.width, 0); + EXPECT_EQ(cloud.header.frame_id, frame_id_); + EXPECT_EQ(cloud.header.stamp, stamp_); +} + +/** + * @note Test basic functionality + * + * Test detected objects obtaining from the statuses list containing Ego. + */ +TEST_F(RaycasterTest, getDetectedObjects) +{ + raycaster_->addPrimitive( + box_name_, box_depth_, box_width_, box_height_, box_pose_); + + raycaster_->raycast(frame_id_, stamp_, origin_); + + const auto & detected_objects = raycaster_->getDetectedObject(); + + ASSERT_FALSE(detected_objects.empty()); + EXPECT_EQ(detected_objects[0], box_name_); +} diff --git a/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.hpp b/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.hpp new file mode 100644 index 00000000000..37fad304554 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.hpp @@ -0,0 +1,77 @@ +// 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_RAYCASTER_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__TEST__TEST_RAYCASTER_HPP_ + +#include + +#include +#include +#include +#include +#include + +using namespace simple_sensor_simulator; +using namespace geometry_msgs::msg; + +constexpr static double degToRad(double deg) { return deg * M_PI / 180.0; } + +class RaycasterTest : public ::testing::Test +{ +protected: + void SetUp() override + { + raycaster_ = std::make_unique(); + configureLidar(); + + origin_ = + geometry_msgs::build() + .position(geometry_msgs::build().x(0).y(0).z(0)) + .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); + box_pose_ = + geometry_msgs::build() + .position(geometry_msgs::build().x(5.0).y(0).z(0)) + .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); + } + + std::unique_ptr raycaster_; + simulation_api_schema::LidarConfiguration config_; + + const float box_depth_{1.0f}; + const float box_width_{1.0f}; + const float box_height_{1.0f}; + const std::string box_name_{"box"}; + + const rclcpp::Time stamp_{0}; + const std::string frame_id_{"frame_id"}; + + geometry_msgs::msg::Pose origin_; + geometry_msgs::msg::Pose box_pose_; + +private: + auto configureLidar() -> void + { + // Setting vertical angles from -15 to +15 degrees in 2 degree steps + for (double angle = -15.0; angle <= 15.0; angle += 2.0) { + config_.add_vertical_angles(degToRad(angle)); + } + // Setting horizontal resolution to 0.5 degrees + config_.set_horizontal_resolution(degToRad(0.5)); + + raycaster_->setDirection(config_); + } +}; + +#endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_RAYCASTER_HPP_ diff --git a/simulation/simple_sensor_simulator/test/src/primitives/test_box.cpp b/simulation/simple_sensor_simulator/test/src/primitives/test_box.cpp new file mode 100644 index 00000000000..5543e200cba --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/primitives/test_box.cpp @@ -0,0 +1,70 @@ +// 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 +#include + +#include +#include +#include + +#include "../utils/expect_eq_macros.hpp" + +using namespace simple_sensor_simulator; +using namespace simple_sensor_simulator::primitives; + +/** + * @note Test initialization correctness + * + * The goal is to test whether the vertices and indices are initialized to create a box shape (can + * use get2DConvexHull or other specially created const reference accessors). + */ +TEST(BoxTest, Box) +{ + const std::vector expected_vertices = { + {-0.5, -0.5, -0.5}, {-0.5, -0.5, 0.5}, {-0.5, 0.5, -0.5}, {-0.5, 0.5, 0.5}, + {0.5, -0.5, -0.5}, {0.5, -0.5, 0.5}, {0.5, 0.5, -0.5}, {0.5, 0.5, 0.5}}; + + // Indices + const std::vector expected_triangles = {{0, 1, 2}, {1, 3, 2}, {4, 6, 5}, {5, 6, 7}, + {0, 4, 1}, {1, 4, 5}, {2, 3, 6}, {3, 7, 6}, + {0, 2, 4}, {2, 6, 4}, {1, 5, 3}, {3, 5, 7}}; + + const auto pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(0).y(0).z(0)) + .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); + + Box box(1.0, 1.0, 1.0, pose); + + auto vertices = box.getVertex(); + auto triangles = box.getTriangles(); + + ASSERT_EQ(triangles.size(), expected_triangles.size()); + ASSERT_EQ(vertices.size(), expected_vertices.size()); + + for (size_t i = 0; i < triangles.size(); ++i) { + EXPECT_TRIANGLE_EQ(triangles[i], expected_triangles[i]); + } + + for (size_t i = 0; i < vertices.size(); ++i) { + EXPECT_VERTEX_EQ(vertices[i], expected_vertices[i]); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/primitives/test_vertex.cpp b/simulation/simple_sensor_simulator/test/src/primitives/test_vertex.cpp new file mode 100644 index 00000000000..353507175c5 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/primitives/test_vertex.cpp @@ -0,0 +1,123 @@ +// 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 + +#include +#include +#include +#include + +#include "../utils/expect_eq_macros.hpp" + +using namespace simple_sensor_simulator; + +/** + * @note Test basic functionality + * + * Test to vertex conversion correctness with a sample point. + */ +TEST(VertexTest, toVertex_onePoint) +{ + const auto point = geometry_msgs::build().x(1.0).y(2.0).z(3.0); + + Vertex vertex = toVertex(point); + + EXPECT_VERTEX_AND_POINT_EQ(vertex, point); +} + +/** + * @note Test basic functionality + * + * Test to vertex conversion correctness with a vector containing multiple sample points. + */ +TEST(VertexTest, toVertex_manyPoints) +{ + const std::vector points = { + geometry_msgs::build().x(1.0).y(2.0).z(3.0), + geometry_msgs::build().x(4.0).y(5.0).z(6.0), + geometry_msgs::build().x(7.0).y(8.0).z(9.0)}; + + std::vector vertices = toVertex(points); + + ASSERT_EQ(vertices.size(), points.size()); + for (size_t i = 0; i < vertices.size(); ++i) { + EXPECT_VERTEX_AND_POINT_EQ(vertices[i], points[i]); + } +} + +/** + * @note Test function behavior when an empty vector is passed + * + * The goal is to get an empty vector. + */ +TEST(VertexTest, toVertex_empty) +{ + std::vector points; + + std::vector vertices = toVertex(points); + + EXPECT_TRUE(vertices.empty()); +} + +/** + * @note Test basic functionality + * + * Test to point conversion correctness with a sample vertex. + */ +TEST(VertexTest, toPoint_oneVertex) +{ + const Vertex vertex{1.0, 2.0, 3.0}; + + geometry_msgs::msg::Point point = toPoint(vertex); + + EXPECT_VERTEX_AND_POINT_EQ(point, vertex); +} + +/** + * @note Test basic functionality + * + * Test to point conversion correctness with a vector containing multiple sample vertices. + */ +TEST(VertexTest, toPoints_manyVertices) +{ + const std::vector vertices = {{1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}, {7.0, 8.0, 9.0}}; + + std::vector points = toPoints(vertices); + + ASSERT_EQ(points.size(), vertices.size()); + for (size_t i = 0; i < points.size(); ++i) { + EXPECT_VERTEX_AND_POINT_EQ(points[i], vertices[i]); + } +} + +/** + * @note Test function behavior when an empty vector is passed + * + * The goal is to get an empty vector. + */ +TEST(VertexTest, toPoints_empty) +{ + std::vector vertices; + + std::vector points = toPoints(vertices); + + EXPECT_TRUE(points.empty()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp b/simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp new file mode 100644 index 00000000000..c93387428cc --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp @@ -0,0 +1,68 @@ +// 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__UTILS__EXPECT_EQ_MACROS_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__TEST__UTILS__EXPECT_EQ_MACROS_HPP_ + +#include + +#include +#include + +#define EXPECT_POINT_EQ(DATA0, DATA1) \ + EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x); \ + EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \ + EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); + +#define EXPECT_POINT_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \ + EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \ + EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE); + +#define EXPECT_QUATERNION_EQ(DATA0, DATA1) \ + EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x); \ + EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \ + EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); \ + EXPECT_DOUBLE_EQ(DATA0.w, DATA1.w); + +#define EXPECT_QUATERNION_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \ + EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \ + EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE); \ + EXPECT_NEAR(DATA0.w, DATA1.w, TOLERANCE); + +#define EXPECT_POSE_EQ(DATA0, DATA1) \ + EXPECT_POINT_EQ(DATA0.position, DATA1.position); \ + EXPECT_QUATERNION_EQ(DATA0.orientation, DATA1.orientation); + +#define EXPECT_POSE_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_POINT_NEAR(DATA0.position, DATA1.position, TOLERANCE); \ + EXPECT_QUATERNION_NEAR(DATA0.orientation, DATA1.orientation, TOLERANCE); + +#define EXPECT_VERTEX_EQ(DATA0, DATA1) \ + EXPECT_FLOAT_EQ(DATA0.x, DATA1.x); \ + EXPECT_FLOAT_EQ(DATA0.y, DATA1.y); \ + EXPECT_FLOAT_EQ(DATA0.z, DATA1.z); + +#define EXPECT_TRIANGLE_EQ(DATA0, DATA1) \ + EXPECT_EQ(DATA0.v0, DATA1.v0); \ + EXPECT_EQ(DATA0.v1, DATA1.v1); \ + EXPECT_EQ(DATA0.v2, DATA1.v2); + +#define EXPECT_VERTEX_AND_POINT_EQ(VERTEX, POINT) \ + EXPECT_FLOAT_EQ(VERTEX.x, POINT.x); \ + EXPECT_FLOAT_EQ(VERTEX.y, POINT.y); \ + EXPECT_FLOAT_EQ(VERTEX.z, POINT.z); + +#endif // SIMPLE_SENSOR_SIMULATOR__TEST__UTILS__EXPECT_EQ_MACROS_HPP_ From 40035b15dabccdd0db612cb8cae0c0e3528cd4b9 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Wed, 12 Jun 2024 15:18:51 +0200 Subject: [PATCH 068/118] test: [RJD-937] to Implement Unit tests on simple_sensor_simulator - Added unit tests to Box and Vertex classes - Added unit tests to Raycaster class - Added set of macros used in unit tests --- .../simple_sensor_simulator/CMakeLists.txt | 4 +- .../test/src/lidar/test_raycaster.cpp | 152 ++++++++++++++++++ .../test/src/lidar/test_raycaster.hpp | 77 +++++++++ .../test/src/primitives/test_box.cpp | 70 ++++++++ .../test/src/primitives/test_vertex.cpp | 123 ++++++++++++++ .../test/src/utils/expect_eq_macros.hpp | 68 ++++++++ 6 files changed, 493 insertions(+), 1 deletion(-) create mode 100644 simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.hpp create mode 100644 simulation/simple_sensor_simulator/test/src/primitives/test_box.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/primitives/test_vertex.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt index df6f898805f..692063321aa 100644 --- a/simulation/simple_sensor_simulator/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/CMakeLists.txt @@ -84,7 +84,9 @@ EXPORT export_simple_sensor_simulator_component if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) ament_lint_auto_find_test_dependencies() -endif() + add_subdirectory(test) +endif() ament_auto_package() diff --git a/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.cpp b/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.cpp new file mode 100644 index 00000000000..57a7308a6e8 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.cpp @@ -0,0 +1,152 @@ +// 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_raycaster.hpp" + +/** + * @note Test basic functionality + * + * Test adding a primitive to scene correctness with a Box. + */ +TEST_F(RaycasterTest, addPrimitive_box) +{ + EXPECT_NO_THROW(raycaster_->addPrimitive( + box_name_, box_depth_, box_width_, box_height_, box_pose_)); +} + +/** + * @note Test function behavior when adding two Boxes with identical names to a scene. + */ +TEST_F(RaycasterTest, addPrimitive_twoIdenticalNames) +{ + raycaster_->addPrimitive( + box_name_, box_depth_, box_width_, box_height_, box_pose_); + + EXPECT_THROW( + raycaster_->addPrimitive( + box_name_, box_depth_, box_width_, box_height_, box_pose_), + std::runtime_error); +} + +/** + * @note Test basic functionality + * + * Test raycasting correctness with an empty scene. + */ +TEST_F(RaycasterTest, raycast_empty) +{ + auto cloud = raycaster_->raycast(frame_id_, stamp_, origin_); + + EXPECT_EQ(cloud.width, 0); + EXPECT_EQ(cloud.header.frame_id, frame_id_); + EXPECT_EQ(cloud.header.stamp, stamp_); +} + +/** + * @note Test basic functionality + * + * Test raycasting correctness with one box on the scene. + */ +TEST_F(RaycasterTest, raycast_box) +{ + raycaster_->addPrimitive( + box_name_, box_depth_, box_width_, box_height_, box_pose_); + + auto cloud = raycaster_->raycast(frame_id_, stamp_, origin_); + + EXPECT_GT(cloud.width, 0); + EXPECT_EQ(cloud.header.frame_id, frame_id_); + EXPECT_EQ(cloud.header.stamp, stamp_); +} + +/** + * @note Test basic functionality + * + * Test setting ray directions with a lidar configuration that has one ray which intersects with the + * only box on the scene. + */ +TEST_F(RaycasterTest, setDirection_oneBox) +{ + raycaster_->addPrimitive( + box_name_, box_depth_, box_width_, box_height_, box_pose_); + + simulation_api_schema::LidarConfiguration config; + config.add_vertical_angles(0.0); // Only one vertical angle for a horizontal ring + config.set_horizontal_resolution(degToRad(1.0)); // Set horizontal resolution to 1 degree + + raycaster_->setDirection(config); + + auto cloud = raycaster_->raycast(frame_id_, stamp_, origin_); + + EXPECT_GT(cloud.width, 0); + EXPECT_EQ(cloud.header.frame_id, frame_id_); + EXPECT_EQ(cloud.header.stamp, stamp_); +} + +/** + * @note Test basic functionality + * + * Test setting ray directions with a lidar configuration that has a ring of horizontal rays which + * intersect with boxes positioned on the ring so that they intersect with the rays. + */ +TEST_F(RaycasterTest, setDirection_manyBoxes) +{ + constexpr double radius = 5.0; + constexpr int num_boxes = 10; + constexpr double angle_increment = 2 * M_PI / num_boxes; + + for (int i = 0; i < num_boxes; ++i) { + const double angle = i * angle_increment; + const auto box_pose = + geometry_msgs::build() + .position(geometry_msgs::build() + .x(radius * cos(angle)) + .y(radius * sin(angle)) + .z(0)) + .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); + + std::string name = "box" + std::to_string(i); + raycaster_->addPrimitive(name, box_depth_, box_width_, box_height_, box_pose); + } + + simulation_api_schema::LidarConfiguration config; + config.add_vertical_angles(0.0); // Only one vertical angle for a horizontal ring + config.set_horizontal_resolution(degToRad(5)); + + raycaster_->setDirection(config); + + auto cloud = raycaster_->raycast(frame_id_, stamp_, origin_); + + EXPECT_GT(cloud.width, 0); + EXPECT_EQ(cloud.header.frame_id, frame_id_); + EXPECT_EQ(cloud.header.stamp, stamp_); +} + +/** + * @note Test basic functionality + * + * Test detected objects obtaining from the statuses list containing Ego. + */ +TEST_F(RaycasterTest, getDetectedObjects) +{ + raycaster_->addPrimitive( + box_name_, box_depth_, box_width_, box_height_, box_pose_); + + raycaster_->raycast(frame_id_, stamp_, origin_); + + const auto & detected_objects = raycaster_->getDetectedObject(); + + ASSERT_FALSE(detected_objects.empty()); + EXPECT_EQ(detected_objects[0], box_name_); +} diff --git a/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.hpp b/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.hpp new file mode 100644 index 00000000000..37fad304554 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.hpp @@ -0,0 +1,77 @@ +// 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_RAYCASTER_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__TEST__TEST_RAYCASTER_HPP_ + +#include + +#include +#include +#include +#include +#include + +using namespace simple_sensor_simulator; +using namespace geometry_msgs::msg; + +constexpr static double degToRad(double deg) { return deg * M_PI / 180.0; } + +class RaycasterTest : public ::testing::Test +{ +protected: + void SetUp() override + { + raycaster_ = std::make_unique(); + configureLidar(); + + origin_ = + geometry_msgs::build() + .position(geometry_msgs::build().x(0).y(0).z(0)) + .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); + box_pose_ = + geometry_msgs::build() + .position(geometry_msgs::build().x(5.0).y(0).z(0)) + .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); + } + + std::unique_ptr raycaster_; + simulation_api_schema::LidarConfiguration config_; + + const float box_depth_{1.0f}; + const float box_width_{1.0f}; + const float box_height_{1.0f}; + const std::string box_name_{"box"}; + + const rclcpp::Time stamp_{0}; + const std::string frame_id_{"frame_id"}; + + geometry_msgs::msg::Pose origin_; + geometry_msgs::msg::Pose box_pose_; + +private: + auto configureLidar() -> void + { + // Setting vertical angles from -15 to +15 degrees in 2 degree steps + for (double angle = -15.0; angle <= 15.0; angle += 2.0) { + config_.add_vertical_angles(degToRad(angle)); + } + // Setting horizontal resolution to 0.5 degrees + config_.set_horizontal_resolution(degToRad(0.5)); + + raycaster_->setDirection(config_); + } +}; + +#endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_RAYCASTER_HPP_ diff --git a/simulation/simple_sensor_simulator/test/src/primitives/test_box.cpp b/simulation/simple_sensor_simulator/test/src/primitives/test_box.cpp new file mode 100644 index 00000000000..5543e200cba --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/primitives/test_box.cpp @@ -0,0 +1,70 @@ +// 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 +#include + +#include +#include +#include + +#include "../utils/expect_eq_macros.hpp" + +using namespace simple_sensor_simulator; +using namespace simple_sensor_simulator::primitives; + +/** + * @note Test initialization correctness + * + * The goal is to test whether the vertices and indices are initialized to create a box shape (can + * use get2DConvexHull or other specially created const reference accessors). + */ +TEST(BoxTest, Box) +{ + const std::vector expected_vertices = { + {-0.5, -0.5, -0.5}, {-0.5, -0.5, 0.5}, {-0.5, 0.5, -0.5}, {-0.5, 0.5, 0.5}, + {0.5, -0.5, -0.5}, {0.5, -0.5, 0.5}, {0.5, 0.5, -0.5}, {0.5, 0.5, 0.5}}; + + // Indices + const std::vector expected_triangles = {{0, 1, 2}, {1, 3, 2}, {4, 6, 5}, {5, 6, 7}, + {0, 4, 1}, {1, 4, 5}, {2, 3, 6}, {3, 7, 6}, + {0, 2, 4}, {2, 6, 4}, {1, 5, 3}, {3, 5, 7}}; + + const auto pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(0).y(0).z(0)) + .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); + + Box box(1.0, 1.0, 1.0, pose); + + auto vertices = box.getVertex(); + auto triangles = box.getTriangles(); + + ASSERT_EQ(triangles.size(), expected_triangles.size()); + ASSERT_EQ(vertices.size(), expected_vertices.size()); + + for (size_t i = 0; i < triangles.size(); ++i) { + EXPECT_TRIANGLE_EQ(triangles[i], expected_triangles[i]); + } + + for (size_t i = 0; i < vertices.size(); ++i) { + EXPECT_VERTEX_EQ(vertices[i], expected_vertices[i]); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/primitives/test_vertex.cpp b/simulation/simple_sensor_simulator/test/src/primitives/test_vertex.cpp new file mode 100644 index 00000000000..353507175c5 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/primitives/test_vertex.cpp @@ -0,0 +1,123 @@ +// 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 + +#include +#include +#include +#include + +#include "../utils/expect_eq_macros.hpp" + +using namespace simple_sensor_simulator; + +/** + * @note Test basic functionality + * + * Test to vertex conversion correctness with a sample point. + */ +TEST(VertexTest, toVertex_onePoint) +{ + const auto point = geometry_msgs::build().x(1.0).y(2.0).z(3.0); + + Vertex vertex = toVertex(point); + + EXPECT_VERTEX_AND_POINT_EQ(vertex, point); +} + +/** + * @note Test basic functionality + * + * Test to vertex conversion correctness with a vector containing multiple sample points. + */ +TEST(VertexTest, toVertex_manyPoints) +{ + const std::vector points = { + geometry_msgs::build().x(1.0).y(2.0).z(3.0), + geometry_msgs::build().x(4.0).y(5.0).z(6.0), + geometry_msgs::build().x(7.0).y(8.0).z(9.0)}; + + std::vector vertices = toVertex(points); + + ASSERT_EQ(vertices.size(), points.size()); + for (size_t i = 0; i < vertices.size(); ++i) { + EXPECT_VERTEX_AND_POINT_EQ(vertices[i], points[i]); + } +} + +/** + * @note Test function behavior when an empty vector is passed + * + * The goal is to get an empty vector. + */ +TEST(VertexTest, toVertex_empty) +{ + std::vector points; + + std::vector vertices = toVertex(points); + + EXPECT_TRUE(vertices.empty()); +} + +/** + * @note Test basic functionality + * + * Test to point conversion correctness with a sample vertex. + */ +TEST(VertexTest, toPoint_oneVertex) +{ + const Vertex vertex{1.0, 2.0, 3.0}; + + geometry_msgs::msg::Point point = toPoint(vertex); + + EXPECT_VERTEX_AND_POINT_EQ(point, vertex); +} + +/** + * @note Test basic functionality + * + * Test to point conversion correctness with a vector containing multiple sample vertices. + */ +TEST(VertexTest, toPoints_manyVertices) +{ + const std::vector vertices = {{1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}, {7.0, 8.0, 9.0}}; + + std::vector points = toPoints(vertices); + + ASSERT_EQ(points.size(), vertices.size()); + for (size_t i = 0; i < points.size(); ++i) { + EXPECT_VERTEX_AND_POINT_EQ(points[i], vertices[i]); + } +} + +/** + * @note Test function behavior when an empty vector is passed + * + * The goal is to get an empty vector. + */ +TEST(VertexTest, toPoints_empty) +{ + std::vector vertices; + + std::vector points = toPoints(vertices); + + EXPECT_TRUE(points.empty()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp b/simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp new file mode 100644 index 00000000000..c93387428cc --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp @@ -0,0 +1,68 @@ +// 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__UTILS__EXPECT_EQ_MACROS_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__TEST__UTILS__EXPECT_EQ_MACROS_HPP_ + +#include + +#include +#include + +#define EXPECT_POINT_EQ(DATA0, DATA1) \ + EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x); \ + EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \ + EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); + +#define EXPECT_POINT_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \ + EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \ + EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE); + +#define EXPECT_QUATERNION_EQ(DATA0, DATA1) \ + EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x); \ + EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \ + EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); \ + EXPECT_DOUBLE_EQ(DATA0.w, DATA1.w); + +#define EXPECT_QUATERNION_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \ + EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \ + EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE); \ + EXPECT_NEAR(DATA0.w, DATA1.w, TOLERANCE); + +#define EXPECT_POSE_EQ(DATA0, DATA1) \ + EXPECT_POINT_EQ(DATA0.position, DATA1.position); \ + EXPECT_QUATERNION_EQ(DATA0.orientation, DATA1.orientation); + +#define EXPECT_POSE_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_POINT_NEAR(DATA0.position, DATA1.position, TOLERANCE); \ + EXPECT_QUATERNION_NEAR(DATA0.orientation, DATA1.orientation, TOLERANCE); + +#define EXPECT_VERTEX_EQ(DATA0, DATA1) \ + EXPECT_FLOAT_EQ(DATA0.x, DATA1.x); \ + EXPECT_FLOAT_EQ(DATA0.y, DATA1.y); \ + EXPECT_FLOAT_EQ(DATA0.z, DATA1.z); + +#define EXPECT_TRIANGLE_EQ(DATA0, DATA1) \ + EXPECT_EQ(DATA0.v0, DATA1.v0); \ + EXPECT_EQ(DATA0.v1, DATA1.v1); \ + EXPECT_EQ(DATA0.v2, DATA1.v2); + +#define EXPECT_VERTEX_AND_POINT_EQ(VERTEX, POINT) \ + EXPECT_FLOAT_EQ(VERTEX.x, POINT.x); \ + EXPECT_FLOAT_EQ(VERTEX.y, POINT.y); \ + EXPECT_FLOAT_EQ(VERTEX.z, POINT.z); + +#endif // SIMPLE_SENSOR_SIMULATOR__TEST__UTILS__EXPECT_EQ_MACROS_HPP_ From 529060e63ff11a8fc5e24f8fd2ca78755bcdb1b1 Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Thu, 13 Jun 2024 14:53:48 +0900 Subject: [PATCH 069/118] empty From bf2da1223b40d355e05b3dc0c8ac2d04a19c7b7f Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Thu, 13 Jun 2024 15:12:58 +0900 Subject: [PATCH 070/118] fix --- .../quaternion/euler_to_quaternion.hpp | 11 +++++----- .../geometry/quaternion/get_rotation.hpp | 4 ++-- .../quaternion/get_rotation_matrix.hpp | 8 ++++--- .../quaternion/quaternion_to_euler.hpp | 6 +----- .../include/geometry/quaternion/slerp.hpp | 21 +++++++++---------- .../openscenario_interpreter/CHANGELOG.rst | 2 +- .../scenario_test_runner/CHANGELOG.rst | 2 +- 7 files changed, 25 insertions(+), 29 deletions(-) diff --git a/common/math/geometry/include/geometry/quaternion/euler_to_quaternion.hpp b/common/math/geometry/include/geometry/quaternion/euler_to_quaternion.hpp index 0106164c027..5be86f2a714 100644 --- a/common/math/geometry/include/geometry/quaternion/euler_to_quaternion.hpp +++ b/common/math/geometry/include/geometry/quaternion/euler_to_quaternion.hpp @@ -29,17 +29,16 @@ template < typename T, std::enable_if_t>, std::nullptr_t> = nullptr> auto convertEulerAngleToQuaternion(const T & v) { - geometry_msgs::msg::Quaternion ret; const double & roll = v.x; const double & pitch = v.y; const double & yaw = v.z; tf2::Quaternion tf_quat; tf_quat.setRPY(roll, pitch, yaw); - ret.x = tf_quat.x(); - ret.y = tf_quat.y(); - ret.z = tf_quat.z(); - ret.w = tf_quat.w(); - return ret; + return geometry_msgs::build() + .x(tf_quat.x()) + .y(tf_quat.y()) + .z(tf_quat.z()) + .w(tf_quat.w()); } } // namespace geometry } // namespace math diff --git a/common/math/geometry/include/geometry/quaternion/get_rotation.hpp b/common/math/geometry/include/geometry/quaternion/get_rotation.hpp index 16e64ff192f..d99df0db6ba 100644 --- a/common/math/geometry/include/geometry/quaternion/get_rotation.hpp +++ b/common/math/geometry/include/geometry/quaternion/get_rotation.hpp @@ -31,11 +31,11 @@ template < nullptr> auto getRotation(T from, U to) { + // This code is inverting the sign of the coordinates (x, y, z) of the 'from' vector. from.x *= -1; from.y *= -1; from.z *= -1; - auto ans = from * to; - return ans; + return from * to; } } // namespace geometry } // namespace math diff --git a/common/math/geometry/include/geometry/quaternion/get_rotation_matrix.hpp b/common/math/geometry/include/geometry/quaternion/get_rotation_matrix.hpp index 7e430fe2fe5..c12b53dd812 100644 --- a/common/math/geometry/include/geometry/quaternion/get_rotation_matrix.hpp +++ b/common/math/geometry/include/geometry/quaternion/get_rotation_matrix.hpp @@ -37,9 +37,11 @@ auto getRotationMatrix(T quat) auto z = quat.z; auto w = quat.w; Eigen::Matrix3d ret(3, 3); - ret << x * x - y * y - z * z + w * w, 2 * (x * y - z * w), 2 * (z * x + w * y), - 2 * (x * y + z * w), -x * x + y * y - z * z + w * w, 2 * (y * z - x * w), 2 * (z * x - w * y), - 2 * (y * z + w * x), -x * x - y * y + z * z + w * w; + // clang-format off + ret << x * x - y * y - z * z + w * w, 2 * (x * y - z * w), 2 * (z * x + w * y), + 2 * (x * y + z * w), -x * x + y * y - z * z + w * w, 2 * (y * z - x * w), + 2 * (z * x - w * y), 2 * (y * z + w * x), -x * x - y * y + z * z + w * w; + // clang-format on return ret; } } // namespace geometry diff --git a/common/math/geometry/include/geometry/quaternion/quaternion_to_euler.hpp b/common/math/geometry/include/geometry/quaternion/quaternion_to_euler.hpp index e73a8c03ea0..72901ccb8e0 100644 --- a/common/math/geometry/include/geometry/quaternion/quaternion_to_euler.hpp +++ b/common/math/geometry/include/geometry/quaternion/quaternion_to_euler.hpp @@ -29,15 +29,11 @@ template < typename T, std::enable_if_t>, std::nullptr_t> = nullptr> auto convertQuaternionToEulerAngle(const T & q) { - geometry_msgs::msg::Vector3 ret; tf2::Quaternion tf_quat(q.x, q.y, q.z, q.w); tf2::Matrix3x3 mat(tf_quat); double roll, pitch, yaw; mat.getRPY(roll, pitch, yaw); - ret.x = roll; - ret.y = pitch; - ret.z = yaw; - return ret; + return geometry_msgs::build().x(roll).y(pitch).z(yaw); } } // namespace geometry } // namespace math diff --git a/common/math/geometry/include/geometry/quaternion/slerp.hpp b/common/math/geometry/include/geometry/quaternion/slerp.hpp index 0b91e64828d..1207de24277 100644 --- a/common/math/geometry/include/geometry/quaternion/slerp.hpp +++ b/common/math/geometry/include/geometry/quaternion/slerp.hpp @@ -29,16 +29,15 @@ template < std::nullptr_t> = nullptr> auto slerp(T quat1, U quat2, V t) { - geometry_msgs::msg::Quaternion q; double qr = quat1.w * quat2.w + quat1.x * quat2.x + quat1.y * quat2.y + quat1.z * quat2.z; double ss = 1.0 - qr * qr; constexpr double e = std::numeric_limits::epsilon(); if (std::fabs(ss) <= e) { - q.w = quat1.w; - q.x = quat1.x; - q.y = quat1.y; - q.z = quat1.z; - return q; + return geometry_msgs::build() + .x(quat1.x) + .y(quat1.y) + .z(quat1.z) + .w(quat1.w); } else { double sp = std::sqrt(ss); double ph = std::acos(qr); @@ -46,11 +45,11 @@ auto slerp(T quat1, U quat2, V t) double t1 = std::sin(pt) / sp; double t0 = std::sin(ph - pt) / sp; - q.w = quat1.w * t0 + quat2.w * t1; - q.x = quat1.x * t0 + quat2.x * t1; - q.y = quat1.y * t0 + quat2.y * t1; - q.z = quat1.z * t0 + quat2.z * t1; - return q; + return geometry_msgs::build() + .x(quat1.x * t0 + quat2.x * t1) + .y(quat1.y * t0 + quat2.y * t1) + .z(quat1.z * t0 + quat2.z * t1) + .w(quat1.w * t0 + quat2.w * t1); } } } // namespace geometry diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index f4765ffbefb..c7ec857f78a 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -2857,7 +2857,7 @@ Changelog for package openscenario_interpreter * Lipsticks * Add optional argument verbose (= True) to openscenario_utility * Rename 'step_time_ms' to 'frame-rate' -* Fix external/math::geometry's commit hash +* Fix external/quaternion_operation's commit hash * Add parameter 'real-time-factor' to openscenario_interpreter * enable pass colcon test * enable pass test case diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 6237bae730d..1327d525f1f 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -1735,7 +1735,7 @@ Changelog for package scenario_test_runner * Rename option 'timeout' to 'global-timeout' * Add optional argument verbose (= True) to openscenario_utility * Rename 'step_time_ms' to 'frame-rate' -* Fix external/math::geometry's commit hash +* Fix external/quaternion_operation's commit hash * Add parameter 'real-time-factor' to openscenario_interpreter * Rename option to '--global-real-time-factor' from 'real-time-factor' * Add launch argument '--real-time-factor' From 0e9e6d2225edf10c8430d3bcf6d05fa6dda14fbe Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Thu, 13 Jun 2024 16:24:46 +0900 Subject: [PATCH 071/118] Update common/math/geometry/include/geometry/quaternion/get_rotation.hpp --- .../math/geometry/include/geometry/quaternion/get_rotation.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/math/geometry/include/geometry/quaternion/get_rotation.hpp b/common/math/geometry/include/geometry/quaternion/get_rotation.hpp index d99df0db6ba..57a89cc863d 100644 --- a/common/math/geometry/include/geometry/quaternion/get_rotation.hpp +++ b/common/math/geometry/include/geometry/quaternion/get_rotation.hpp @@ -31,7 +31,7 @@ template < nullptr> auto getRotation(T from, U to) { - // This code is inverting the sign of the coordinates (x, y, z) of the 'from' vector. + // This function gets the conjugate of the quaternion. from.x *= -1; from.y *= -1; from.z *= -1; From e2b12fadbe595ebf6909ea805157468721d48397 Mon Sep 17 00:00:00 2001 From: Release Bot Date: Thu, 13 Jun 2024 07:26:04 +0000 Subject: [PATCH 072/118] Bump version of scenario_simulator_v2 from version 2.1.1 to version 2.1.2 --- common/math/arithmetic/CHANGELOG.rst | 14 +++++++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 17 +++++++++++ common/math/geometry/package.xml | 2 +- .../CHANGELOG.rst | 14 +++++++++ .../scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 14 +++++++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 13 ++++++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 18 +++++++++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 9 ++++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 13 ++++++++ map/kashiwanoha_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 19 ++++++++++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../CHANGELOG.rst | 14 +++++++++ .../package.xml | 2 +- .../openscenario_interpreter/CHANGELOG.rst | 30 +++++++++++++++++++ .../openscenario_interpreter/package.xml | 2 +- .../CHANGELOG.rst | 13 ++++++++ .../package.xml | 2 +- .../CHANGELOG.rst | 13 ++++++++ .../openscenario_interpreter_msgs/package.xml | 2 +- .../openscenario_preprocessor/CHANGELOG.rst | 15 ++++++++++ .../openscenario_preprocessor/package.xml | 2 +- .../CHANGELOG.rst | 14 +++++++++ .../package.xml | 2 +- .../openscenario_utility/CHANGELOG.rst | 13 ++++++++ openscenario/openscenario_utility/package.xml | 2 +- .../openscenario_validator/CHANGELOG.rst | 8 +++++ .../openscenario_validator/package.xml | 2 +- .../openscenario_visualization/CHANGELOG.rst | 10 +++++++ .../openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 10 +++++++ .../package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 13 ++++++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 18 +++++++++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 18 +++++++++++ simulation/do_nothing_plugin/package.xml | 2 +- .../simple_sensor_simulator/CHANGELOG.rst | 17 +++++++++++ .../simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 16 ++++++++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 23 ++++++++++++++ simulation/traffic_simulator/package.xml | 2 +- .../traffic_simulator_msgs/CHANGELOG.rst | 14 +++++++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 19 ++++++++++++ test_runner/random_test_runner/package.xml | 2 +- .../scenario_test_runner/CHANGELOG.rst | 22 ++++++++++++++ test_runner/scenario_test_runner/package.xml | 2 +- 56 files changed, 459 insertions(+), 28 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 01c3e49de7b..defee849eb8 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -13,6 +13,20 @@ Changelog for package arithmetic * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index e18833712ea..7d1c45ab424 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 2.1.1 + 2.1.2 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index 62db6d0d44f..acbe9c08466 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -13,6 +13,23 @@ Changelog for package geometry * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index c055ce2349d..b788b262c3b 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 2.1.1 + 2.1.2 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index e2ff6910da9..38c2f5e56de 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -13,6 +13,20 @@ Changelog for package scenario_simulator_exception * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index e6a2c6b644c..1876c4523df 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 2.1.1 + 2.1.2 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index e6d699d5042..f6020b84cac 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -13,6 +13,20 @@ Changelog for package junit_exporter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 9aab308f0d8..cf43ebd66ed 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 2.1.1 + 2.1.2 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index fdf4a23b211..8c7c041a359 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -13,6 +13,19 @@ Changelog for package status_monitor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 4e796af2e09..2a5471e3434 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 2.1.1 + 2.1.2 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 4c424ab9c05..ea2e4afdc68 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -13,6 +13,24 @@ Changelog for package concealer * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 31108291945..80c3eaef916 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 2.1.1 + 2.1.2 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 469bdbb375b..ddf6b1eac90 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -16,6 +16,15 @@ Changelog for package embree_vendor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index da3fdf03fc5..de94adc0d3c 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 2.1.1 + 2.1.2 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 0e3b8403f64..2bbb3a99897 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -13,6 +13,19 @@ Changelog for package kashiwanoha_map * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 03b2e8f6513..a36c28ae63c 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 2.1.1 + 2.1.2 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index 227e238da34..7b43c9f1765 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -13,6 +13,25 @@ Changelog for package cpp_mock_scenarios * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index c4aadd98806..359001ee36c 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 2.1.1 + 2.1.2 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 6f3b9f30ae8..81cac3fd43b 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -13,6 +13,20 @@ Changelog for package openscenario_experimental_catalog * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index faefc7f51a6..c188cc0f86e 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 2.1.1 + 2.1.2 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index c7ec857f78a..a3fead32246 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -14,6 +14,36 @@ Changelog for package openscenario_interpreter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge pull request `#1012 `_ from tier4/fix/interpreter/fault-injection-action + Fix/interpreter/fault injection action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge pull request `#1021 `_ from tier4/fix/interpreter/fault_injection_action/qos_depth + fix(action): fix fault injection action - qos +* ref(action): apply clang_format +* fix(action): fix fault injection - history_depth in qos +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Lipsticks +* Update FaultInjection to prepare Node and Publisher in the parse phase +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 0e6d953363e..db5d4f5de94 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 2.1.1 + 2.1.2 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 8e03db065a0..730e8074d07 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -13,6 +13,19 @@ Changelog for package openscenario_interpreter_example * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 95c3cb494f2..6c4ca994cab 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 2.1.1 + 2.1.2 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index b396433f5fe..3ebd4aa71bd 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -13,6 +13,19 @@ Changelog for package openscenario_interpreter_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 90b116f3fb4..613bfd2c446 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 2.1.1 + 2.1.2 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index 9c81e7eb4b1..fcf10055cfa 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -13,6 +13,21 @@ Changelog for package openscenario_preprocessor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 426173bef33..55e8cca9ddc 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 2.1.1 + 2.1.2 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index f71febe9521..c09df8ae481 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -13,6 +13,20 @@ Changelog for package openscenario_preprocessor_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index f62a2ae8063..f94f8a09bcf 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 2.1.1 + 2.1.2 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 5d752088053..a29dbc2efce 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -16,6 +16,19 @@ Changelog for package openscenario_utility * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 91c10be90ae..04e2c7450e7 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 2.1.1 + 2.1.2 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index b60366f87f4..4cb6e150fad 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package openscenario_validator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 440094b65d3..fc5a2d1d563 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 2.1.1 + 2.1.2 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 88425f2d625..cd5586df7b4 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -13,6 +13,16 @@ Changelog for package openscenario_visualization * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 26ad5618fca..0a0d47b72f2 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 2.1.1 + 2.1.2 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 39c308da9a9..8ddb2fcb56b 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -13,6 +13,16 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index 331f9ec27d5..b88361134b3 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 2.1.1 + 2.1.2 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 0213c0475ae..eb265498ad0 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -13,6 +13,19 @@ Changelog for package scenario_simulator_v2 * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 1b2cddf54b6..3117b3bf2ff 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 2.1.1 + 2.1.2 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 7d2c5505f3a..9b29186f898 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -13,6 +13,24 @@ Changelog for package behavior_tree_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index f1bdc5167e1..249c7ed2f1f 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 2.1.1 + 2.1.2 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index c682b45fbe2..c7d760eaaa7 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -13,6 +13,24 @@ Changelog for package do_nothing_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index aec31e098b1..b4cbcbf8363 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 2.1.1 + 2.1.2 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index f216f8cbb91..76adbddd9ff 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -13,6 +13,23 @@ Changelog for package simple_sensor_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge pull request `#1279 `_ from tier4/fix/reorder diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 8e04fc1f30a..a9b8b1414b8 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 2.1.1 + 2.1.2 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index ca76dc214dc..89fc8a2fcd6 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -13,6 +13,22 @@ Changelog for package simulation_interface * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index d42e4cc07cc..9e22b525c5f 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 2.1.1 + 2.1.2 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index 59527a69f27..b25ffae3c02 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -13,6 +13,29 @@ Changelog for package traffic_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index a5ff0d45510..e28c0afc8d1 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 2.1.1 + 2.1.2 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 0c936582005..876d54227a6 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -13,6 +13,20 @@ Changelog for package openscenario_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index f93910f4943..5a02bf941ca 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 2.1.1 + 2.1.2 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 0bb935cd39d..0dd69e8b361 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -13,6 +13,25 @@ Changelog for package random_test_runner * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 420d2fb6e11..a5813e1257e 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 2.1.1 + 2.1.2 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 1327d525f1f..38528ed15cd 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -23,6 +23,28 @@ Changelog for package scenario_test_runner * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.2 (2024-06-13) +------------------ +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge branch 'master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + 2.1.1 (2024-06-11) ------------------ * Merge branch 'master' into fix/reorder diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 103cdd0135d..c3616df9913 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 2.1.1 + 2.1.2 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From aaeba299d7c639e763eeb946e1078de74d427fa3 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 13 Jun 2024 12:32:13 +0200 Subject: [PATCH 073/118] Declare parameters before getting values Signed-off-by: Mateusz Palczuk --- simulation/traffic_simulator/src/entity/ego_entity.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index b9adbac4b89..d74de1753d4 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -47,15 +47,16 @@ auto EgoEntity::makeFieldOperatorApplication( return node_parameter_handler->getParameterOrDeclare("launch_autoware", true) ? std::make_unique< concealer::FieldOperatorApplicationFor>( - node_parameter_handler->getParameter("autoware_launch_package"), - node_parameter_handler->getParameter("autoware_launch_file"), + node_parameter_handler->getParameterOrDeclare( + "autoware_launch_package"), + node_parameter_handler->getParameterOrDeclare("autoware_launch_file"), "map_path:=" + configuration.map_path.string(), "lanelet2_map_file:=" + configuration.getLanelet2MapFile(), "pointcloud_map_file:=" + configuration.getPointCloudMapFile(), "sensor_model:=" + - node_parameter_handler->getParameter("sensor_model"), + node_parameter_handler->getParameterOrDeclare("sensor_model"), "vehicle_model:=" + - node_parameter_handler->getParameter("vehicle_model"), + node_parameter_handler->getParameterOrDeclare("vehicle_model"), "rviz_config:=" + ((rviz_config == "") ? configuration.rviz_config_path.string() : Configuration::Pathname(rviz_config).string()), From e82c4510cdc14b8c976a577e9197d4f1b4380d04 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 13 Jun 2024 13:39:58 +0200 Subject: [PATCH 074/118] setting up zmq server --- .../simple_sensor_simulator/CMakeLists.txt | 3 + .../test/CMakeLists.txt | 3 +- .../src/vehicle_simulation/CMakeLists.txt | 2 + .../test_scenario_simulator.cpp | 85 +++++++++++++++++++ 4 files changed, 91 insertions(+), 2 deletions(-) create mode 100644 simulation/simple_sensor_simulator/test/src/vehicle_simulation/CMakeLists.txt create mode 100644 simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_scenario_simulator.cpp diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt index 2cfa16ebc5c..3b649cffe22 100644 --- a/simulation/simple_sensor_simulator/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/CMakeLists.txt @@ -88,6 +88,9 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() add_subdirectory(test) + find_package(Protobuf REQUIRED) + include_directories(${Protobuf_INCLUDE_DIRS}) + target_link_libraries(simple_sensor_simulator_component ${Protobuf_LIBRARIES}) endif() ament_auto_package() diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt index 138711416a0..ff941219a36 100644 --- a/simulation/simple_sensor_simulator/test/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/CMakeLists.txt @@ -10,5 +10,4 @@ target_link_libraries(test_vertex simple_sensor_simulator_component) ament_add_gtest(test_raycaster src/lidar/test_raycaster.cpp) target_link_libraries(test_raycaster simple_sensor_simulator_component ${Protobuf_LIBRARIES}) -ament_add_gtest(test_grid_traversal src/lidar/test_grid_traversal.cpp) -target_link_libraries(test_grid_traversal simple_sensor_simulator_component) \ No newline at end of file +add_subdirectory(src/vehicle_simulation) \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/CMakeLists.txt new file mode 100644 index 00000000000..4c46707be29 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_scenario_simulator test_scenario_simulator.cpp) +target_link_libraries(test_scenario_simulator simple_sensor_simulator_component) diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_scenario_simulator.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_scenario_simulator.cpp new file mode 100644 index 00000000000..f0db0a5f87d --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_scenario_simulator.cpp @@ -0,0 +1,85 @@ +// 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 + +#include +#include +#include +#include +#include + +#include "../utils/expect_eq_macros.hpp" + +class Client +{ +public: + Client() + : context(), socket(this->context, zmqpp::socket_type::request), address{"tcp://localhost:5555"} + { + socket.connect(address); + } + auto send(zmqpp::message & msg) -> void { socket.send(msg); } + auto recv(zmqpp::message & msg) -> void { socket.receive(msg); } + +private: + zmqpp::context context; + zmqpp::socket socket; + std::string address; +}; + +int main(int argc, char ** argv) +{ + auto server = std::thread([&argc, &argv] { + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + }); + + auto client = Client(); + + auto clock = traffic_simulator::SimulationClock(true, 1.0, 10.0); + auto request = simulation_api_schema::InitializeRequest(); + request.set_realtime_factor(1.0); + request.set_step_time(0.1); + request.set_initialize_time(0.0); + simulation_interface::toProto(clock.getCurrentRosTime(), *request.mutable_initialize_ros_time()); + request.set_lanelet2_map_path( + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"); + + zmqpp::message req_message = zeromq::toZMQ(request); + std::cout << "REQ\n" << request.DebugString() << std::endl; + client.send(req_message); + + zmqpp::message rep_message; + client.recv(rep_message); + + auto response = zeromq::toProto(rep_message); + std::cout << "REP\n" << response.DebugString() << std::endl; + if (response.result().success()) { + std::cout << "worked lol\n"; + } else { + std::cout << ":(\n" << response.result().DebugString() << std::endl; + } + + { + zmqpp::message my_final_message; + my_final_message << "destroy"; + client.send(my_final_message); + } + server.join(); + return 0; +} \ No newline at end of file From 39723a71f2e49fe96e0c203f94719ab91d0e6023 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 13 Jun 2024 16:19:33 +0200 Subject: [PATCH 075/118] Revert changes adding parameter checking After thic change the code is functionally the same as in the beginning Signed-off-by: Mateusz Palczuk --- .../src/cpp_scenario_node.cpp | 2 +- .../simulator_core.hpp | 6 ++-- .../include/traffic_simulator/api/api.hpp | 7 ++--- .../entity/entity_manager.hpp | 7 ++--- .../utils/node_parameter_handler.hpp | 29 +++++++++---------- simulation/traffic_simulator/src/api/api.cpp | 6 ++-- .../src/entity/ego_entity.cpp | 18 +++++------- 7 files changed, 34 insertions(+), 41 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index 4817ef28752..b339e781252 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -101,7 +101,7 @@ void CppScenarioNode::spawnEgoEntity( api_.attachOccupancyGridSensor([this] { simulation_api_schema::OccupancyGridSensorConfiguration configuration; // clang-format off - configuration.set_architecture_type(api_.getParameterOrDeclare("architecture_type", "awf/universe")); + configuration.set_architecture_type(api_.getParameter("architecture_type", "awf/universe")); configuration.set_entity("ego"); configuration.set_filter_by_range(true); configuration.set_height(200); diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 42f3b828c96..760a54caba9 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -338,7 +338,7 @@ class SimulatorCore core->attachDetectionSensor([&]() { simulation_api_schema::DetectionSensorConfiguration configuration; // clang-format off - configuration.set_architecture_type(core->getParameterOrDeclare("architecture_type", "awf/universe")); + configuration.set_architecture_type(core->getParameter("architecture_type", "awf/universe")); configuration.set_entity(entity_ref); configuration.set_detect_all_objects_in_range(controller.properties.template get("isClairvoyant")); configuration.set_object_recognition_delay(controller.properties.template get("detectedObjectPublishingDelay")); @@ -355,7 +355,7 @@ class SimulatorCore core->attachOccupancyGridSensor([&]() { simulation_api_schema::OccupancyGridSensorConfiguration configuration; // clang-format off - configuration.set_architecture_type(core->getParameterOrDeclare("architecture_type", "awf/universe")); + configuration.set_architecture_type(core->getParameter("architecture_type", "awf/universe")); configuration.set_entity(entity_ref); configuration.set_filter_by_range(controller.properties.template get("isClairvoyant")); configuration.set_height(200); @@ -370,7 +370,7 @@ class SimulatorCore core->attachPseudoTrafficLightDetector([&]() { simulation_api_schema::PseudoTrafficLightDetectorConfiguration configuration; configuration.set_architecture_type( - core->getParameterOrDeclare("architecture_type", "awf/universe")); + core->getParameter("architecture_type", "awf/universe")); return configuration; }()); diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 22f37c8fd2a..d3e6bc4e1f4 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -388,11 +388,10 @@ class API public: #undef FORWARD_TO_ENTITY_MANAGER - template - auto getParameterOrDeclare( - const std::string & name, const ParameterType & default_value = {}) const -> decltype(auto) + template + auto getParameter(const std::string & name, Ts &&... xs) const -> decltype(auto) { - return node_parameter_handler_->getParameterOrDeclare(name, default_value); + return node_parameter_handler_->getParameter(name, std::forward(xs)...); } auto canonicalize(const LaneletPose & maybe_non_canonicalized_lanelet_pose) const diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 2acce866848..bd08db62729 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -132,8 +132,8 @@ class EntityManager template auto makeV2ITrafficLightPublisher(Ts &&... xs) -> std::shared_ptr { - if (const auto architecture_type = node_parameter_handler_->getParameterOrDeclare( - "architecture_type", "awf/universe"); + if (const auto architecture_type = + node_parameter_handler_->getParameter("architecture_type", "awf/universe"); architecture_type.find("awf/universe") != std::string::npos) { return std::make_shared< TrafficLightPublisher>( @@ -499,8 +499,7 @@ class EntityManager entity_status.lanelet_pose = *lanelet_pose; entity_status.lanelet_pose_valid = true; /// @note fix z, roll and pitch to fitting to the lanelet - if (node_parameter_handler_->getParameterOrDeclare( - "consider_pose_by_road_slope", false)) { + if (node_parameter_handler_->getParameter("consider_pose_by_road_slope", false)) { entity_status.pose = hdmap_utils_ptr_->toMapPose(*lanelet_pose).pose; } else { entity_status.pose = pose; diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameter_handler.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameter_handler.hpp index e8c0937cefc..9cbf2f11722 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameter_handler.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameter_handler.hpp @@ -23,30 +23,27 @@ namespace traffic_simulator class NodeParameterHandler { public: - template - explicit NodeParameterHandler(NodeType && node) + template + explicit NodeParameterHandler(NodeT && node) : node_parameters_interface_( - rclcpp::node_interfaces::get_node_parameters_interface(std::forward(node))) + rclcpp::node_interfaces::get_node_parameters_interface(std::forward(node))) { } - template - auto getParameterOrDeclare( - const std::string & name, const ParameterType & default_value = {}) const -> ParameterType + /** + * Get parameter or declare it if it has not been declared before. Declare it with a default value. + * @param name The name of the parameter + * @param default_value The default value of the parameter + * @return The value of the parameter + */ + template + auto getParameter(const std::string & name, const ParameterT & default_value = {}) const + -> ParameterT { if (not node_parameters_interface_->has_parameter(name)) { node_parameters_interface_->declare_parameter(name, rclcpp::ParameterValue(default_value)); } - return node_parameters_interface_->get_parameter(name).get_value(); - } - - template - auto getParameter(const std::string & name) const -> ParameterType - { - if (not node_parameters_interface_->has_parameter(name)) { - THROW_SEMANTIC_ERROR("Parameter ", std::quoted(name), " does not exist"); - } - return node_parameters_interface_->get_parameter(name).get_value(); + return node_parameters_interface_->get_parameter(name).get_value(); } private: diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 1ac65fcc4e2..68a67a8d5d2 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -217,7 +217,7 @@ bool API::attachDetectionSensor( double object_recognition_delay) { return attachDetectionSensor(helper::constructDetectionSensorConfiguration( - entity_name, getParameterOrDeclare("architecture_type", "awf/universe"), 0.1, + entity_name, getParameter("architecture_type", "awf/universe"), 0.1, detection_sensor_range, detect_all_objects_in_range, pos_noise_stddev, random_seed, probability_of_lost, object_recognition_delay)); } @@ -250,8 +250,8 @@ bool API::attachLidarSensor( const helper::LidarType lidar_type) { return attachLidarSensor(helper::constructLidarConfiguration( - lidar_type, entity_name, - getParameterOrDeclare("architecture_type", "awf/universe"), lidar_sensor_delay)); + lidar_type, entity_name, getParameter("architecture_type", "awf/universe"), + lidar_sensor_delay)); } bool API::updateTimeInSim() diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index d6198dc3e99..489ab6f647e 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -39,24 +39,22 @@ auto EgoEntity::makeFieldOperatorApplication( const std::shared_ptr & node_parameter_handler) -> std::unique_ptr { - if (const auto architecture_type = node_parameter_handler->getParameterOrDeclare( - "architecture_type", "awf/universe"); + if (const auto architecture_type = + node_parameter_handler->getParameter("architecture_type", "awf/universe"); architecture_type.find("awf/universe") != std::string::npos) { - std::string rviz_config = - node_parameter_handler->getParameterOrDeclare("rviz_config", ""); - return node_parameter_handler->getParameterOrDeclare("launch_autoware", true) + std::string rviz_config = node_parameter_handler->getParameter("rviz_config", ""); + return node_parameter_handler->getParameter("launch_autoware", true) ? std::make_unique< concealer::FieldOperatorApplicationFor>( - node_parameter_handler->getParameterOrDeclare( - "autoware_launch_package"), - node_parameter_handler->getParameterOrDeclare("autoware_launch_file"), + node_parameter_handler->getParameter("autoware_launch_package"), + node_parameter_handler->getParameter("autoware_launch_file"), "map_path:=" + configuration.map_path.string(), "lanelet2_map_file:=" + configuration.getLanelet2MapFile(), "pointcloud_map_file:=" + configuration.getPointCloudMapFile(), "sensor_model:=" + - node_parameter_handler->getParameterOrDeclare("sensor_model"), + node_parameter_handler->getParameter("sensor_model"), "vehicle_model:=" + - node_parameter_handler->getParameterOrDeclare("vehicle_model"), + node_parameter_handler->getParameter("vehicle_model"), "rviz_config:=" + ((rviz_config == "") ? configuration.rviz_config_path.string() : Configuration::Pathname(rviz_config).string()), From e777c45fe826c627381bb50ce1047c858fda1165 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 13 Jun 2024 18:55:36 +0200 Subject: [PATCH 076/118] add a bunch of working tests, first bug found --- .../simple_sensor_simulator/CMakeLists.txt | 1 + .../test_scenario_simulator.cpp | 134 +++++++++++------- 2 files changed, 86 insertions(+), 49 deletions(-) diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt index 3b649cffe22..5f709e9d23e 100644 --- a/simulation/simple_sensor_simulator/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/CMakeLists.txt @@ -89,6 +89,7 @@ if(BUILD_TESTING) add_subdirectory(test) find_package(Protobuf REQUIRED) + find_package(rclcpp REQUIRED) include_directories(${Protobuf_INCLUDE_DIRS}) target_link_libraries(simple_sensor_simulator_component ${Protobuf_LIBRARIES}) endif() diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_scenario_simulator.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_scenario_simulator.cpp index f0db0a5f87d..c3a86e6d2aa 100644 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_scenario_simulator.cpp +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_scenario_simulator.cpp @@ -17,69 +17,105 @@ #include #include #include +#include +#include #include #include #include "../utils/expect_eq_macros.hpp" -class Client +auto makeInitializeRequest() -> simulation_api_schema::InitializeRequest { -public: - Client() - : context(), socket(this->context, zmqpp::socket_type::request), address{"tcp://localhost:5555"} - { - socket.connect(address); - } - auto send(zmqpp::message & msg) -> void { socket.send(msg); } - auto recv(zmqpp::message & msg) -> void { socket.receive(msg); } - -private: - zmqpp::context context; - zmqpp::socket socket; - std::string address; -}; + auto request = simulation_api_schema::InitializeRequest(); + request.set_lanelet2_map_path( + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"); + return request; +} -int main(int argc, char ** argv) +auto makeUpdateFrameRequest() -> simulation_api_schema::UpdateFrameRequest +{ + auto request = simulation_api_schema::UpdateFrameRequest(); + return request; +} + +TEST(ScenarioSimulator, initialize_defaultPort) { - auto server = std::thread([&argc, &argv] { - rclcpp::init(argc, argv); + auto server = std::thread([] { + rclcpp::init(0, nullptr); rclcpp::NodeOptions options; auto component = std::make_shared(options); rclcpp::spin(component); - rclcpp::shutdown(); }); - auto client = Client(); + auto multi_client = + zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); - auto clock = traffic_simulator::SimulationClock(true, 1.0, 10.0); - auto request = simulation_api_schema::InitializeRequest(); - request.set_realtime_factor(1.0); - request.set_step_time(0.1); - request.set_initialize_time(0.0); - simulation_interface::toProto(clock.getCurrentRosTime(), *request.mutable_initialize_ros_time()); - request.set_lanelet2_map_path( - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"); + EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + + rclcpp::shutdown(); + server.join(); +} + +TEST(ScenarioSimulator, initialize_customPort) +{ + auto server = std::thread([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + rclcpp::Parameter port_param("port", rclcpp::ParameterValue(1234)); + options.parameter_overrides().push_back(port_param); + auto component = std::make_shared(options); + rclcpp::spin(component); + }); + + auto multi_client = + zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 1234U); + + EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + + rclcpp::shutdown(); + server.join(); +} - zmqpp::message req_message = zeromq::toZMQ(request); - std::cout << "REQ\n" << request.DebugString() << std::endl; - client.send(req_message); - - zmqpp::message rep_message; - client.recv(rep_message); - - auto response = zeromq::toProto(rep_message); - std::cout << "REP\n" << response.DebugString() << std::endl; - if (response.result().success()) { - std::cout << "worked lol\n"; - } else { - std::cout << ":(\n" << response.result().DebugString() << std::endl; - } - - { - zmqpp::message my_final_message; - my_final_message << "destroy"; - client.send(my_final_message); - } +TEST(ScenarioSimulator, updateFrame_correct) +{ + auto server = std::thread([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + }); + + auto multi_client = + zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); + + EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(multi_client.call(makeUpdateFrameRequest()).result().success()); + + rclcpp::shutdown(); server.join(); - return 0; +} + +TEST(ScenarioSimulator, updateFrame_noInitialize) +{ + // uhhh theres missing "initialized_(false)," in the constructor + auto server = std::thread([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + }); + + auto multi_client = + zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); + + EXPECT_FALSE(multi_client.call(makeUpdateFrameRequest()).result().success()); + + rclcpp::shutdown(); + server.join(); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } \ No newline at end of file From 4ba52d497f7421e9aa9d9594f696b9b6a6cef89c Mon Sep 17 00:00:00 2001 From: Release Bot Date: Fri, 14 Jun 2024 07:39:32 +0000 Subject: [PATCH 077/118] Bump version of scenario_simulator_v2 from version 2.1.2 to version 2.1.3 --- common/math/arithmetic/CHANGELOG.rst | 5 +++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 5 +++++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 5 +++++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 5 +++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 5 +++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 5 +++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 5 +++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 5 +++++ map/kashiwanoha_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 5 +++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../openscenario_experimental_catalog/CHANGELOG.rst | 5 +++++ .../openscenario_experimental_catalog/package.xml | 2 +- openscenario/openscenario_interpreter/CHANGELOG.rst | 5 +++++ openscenario/openscenario_interpreter/package.xml | 2 +- .../openscenario_interpreter_example/CHANGELOG.rst | 5 +++++ .../openscenario_interpreter_example/package.xml | 2 +- openscenario/openscenario_interpreter_msgs/CHANGELOG.rst | 5 +++++ openscenario/openscenario_interpreter_msgs/package.xml | 2 +- openscenario/openscenario_preprocessor/CHANGELOG.rst | 5 +++++ openscenario/openscenario_preprocessor/package.xml | 2 +- .../openscenario_preprocessor_msgs/CHANGELOG.rst | 5 +++++ openscenario/openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 5 +++++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 5 +++++ openscenario/openscenario_validator/package.xml | 2 +- rviz_plugins/openscenario_visualization/CHANGELOG.rst | 5 +++++ rviz_plugins/openscenario_visualization/package.xml | 2 +- .../real_time_factor_control_rviz_plugin/CHANGELOG.rst | 5 +++++ .../real_time_factor_control_rviz_plugin/package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 5 +++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 5 +++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 5 +++++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 5 +++++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 5 +++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 9 +++++++++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 5 +++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 5 +++++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 5 +++++ test_runner/scenario_test_runner/package.xml | 2 +- 56 files changed, 172 insertions(+), 28 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index defee849eb8..3bd0761efdf 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package arithmetic * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 7d1c45ab424..fbf7251a6af 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 2.1.2 + 2.1.3 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index acbe9c08466..e55ee8a70b6 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package geometry * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index b788b262c3b..91a7dc92e4f 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 2.1.2 + 2.1.3 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 38c2f5e56de..83345343e1c 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package scenario_simulator_exception * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 1876c4523df..6a8c536c20c 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 2.1.2 + 2.1.3 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index f6020b84cac..2f0c7b15286 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package junit_exporter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index cf43ebd66ed..d3f090ff822 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 2.1.2 + 2.1.3 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index 8c7c041a359..e3ddb922c3e 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package status_monitor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 2a5471e3434..4135350106e 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 2.1.2 + 2.1.3 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index ea2e4afdc68..df7453db10f 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package concealer * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 80c3eaef916..91b970b2168 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 2.1.2 + 2.1.3 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index ddf6b1eac90..19505952a7e 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -16,6 +16,11 @@ Changelog for package embree_vendor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index de94adc0d3c..01a84e66ded 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 2.1.2 + 2.1.3 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 2bbb3a99897..80e6124de26 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package kashiwanoha_map * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index a36c28ae63c..67a75f9672a 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 2.1.2 + 2.1.3 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index 7b43c9f1765..c8875b1d4d0 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package cpp_mock_scenarios * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 359001ee36c..b25777d817a 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 2.1.2 + 2.1.3 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 81cac3fd43b..16372a53a99 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_experimental_catalog * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index c188cc0f86e..ba438a3fa5c 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 2.1.2 + 2.1.3 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index a3fead32246..09b28ee4665 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -14,6 +14,11 @@ Changelog for package openscenario_interpreter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge pull request `#1012 `_ from tier4/fix/interpreter/fault-injection-action diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index db5d4f5de94..c2b59fa5498 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 2.1.2 + 2.1.3 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 730e8074d07..bc437ad8148 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_interpreter_example * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 6c4ca994cab..a509a6b1608 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 2.1.2 + 2.1.3 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index 3ebd4aa71bd..1897059fbc3 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_interpreter_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 613bfd2c446..2e5d92e9973 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 2.1.2 + 2.1.3 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index fcf10055cfa..7d851491e29 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_preprocessor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 55e8cca9ddc..19602619d61 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 2.1.2 + 2.1.3 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index c09df8ae481..2b42f6ebe78 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_preprocessor_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index f94f8a09bcf..6d99d9cf005 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 2.1.2 + 2.1.3 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index a29dbc2efce..746e8cafbaf 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -16,6 +16,11 @@ Changelog for package openscenario_utility * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 04e2c7450e7..0677acc4a65 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 2.1.2 + 2.1.3 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index 4cb6e150fad..05b905cc58e 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package openscenario_validator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index fc5a2d1d563..b5088b40533 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 2.1.2 + 2.1.3 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index cd5586df7b4..2efd0579537 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_visualization * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 0a0d47b72f2..fb8915c9d1f 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 2.1.2 + 2.1.3 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 8ddb2fcb56b..36e8bbfc45c 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index b88361134b3..a23f8e5d6f8 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 2.1.2 + 2.1.3 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index eb265498ad0..7babd39b21c 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package scenario_simulator_v2 * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 3117b3bf2ff..76d4c030fd7 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 2.1.2 + 2.1.3 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 9b29186f898..46a9406820a 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package behavior_tree_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 249c7ed2f1f..e8df2add1e8 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 2.1.2 + 2.1.3 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index c7d760eaaa7..6622ef9d084 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package do_nothing_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index b4cbcbf8363..3ac539dd628 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 2.1.2 + 2.1.3 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 76adbddd9ff..8e7a1eed134 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package simple_sensor_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index a9b8b1414b8..99461c13094 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 2.1.2 + 2.1.3 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 89fc8a2fcd6..8c93852841a 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package simulation_interface * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 9e22b525c5f..ce4eaf087cd 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 2.1.2 + 2.1.3 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index b25ffae3c02..f5d64ac8baf 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -13,6 +13,15 @@ Changelog for package traffic_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge pull request `#1284 `_ from tier4/fix/issue1276 + Optimize `EntityManager::broadcastEntityTransform` to Execute Only Once +* Merge branch 'master' into fix/issue1276 +* fix format +* Only publsih tf onece. +* Contributors: Masaya Kataoka, Taiga Takano + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index e28c0afc8d1..dca691d6448 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 2.1.2 + 2.1.3 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 876d54227a6..6057c1efb16 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 5a02bf941ca..b41a78bc132 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 2.1.2 + 2.1.3 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 0dd69e8b361..e9ba5335b24 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package random_test_runner * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index a5813e1257e..2c01aafeb0c 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 2.1.2 + 2.1.3 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 38528ed15cd..cdbf07c527a 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -23,6 +23,11 @@ Changelog for package scenario_test_runner * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.3 (2024-06-14) +------------------ +* Merge branch 'master' into fix/issue1276 +* Contributors: Masaya Kataoka + 2.1.2 (2024-06-13) ------------------ * Merge branch 'master' into fix/interpreter/fault-injection-action diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index c3616df9913..c43ed3d5e68 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 2.1.2 + 2.1.3 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 08935944755bf75904fc304ea9875768f9caa2be Mon Sep 17 00:00:00 2001 From: Release Bot Date: Fri, 14 Jun 2024 12:54:27 +0000 Subject: [PATCH 078/118] Bump version of scenario_simulator_v2 from version 2.1.3 to version 2.1.4 --- common/math/arithmetic/CHANGELOG.rst | 6 ++++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 11 +++++++++++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 6 ++++++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 6 ++++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 6 ++++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 6 ++++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 6 ++++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 6 ++++++ map/kashiwanoha_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 11 +++++++++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../openscenario_experimental_catalog/CHANGELOG.rst | 6 ++++++ .../openscenario_experimental_catalog/package.xml | 2 +- openscenario/openscenario_interpreter/CHANGELOG.rst | 11 +++++++++++ openscenario/openscenario_interpreter/package.xml | 2 +- .../openscenario_interpreter_example/CHANGELOG.rst | 6 ++++++ .../openscenario_interpreter_example/package.xml | 2 +- .../openscenario_interpreter_msgs/CHANGELOG.rst | 6 ++++++ .../openscenario_interpreter_msgs/package.xml | 2 +- openscenario/openscenario_preprocessor/CHANGELOG.rst | 6 ++++++ openscenario/openscenario_preprocessor/package.xml | 2 +- .../openscenario_preprocessor_msgs/CHANGELOG.rst | 6 ++++++ .../openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 6 ++++++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 6 ++++++ openscenario/openscenario_validator/package.xml | 2 +- rviz_plugins/openscenario_visualization/CHANGELOG.rst | 6 ++++++ rviz_plugins/openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 6 ++++++ .../real_time_factor_control_rviz_plugin/package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 6 ++++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 10 ++++++++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 10 ++++++++++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 10 ++++++++++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 6 ++++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 9 +++++++++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 6 ++++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 6 ++++++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 8 ++++++++ test_runner/scenario_test_runner/package.xml | 2 +- 56 files changed, 228 insertions(+), 28 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 3bd0761efdf..feafa5ad3c0 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package arithmetic * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index fbf7251a6af..510d19f289e 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 2.1.3 + 2.1.4 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index e55ee8a70b6..8df31047d01 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -13,6 +13,17 @@ Changelog for package geometry * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge pull request `#1281 `_ from tier4/fix/remove_quaternion_operation + Remove quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Update common/math/geometry/include/geometry/quaternion/get_rotation.hpp +* fix +* Merge branch 'master' into fix/remove_quaternion_operation +* Remove quaternion_operation +* Contributors: Masaya Kataoka, Taiga Takano + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 708c15c2dfa..2e7dfc41e90 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 2.1.3 + 2.1.4 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 83345343e1c..617fb68ab54 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package scenario_simulator_exception * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 6a8c536c20c..3bafdfff009 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 2.1.3 + 2.1.4 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index 2f0c7b15286..de122be8880 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package junit_exporter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index d3f090ff822..29fb0dad2db 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 2.1.3 + 2.1.4 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index e3ddb922c3e..7ae2845dffa 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package status_monitor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 4135350106e..b664a5dd99d 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 2.1.3 + 2.1.4 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index df7453db10f..4f241fa2f75 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package concealer * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 91b970b2168..681173a9da6 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 2.1.3 + 2.1.4 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 19505952a7e..919128b14b5 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -16,6 +16,12 @@ Changelog for package embree_vendor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 01a84e66ded..9090a547e3e 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 2.1.3 + 2.1.4 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 80e6124de26..6da844d4b5b 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package kashiwanoha_map * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 67a75f9672a..78060893748 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 2.1.3 + 2.1.4 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index c8875b1d4d0..5c60d2e8f33 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -13,6 +13,17 @@ Changelog for package cpp_mock_scenarios * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge pull request `#1281 `_ from tier4/fix/remove_quaternion_operation + Remove quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* change format +* fix +* Remove quaternion_operation +* Contributors: Masaya Kataoka, Taiga Takano + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index b25777d817a..332e5e276e3 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 2.1.3 + 2.1.4 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 16372a53a99..3b2ddc18c48 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_experimental_catalog * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index ba438a3fa5c..d25fdfd58ed 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 2.1.3 + 2.1.4 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 09b28ee4665..41bbdc9b134 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -14,6 +14,17 @@ Changelog for package openscenario_interpreter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge pull request `#1281 `_ from tier4/fix/remove_quaternion_operation + Remove quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* fix +* Merge branch 'master' into fix/remove_quaternion_operation +* Remove quaternion_operation +* Contributors: Masaya Kataoka, Taiga Takano + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index c2b59fa5498..57f832fa6ac 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 2.1.3 + 2.1.4 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index bc437ad8148..2af2bc3a476 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_interpreter_example * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index a509a6b1608..393fca871f3 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 2.1.3 + 2.1.4 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index 1897059fbc3..1c9316e048a 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_interpreter_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 2e5d92e9973..43552341374 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 2.1.3 + 2.1.4 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index 7d851491e29..26c6d3fec13 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_preprocessor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 19602619d61..728d319fd8c 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 2.1.3 + 2.1.4 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 2b42f6ebe78..d9307bbaa5a 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_preprocessor_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index 6d99d9cf005..b920c262800 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 2.1.3 + 2.1.4 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 746e8cafbaf..49e98af902f 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -16,6 +16,12 @@ Changelog for package openscenario_utility * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 0677acc4a65..bb68a5dcf5c 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 2.1.3 + 2.1.4 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index 05b905cc58e..a4c1eefb9da 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package openscenario_validator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index b5088b40533..c2f3beeaefc 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 2.1.3 + 2.1.4 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 2efd0579537..a35b5c8e9bd 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_visualization * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index fb8915c9d1f..1dc52a9beb4 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 2.1.3 + 2.1.4 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 36e8bbfc45c..a1b09e6aefb 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index a23f8e5d6f8..d39a9ccd9f8 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 2.1.3 + 2.1.4 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 7babd39b21c..43a76ae05f7 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package scenario_simulator_v2 * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 76d4c030fd7..67897e18872 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 2.1.3 + 2.1.4 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 46a9406820a..9c26b4e96fa 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -13,6 +13,16 @@ Changelog for package behavior_tree_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge pull request `#1281 `_ from tier4/fix/remove_quaternion_operation + Remove quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* fix package xml +* Remove quaternion_operation +* Contributors: Masaya Kataoka, Taiga Takano + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 45eb94bfe98..30c07969c7c 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 2.1.3 + 2.1.4 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 6622ef9d084..3b59574c869 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -13,6 +13,16 @@ Changelog for package do_nothing_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge pull request `#1281 `_ from tier4/fix/remove_quaternion_operation + Remove quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* fix package xml +* Remove quaternion_operation +* Contributors: Masaya Kataoka, Taiga Takano + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index d6263a52918..1f50295bd35 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 2.1.3 + 2.1.4 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 8e7a1eed134..96e64e595bc 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -13,6 +13,16 @@ Changelog for package simple_sensor_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge pull request `#1281 `_ from tier4/fix/remove_quaternion_operation + Remove quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* fix package xml +* Remove quaternion_operation +* Contributors: Masaya Kataoka, Taiga Takano + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 93b186b2d5a..4ad80262459 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 2.1.3 + 2.1.4 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 8c93852841a..baa6af4f01a 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package simulation_interface * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index ce4eaf087cd..1e4e8b46a50 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 2.1.3 + 2.1.4 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index f5d64ac8baf..1533e609f2b 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -13,6 +13,15 @@ Changelog for package traffic_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge pull request `#1281 `_ from tier4/fix/remove_quaternion_operation + Remove quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Remove quaternion_operation +* Contributors: Masaya Kataoka, Taiga Takano + 2.1.3 (2024-06-14) ------------------ * Merge pull request `#1284 `_ from tier4/fix/issue1276 diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 4e4adf53a62..a9f99a9a156 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 2.1.3 + 2.1.4 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 6057c1efb16..493c562fefb 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index b41a78bc132..b388fec2d43 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 2.1.3 + 2.1.4 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index e9ba5335b24..da110aa6480 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package random_test_runner * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* Merge branch 'master' into fix/remove_quaternion_operation +* Contributors: Masaya Kataoka + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 2c01aafeb0c..142eb007044 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 2.1.3 + 2.1.4 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index cdbf07c527a..4a92a68fcb8 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -23,6 +23,14 @@ Changelog for package scenario_test_runner * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.4 (2024-06-14) +------------------ +* Merge branch 'master' into fix/remove_quaternion_operation +* fix +* Merge branch 'master' into fix/remove_quaternion_operation +* Remove quaternion_operation +* Contributors: Masaya Kataoka, Taiga Takano + 2.1.3 (2024-06-14) ------------------ * Merge branch 'master' into fix/issue1276 diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index c43ed3d5e68..a4ac02a4822 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 2.1.3 + 2.1.4 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 51fb643fd1291a454f6a41bd8d202bf318ebc588 Mon Sep 17 00:00:00 2001 From: robomic Date: Fri, 14 Jun 2024 16:54:38 +0200 Subject: [PATCH 079/118] more tests --- .../test_scenario_simulator.cpp | 240 +++++++++++++++++- 1 file changed, 237 insertions(+), 3 deletions(-) diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_scenario_simulator.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_scenario_simulator.cpp index c3a86e6d2aa..c9c593a356f 100644 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_scenario_simulator.cpp +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_scenario_simulator.cpp @@ -15,7 +15,9 @@ #include #include +#include #include +#include #include #include #include @@ -35,9 +37,52 @@ auto makeInitializeRequest() -> simulation_api_schema::InitializeRequest auto makeUpdateFrameRequest() -> simulation_api_schema::UpdateFrameRequest { auto request = simulation_api_schema::UpdateFrameRequest(); + // to be filled up return request; } +auto makeDespawnEntityRequest(const std::string name = "name") + -> simulation_api_schema::DespawnEntityRequest +{ + auto request = simulation_api_schema::DespawnEntityRequest(); + request.set_name(name); + return request; +} + +auto makeSpawnVehicleEntityRequest(const std::string name = "name", const bool is_ego = false) + -> simulation_api_schema::SpawnVehicleEntityRequest +{ + auto request = simulation_api_schema::SpawnVehicleEntityRequest(); + const auto params = traffic_simulator_msgs::msg::VehicleParameters{}; + simulation_interface::toProto(params, *request.mutable_parameters()); + request.mutable_parameters()->set_name(name); + request.set_is_ego(is_ego); + return request; +} + +auto makeSpawnPedestrianEntityRequest(const std::string name = "name") + -> simulation_api_schema::SpawnPedestrianEntityRequest +{ + auto request = simulation_api_schema::SpawnPedestrianEntityRequest(); + const auto params = traffic_simulator_msgs::msg::PedestrianParameters{}; + simulation_interface::toProto(params, *request.mutable_parameters()); + request.mutable_parameters()->set_name(name); + return request; +} + +auto makeSpawnMiscObjectEntityRequest(const std::string name = "name") + -> simulation_api_schema::SpawnMiscObjectEntityRequest +{ + auto request = simulation_api_schema::SpawnMiscObjectEntityRequest(); + const auto params = traffic_simulator_msgs::msg::MiscObjectParameters{}; + simulation_interface::toProto(params, *request.mutable_parameters()); + request.mutable_parameters()->set_name(name); + return request; +} + +/** + * @note Test initialization correctness with a sample request with the default port (5555). + */ TEST(ScenarioSimulator, initialize_defaultPort) { auto server = std::thread([] { @@ -56,13 +101,16 @@ TEST(ScenarioSimulator, initialize_defaultPort) server.join(); } +/** + * @note Test initialization correctness with a sample request with a custom port. + */ TEST(ScenarioSimulator, initialize_customPort) { auto server = std::thread([] { rclcpp::init(0, nullptr); rclcpp::NodeOptions options; - rclcpp::Parameter port_param("port", rclcpp::ParameterValue(1234)); - options.parameter_overrides().push_back(port_param); + options.parameter_overrides().push_back( + rclcpp::Parameter{"port", rclcpp::ParameterValue(1234)}); auto component = std::make_shared(options); rclcpp::spin(component); }); @@ -76,6 +124,9 @@ TEST(ScenarioSimulator, initialize_customPort) server.join(); } +/** + * @note Test updating frame correctness with a sample request. +*/ TEST(ScenarioSimulator, updateFrame_correct) { auto server = std::thread([] { @@ -95,9 +146,12 @@ TEST(ScenarioSimulator, updateFrame_correct) server.join(); } +/** + * @note Test updating frame correctness with a sample request before requesting initialization. + */ TEST(ScenarioSimulator, updateFrame_noInitialize) { - // uhhh theres missing "initialized_(false)," in the constructor + // undefined behaviour !!! there is missing "initialized_(false)," in the ScenarioSimulator constructor auto server = std::thread([] { rclcpp::init(0, nullptr); rclcpp::NodeOptions options; @@ -114,6 +168,186 @@ TEST(ScenarioSimulator, updateFrame_noInitialize) server.join(); } +/** + * @note Test spawning vehicle entity correctness with a request to spawn + * Ego vehicle when Ego vehicle is not spawned yet. + */ +TEST(ScenarioSimulator, spawnVehicleEntity_firstEgo) +{ + auto server = std::thread([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + }); + + auto multi_client = + zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); + + EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(multi_client.call(makeSpawnVehicleEntityRequest("ego", true)).result().success()); + + rclcpp::shutdown(); + server.join(); +} + +/** + * @note Test spawning vehicle entity correctness with a request to spawn a NPC vehicle. + */ +TEST(ScenarioSimulator, spawnVehicleEntity_npc) +{ + auto server = std::thread([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + }); + + auto multi_client = + zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); + + EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(multi_client.call(makeSpawnVehicleEntityRequest("npc", false)).result().success()); + + rclcpp::shutdown(); + server.join(); +} + +/** + * @note Test spawning pedestrian entity correctness with a request to spawn a NPC pedestrian. + */ +TEST(ScenarioSimulator, spawnPedestrianEntity) +{ + auto server = std::thread([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + }); + + auto multi_client = + zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); + + EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(multi_client.call(makeSpawnPedestrianEntityRequest("bob")).result().success()); + + rclcpp::shutdown(); + server.join(); +} + +/** + * @note Test spawning misc object entity correctness with a request to spawn a misc object. + */ +TEST(ScenarioSimulator, spawnMiscObjectEntity) +{ + auto server = std::thread([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + }); + + auto multi_client = + zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); + + EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(multi_client.call(makeSpawnMiscObjectEntityRequest("blob")).result().success()); + + rclcpp::shutdown(); + server.join(); +} + +/** + * @note Test despawning an entity with a request to despawn an existing vehicle. + */ +TEST(ScenarioSimulator, despawnEntity_vehicle) +{ + auto server = std::thread([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + }); + + auto multi_client = + zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); + + EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(multi_client.call(makeSpawnVehicleEntityRequest("npc", false)).result().success()); + EXPECT_TRUE(multi_client.call(makeDespawnEntityRequest("npc")).result().success()); + + rclcpp::shutdown(); + server.join(); +} + +/** + * @note Test despawning an entity with a request to despawn an existing pedestrian. + */ +TEST(ScenarioSimulator, despawnEntity_pedestrian) +{ + auto server = std::thread([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + }); + + auto multi_client = + zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); + + EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(multi_client.call(makeSpawnPedestrianEntityRequest("bob")).result().success()); + EXPECT_TRUE(multi_client.call(makeDespawnEntityRequest("bob")).result().success()); + + rclcpp::shutdown(); + server.join(); +} + +/** + * @note Test despawning an entity with a request to despawn an existing misc object. + */ +TEST(ScenarioSimulator, despawnEntity_miscObject) +{ + auto server = std::thread([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + }); + + auto multi_client = + zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); + + EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(multi_client.call(makeSpawnMiscObjectEntityRequest("blob")).result().success()); + EXPECT_TRUE(multi_client.call(makeDespawnEntityRequest("blob")).result().success()); + + rclcpp::shutdown(); + server.join(); +} + +/** + * @note Test despawning an entity with a request to despawn an entity that does not exist. + */ +TEST(ScenarioSimulator, despawnEntity_invalidName) +{ + auto server = std::thread([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + }); + + auto multi_client = + zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); + + EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + EXPECT_FALSE(multi_client.call(makeDespawnEntityRequest("invalid")).result().success()); + + rclcpp::shutdown(); + server.join(); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 3eae6c0bb5cfe8199a34e4eb179c83211c3e77ba Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 17 Jun 2024 13:48:57 +0200 Subject: [PATCH 080/118] internal review corrections --- .../src/entity/test_misc_object_entity.cpp | 157 +++++++++--------- 1 file changed, 74 insertions(+), 83 deletions(-) diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp index a62292c856b..5a335618fda 100644 --- a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp @@ -29,10 +29,10 @@ int main(int argc, char ** argv) return RUN_ALL_TESTS(); } -class MiscObjectEntityTest : public testing::Test +class MiscObjectEntityTest_HdMapUtils : public testing::Test { protected: - MiscObjectEntityTest() + MiscObjectEntityTest_HdMapUtils() : hdmap_utils_ptr(makeHdMapUtilsSharedPointer()), entity_name("misc_object_entity") { } @@ -41,24 +41,21 @@ class MiscObjectEntityTest : public testing::Test const std::string entity_name; }; -class EntityBaseWithMiscObjectTest : public testing::Test +class MiscObjectEntityTest_FullObject : public MiscObjectEntityTest_HdMapUtils { protected: - EntityBaseWithMiscObjectTest() + MiscObjectEntityTest_FullObject() : id(120659), - hdmap_utils(makeHdMapUtilsSharedPointer()), - pose(makeCanonicalizedLaneletPose(hdmap_utils, id)), + pose(makeCanonicalizedLaneletPose(hdmap_utils_ptr, id)), bbox(makeBoundingBox()), - status(makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox)), + status(makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, 0.0, entity_name)), misc_object( - "default_entity_name", status, hdmap_utils, - traffic_simulator_msgs::msg::MiscObjectParameters{}), + entity_name, status, hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}), entity_base(&misc_object) { } const lanelet::Id id; - std::shared_ptr hdmap_utils; traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose; traffic_simulator_msgs::msg::BoundingBox bbox; traffic_simulator::entity_status::CanonicalizedEntityStatus status; @@ -69,7 +66,7 @@ class EntityBaseWithMiscObjectTest : public testing::Test /** * @note Test basic functionality. Test current action obtaining when NPC logic is not started. */ -TEST_F(MiscObjectEntityTest, getCurrentAction_npcNotStarted) +TEST_F(MiscObjectEntityTest_HdMapUtils, getCurrentAction_npcNotStarted) { auto non_canonicalized_status = makeEntityStatus( hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), 0.0, @@ -89,7 +86,7 @@ TEST_F(MiscObjectEntityTest, getCurrentAction_npcNotStarted) /** * @note Test function behavior when absolute speed change is requested - the goal is to test throwing error. */ -TEST_F(MiscObjectEntityTest, requestSpeedChange_absolute) +TEST_F(MiscObjectEntityTest_HdMapUtils, requestSpeedChange_absolute) { EXPECT_THROW( traffic_simulator::entity::MiscObjectEntity( @@ -105,7 +102,7 @@ TEST_F(MiscObjectEntityTest, requestSpeedChange_absolute) /** * @note Test function behavior when relative speed change is requested - the goal is to test throwing error. */ -TEST_F(MiscObjectEntityTest, requestSpeedChange_relative) +TEST_F(MiscObjectEntityTest_HdMapUtils, requestSpeedChange_relative) { auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659); auto bbox = makeBoundingBox(); @@ -137,7 +134,7 @@ TEST_F(MiscObjectEntityTest, requestSpeedChange_relative) * @note Test function behavior when relative speed change with transition type is requested * - the goal is to test throwing error. */ -TEST_F(MiscObjectEntityTest, requestSpeedChange_absoluteTransition) +TEST_F(MiscObjectEntityTest_HdMapUtils, requestSpeedChange_absoluteTransition) { EXPECT_THROW( traffic_simulator::entity::MiscObjectEntity( @@ -158,7 +155,7 @@ TEST_F(MiscObjectEntityTest, requestSpeedChange_absoluteTransition) * @note Test function behavior when route assigning is requested with lanelet pose * - the goal is to test throwing error. */ -TEST_F(MiscObjectEntityTest, requestAssignRoute_laneletPose) +TEST_F(MiscObjectEntityTest_HdMapUtils, requestAssignRoute_laneletPose) { EXPECT_THROW( traffic_simulator::entity::MiscObjectEntity( @@ -176,7 +173,7 @@ TEST_F(MiscObjectEntityTest, requestAssignRoute_laneletPose) * @note Test function behavior when route assigning is requested with pose * - the goal is to test throwing error. */ -TEST_F(MiscObjectEntityTest, requestAssignRoute_pose) +TEST_F(MiscObjectEntityTest_HdMapUtils, requestAssignRoute_pose) { EXPECT_THROW( traffic_simulator::entity::MiscObjectEntity( @@ -194,7 +191,7 @@ TEST_F(MiscObjectEntityTest, requestAssignRoute_pose) * @note Test function behavior when position acquiring is requested with lanelet pose * - the goal is to test throwing error. */ -TEST_F(MiscObjectEntityTest, requestAcquirePosition_laneletPose) +TEST_F(MiscObjectEntityTest_HdMapUtils, requestAcquirePosition_laneletPose) { EXPECT_THROW( traffic_simulator::entity::MiscObjectEntity( @@ -211,7 +208,7 @@ TEST_F(MiscObjectEntityTest, requestAcquirePosition_laneletPose) * @note Test function behavior when position acquiring is requested with pose * - the goal is to test throwing error. */ -TEST_F(MiscObjectEntityTest, requestAcquirePosition_pose) +TEST_F(MiscObjectEntityTest_HdMapUtils, requestAcquirePosition_pose) { EXPECT_THROW( traffic_simulator::entity::MiscObjectEntity( @@ -227,7 +224,7 @@ TEST_F(MiscObjectEntityTest, requestAcquirePosition_pose) /** * @note Test function behavior when called with any argument - the goal is to test error throwing. */ -TEST_F(MiscObjectEntityTest, getRouteLanelets) +TEST_F(MiscObjectEntityTest_HdMapUtils, getRouteLanelets) { EXPECT_THROW( traffic_simulator::entity::MiscObjectEntity( @@ -243,7 +240,7 @@ TEST_F(MiscObjectEntityTest, getRouteLanelets) /** * @note Test basic functionality; test whether the function does nothing. */ -TEST_F(EntityBaseWithMiscObjectTest, appendDebugMarker) +TEST_F(MiscObjectEntityTest_FullObject, appendDebugMarker) { visualization_msgs::msg::MarkerArray markers{}; @@ -272,7 +269,7 @@ TEST_F(EntityBaseWithMiscObjectTest, appendDebugMarker) /** * @note Test basic functionality; test whether the function throws an error. */ -TEST_F(EntityBaseWithMiscObjectTest, asFieldOperatorApplication) +TEST_F(MiscObjectEntityTest_FullObject, asFieldOperatorApplication) { EXPECT_THROW(misc_object.asFieldOperatorApplication(), common::Error); } @@ -280,7 +277,7 @@ TEST_F(EntityBaseWithMiscObjectTest, asFieldOperatorApplication) /** * @note Test functionality used by other units; test correctness of 2d polygon calculations. */ -TEST_F(EntityBaseWithMiscObjectTest, get2DPolygon) +TEST_F(MiscObjectEntityTest_FullObject, get2DPolygon) { const auto polygon = misc_object.get2DPolygon(); @@ -299,7 +296,7 @@ TEST_F(EntityBaseWithMiscObjectTest, get2DPolygon) /** * @note Test basic functionality; test whether the NPC logic is started correctly. */ -TEST_F(EntityBaseWithMiscObjectTest, startNpcLogic) +TEST_F(MiscObjectEntityTest_FullObject, startNpcLogic) { EXPECT_FALSE(misc_object.isNpcLogicStarted()); misc_object.startNpcLogic(0.0); @@ -310,10 +307,9 @@ TEST_F(EntityBaseWithMiscObjectTest, startNpcLogic) * @note Test basic functionality; test activating an out of range job with * an entity that has a positive speed and a speed range specified in the job = [0, 0] */ -TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_speed) +TEST_F(MiscObjectEntityTest_FullObject, activateOutOfRangeJob_speed) { - constexpr double velocity = 1.0; - misc_object.setLinearVelocity(velocity); + misc_object.setLinearVelocity(1.0); misc_object.activateOutOfRangeJob(0.0, 0.0, -100.0, 100.0, -100.0, 100.0); constexpr double current_time = 0.0; @@ -327,10 +323,9 @@ TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_speed) * with an entity that has a positive acceleration * and an acceleration range specified in the job = [0, 0]. */ -TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_acceleration) +TEST_F(MiscObjectEntityTest_FullObject, activateOutOfRangeJob_acceleration) { - constexpr double acceleration = 1.0; - misc_object.setLinearAcceleration(acceleration); + misc_object.setLinearAcceleration(1.0); misc_object.activateOutOfRangeJob(-100.0, 100.0, 0.0, 0.0, -100.0, 100.0); constexpr double current_time = 0.0; @@ -344,10 +339,9 @@ TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_acceleration) * with an entity that has a positive jerk * and a jerk range specified in the job = [0, 0]. */ -TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_jerk) +TEST_F(MiscObjectEntityTest_FullObject, activateOutOfRangeJob_jerk) { - constexpr double jerk = 1.0; - misc_object.setLinearJerk(jerk); + misc_object.setLinearJerk(1.0); misc_object.activateOutOfRangeJob(-100.0, 100.0, -100.0, 100.0, 0.0, 0.0); constexpr double current_time = 0.0; @@ -359,7 +353,7 @@ TEST_F(EntityBaseWithMiscObjectTest, activateOutOfRangeJob_jerk) /** * @note Test basic functionality; test wrapper function with invalid relative target lanelet pose. */ -TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetLaneletPose) +TEST_F(MiscObjectEntityTest_FullObject, requestLaneChange_relativeTargetLaneletPose) { const std::string target_name = "target_name"; @@ -367,7 +361,7 @@ TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetLaneletPose std::unordered_map{}; other_status.emplace( target_name, - makeCanonicalizedEntityStatus(hdmap_utils, makePose(makePoint(3810.0, 73745.0)), bbox)); + makeCanonicalizedEntityStatus(hdmap_utils_ptr, makePose(makePoint(3810.0, 73745.0)), bbox)); entity_base->setOtherStatus(other_status); @@ -384,15 +378,16 @@ TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetLaneletPose /** * @note Test basic functionality; test wrapper function with invalid relative target name. */ -TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetName) +TEST_F(MiscObjectEntityTest_FullObject, requestLaneChange_relativeTargetName) { const std::string target_name = "target_name"; auto other_status = std::unordered_map{}; other_status.emplace( - target_name, makeCanonicalizedEntityStatus( - hdmap_utils, makeCanonicalizedLaneletPose(hdmap_utils, 34468, 5.0), bbox)); + target_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 34468, 5.0), bbox)); entity_base->setOtherStatus(other_status); EXPECT_THROW( @@ -410,15 +405,16 @@ TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetName) * @note Test basic functionality; test wrapper function with invalid relative target lane change * - the goal is to request a lane change in the location where the lane change is impossible. */ -TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetInvalid) +TEST_F(MiscObjectEntityTest_FullObject, requestLaneChange_relativeTargetInvalid) { const std::string target_name = "target_name"; auto other_status = std::unordered_map{}; other_status.emplace( - target_name, makeCanonicalizedEntityStatus( - hdmap_utils, makeCanonicalizedLaneletPose(hdmap_utils, 34468, 5.0), bbox)); + target_name, + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 34468, 5.0), bbox)); entity_base->setOtherStatus(other_status); EXPECT_THROW( @@ -436,7 +432,7 @@ TEST_F(EntityBaseWithMiscObjectTest, requestLaneChange_relativeTargetInvalid) /** * @note Test function behavior when called with any argument - the goal is to test error throwing. */ -TEST_F(EntityBaseWithMiscObjectTest, requestFollowTrajectory) +TEST_F(MiscObjectEntityTest_FullObject, requestFollowTrajectory) { EXPECT_THROW( misc_object.requestFollowTrajectory( @@ -447,7 +443,7 @@ TEST_F(EntityBaseWithMiscObjectTest, requestFollowTrajectory) /** * @note Test function behavior when called with any argument - the goal is to test error throwing. */ -TEST_F(EntityBaseWithMiscObjectTest, requestWalkStraight) +TEST_F(MiscObjectEntityTest_FullObject, requestWalkStraight) { EXPECT_THROW(misc_object.requestWalkStraight(), common::Error); } @@ -456,7 +452,7 @@ TEST_F(EntityBaseWithMiscObjectTest, requestWalkStraight) * @note test basic functionality; test updating stand still duration * when NPC logic is started and velocity is greater than 0. */ -TEST_F(EntityBaseWithMiscObjectTest, updateStandStillDuration_startedMoving) +TEST_F(MiscObjectEntityTest_FullObject, updateStandStillDuration_startedMoving) { misc_object.startNpcLogic(0.0); misc_object.setLinearVelocity(3.0); @@ -468,7 +464,7 @@ TEST_F(EntityBaseWithMiscObjectTest, updateStandStillDuration_startedMoving) * @note Test basic functionality; test updating stand still duration * when NPC logic is not started. */ -TEST_F(EntityBaseWithMiscObjectTest, updateStandStillDuration_notStarted) +TEST_F(MiscObjectEntityTest_FullObject, updateStandStillDuration_notStarted) { misc_object.setLinearVelocity(3.0); EXPECT_EQ(0.0, misc_object.updateStandStillDuration(0.1)); @@ -481,7 +477,7 @@ TEST_F(EntityBaseWithMiscObjectTest, updateStandStillDuration_notStarted) * @note Test basic functionality; test updating traveled distance correctness * with NPC logic started and velocity greater than 0. */ -TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_startedMoving) +TEST_F(MiscObjectEntityTest_FullObject, updateTraveledDistance_startedMoving) { constexpr double velocity = 3.0; constexpr double step_time = 0.1; @@ -497,7 +493,7 @@ TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_startedMoving) /** * @note Test basic functionality; test updating traveled distance correctness with NPC not started. */ -TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_notStarted) +TEST_F(MiscObjectEntityTest_FullObject, updateTraveledDistance_notStarted) { constexpr double step_time = 0.1; misc_object.setLinearVelocity(3.0); @@ -511,7 +507,7 @@ TEST_F(EntityBaseWithMiscObjectTest, updateTraveledDistance_notStarted) * @note Test basic functionality; test stopping correctness - the goal * is to check whether the entity status is changed to stopped (no velocity etc.). */ -TEST_F(EntityBaseWithMiscObjectTest, stopAtCurrentPosition) +TEST_F(MiscObjectEntityTest_FullObject, stopAtCurrentPosition) { constexpr double velocity = 3.0; misc_object.setLinearVelocity(velocity); @@ -521,23 +517,34 @@ TEST_F(EntityBaseWithMiscObjectTest, stopAtCurrentPosition) EXPECT_EQ(misc_object.getCurrentTwist().linear.x, 0.0); } +/** + * @note Test functionality used by other units; test relative pose calculations + * correctness with a transformation argument passed. + */ +TEST_F(MiscObjectEntityTest_FullObject, getMapPoseFromRelativePose_relative) +{ + constexpr double s = 5.0; + EXPECT_POSE_NEAR( + misc_object.getMapPoseFromRelativePose(makePose(makePoint(s, 0.0))), + static_cast(makeCanonicalizedLaneletPose(hdmap_utils_ptr, id, s)), + 0.1); +} + /** * @note Test functionality used by other units; test lanelet pose obtaining * with a matching distance smaller than a distance from an entity to the lanelet * (both crosswalk and road) and status_.type.type != PEDESTRIAN. */ -TEST(EntityBaseWithMiscObject, getLaneletPose_notOnRoadAndCrosswalkNotPedestrian) +TEST_F(MiscObjectEntityTest_HdMapUtils, getLaneletPose_notOnRoadAndCrosswalkNotPedestrian) { - auto hdmap_utils = makeHdMapUtilsSharedPointer(); - EXPECT_FALSE(traffic_simulator::entity::MiscObjectEntity( - "misc_object_entity", + entity_name, traffic_simulator::CanonicalizedEntityStatus( makeEntityStatus( - hdmap_utils, makePose(makePoint(3810.0, 73745.0)), makeBoundingBox(), 0.0, - "misc_object_entity", traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), - hdmap_utils), - hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}) + hdmap_utils_ptr, makePose(makePoint(3810.0, 73745.0)), makeBoundingBox(), 0.0, + entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + hdmap_utils_ptr), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getLaneletPose(5.0) .has_value()); } @@ -547,21 +554,19 @@ TEST(EntityBaseWithMiscObject, getLaneletPose_notOnRoadAndCrosswalkNotPedestrian * with a matching distance greater than a distance from an entity to the lanelet * (both crosswalk and road) and status_.type.type != PEDESTRIAN. */ -TEST(EntityBaseWithMiscObject, getLaneletPose_onRoadAndCrosswalkNotPedestrian) +TEST_F(MiscObjectEntityTest_HdMapUtils, getLaneletPose_onRoadAndCrosswalkNotPedestrian) { - auto hdmap_utils = makeHdMapUtilsSharedPointer(); - EXPECT_TRUE( traffic_simulator::entity::MiscObjectEntity( - "misc_object_entity", + entity_name, traffic_simulator::CanonicalizedEntityStatus( makeEntityStatus( - hdmap_utils, + hdmap_utils_ptr, makePose(makePoint(3766.1, 73738.2), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), - makeBoundingBox(), 0.0, "misc_object_entity", + makeBoundingBox(), 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), - hdmap_utils), - hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}) + hdmap_utils_ptr), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getLaneletPose(1.0) .has_value()); } @@ -571,33 +576,19 @@ TEST(EntityBaseWithMiscObject, getLaneletPose_onRoadAndCrosswalkNotPedestrian) * with a matching distance greater than a distance from an entity to the crosswalk lanelet, * but smaller than to the road lanelet and status_.type.type != PEDESTRIAN. */ -TEST(EntityBaseWithMiscObject, getLaneletPose_onCrosswalkNotOnRoadNotPedestrian) +TEST_F(MiscObjectEntityTest_HdMapUtils, getLaneletPose_onCrosswalkNotOnRoadNotPedestrian) { - auto hdmap_utils = makeHdMapUtilsSharedPointer(); - EXPECT_FALSE( traffic_simulator::entity::MiscObjectEntity( - "misc_object_entity", + entity_name, traffic_simulator::CanonicalizedEntityStatus( makeEntityStatus( - hdmap_utils, + hdmap_utils_ptr, makePose(makePoint(3764.5, 73737.5), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), - makeBoundingBox(), 0.0, "misc_object_entity", + makeBoundingBox(), 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), - hdmap_utils), - hdmap_utils, traffic_simulator_msgs::msg::MiscObjectParameters{}) + hdmap_utils_ptr), + hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getLaneletPose(1.0) .has_value()); } - -/** - * @note Test functionality used by other units; test relative pose calculations - * correctness with a transformation argument passed. - */ -TEST_F(EntityBaseWithMiscObjectTest, getMapPoseFromRelativePose_relative) -{ - constexpr double s = 5.0; - EXPECT_POSE_NEAR( - misc_object.getMapPoseFromRelativePose(makePose(makePoint(s, 0.0))), - static_cast(makeCanonicalizedLaneletPose(hdmap_utils, id, s)), 0.1); -} From 27772ef78e17f1b794c90e36cb0db2830ebb8e1b Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 17 Jun 2024 14:43:37 +0200 Subject: [PATCH 081/118] add copy of helper_functions, apply review requests --- .../traffic_simulator/test/CMakeLists.txt | 1 - .../test/src/behavior/test_route_planner.cpp | 2 +- .../test/src/helper_functions.hpp | 145 ++++++++++++++++++ .../src/traffic_lights/test_traffic_light.cpp | 26 ++-- 4 files changed, 157 insertions(+), 17 deletions(-) create mode 100644 simulation/traffic_simulator/test/src/helper_functions.hpp diff --git a/simulation/traffic_simulator/test/CMakeLists.txt b/simulation/traffic_simulator/test/CMakeLists.txt index 5d773ff92b2..713c8b40678 100644 --- a/simulation/traffic_simulator/test/CMakeLists.txt +++ b/simulation/traffic_simulator/test/CMakeLists.txt @@ -5,4 +5,3 @@ add_subdirectory(src/behavior) add_subdirectory(src/job) add_subdirectory(src/simulation_clock) add_subdirectory(src/hdmap_utils) -add_subdirectory(src/behavior) diff --git a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp index ccbc28ff863..7eff3826d6b 100644 --- a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp +++ b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp @@ -17,8 +17,8 @@ #include #include "../catalogs.hpp" -#include "../entity_helper_functions.hpp" #include "../expect_eq_macros.hpp" +#include "../helper_functions.hpp" int main(int argc, char ** argv) { diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp new file mode 100644 index 00000000000..f8f73560d0a --- /dev/null +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -0,0 +1,145 @@ +// 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 TRAFFIC_SIMULATOR__TEST__HELPER_FUNCTIONS_HPP_ +#define TRAFFIC_SIMULATOR__TEST__HELPER_FUNCTIONS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "catalogs.hpp" +#include "expect_eq_macros.hpp" + +auto makePoint(const double x, const double y, const double z = 0.0) -> geometry_msgs::msg::Point +{ + return geometry_msgs::build().x(x).y(y).z(z); +} + +auto makeBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs::msg::BoundingBox +{ + return traffic_simulator_msgs::build() + .center(makePoint(1.0, center_y)) + .dimensions(geometry_msgs::build().x(4.0).y(2.0).z(1.5)); +} + +auto makeSmallBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs::msg::BoundingBox +{ + return traffic_simulator_msgs::build() + .center(makePoint(0.0, center_y)) + .dimensions(geometry_msgs::build().x(1.0).y(1.0).z(1.0)); +} + +auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion +{ + return quaternion_operation::convertEulerAngleToQuaternion( + geometry_msgs::build().x(0.0).y(0.0).z(yaw)); +} + +auto makePose( + geometry_msgs::msg::Point position, + geometry_msgs::msg::Quaternion orientation = geometry_msgs::msg::Quaternion()) + -> geometry_msgs::msg::Pose +{ + return geometry_msgs::build().position(position).orientation( + orientation); +} + +auto makeHdMapUtilsSharedPointer() -> std::shared_ptr +{ + return std::make_shared( + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.9037067912303) + .longitude(139.9337945139059) + .altitude(0.0)); +} + +auto makeCanonicalizedLaneletPose( + std::shared_ptr hdmap_utils, const lanelet::Id id = 120659, + const double s = 0.0, const double offset = 0.0) + -> traffic_simulator::lanelet_pose::CanonicalizedLaneletPose +{ + return traffic_simulator::lanelet_pose::CanonicalizedLaneletPose( + traffic_simulator::helper::constructLaneletPose(id, s, offset), hdmap_utils); +} + +auto makeEntityStatus( + std::shared_ptr hdmap_utils, + traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, + traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, + const std::string name = "default_entity_name", + const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) + -> traffic_simulator::EntityStatus +{ + return traffic_simulator_msgs::build() + .type(traffic_simulator_msgs::build().type(type)) + .subtype(traffic_simulator_msgs::build().value( + traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN)) + .time(0.0) + .name(name) + .bounding_box(bbox) + .action_status(traffic_simulator::helper::constructActionStatus(speed, 0.0, 0.0, 0.0)) + .pose(hdmap_utils->toMapPose(static_cast(pose)).pose) + .lanelet_pose(static_cast(pose)) + .lanelet_pose_valid(true); +} + +auto makeEntityStatus( + std::shared_ptr /* hdmap_utils */, geometry_msgs::msg::Pose pose, + traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, + const std::string name = "default_entity_name", + const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) + -> traffic_simulator::EntityStatus +{ + return traffic_simulator_msgs::build() + .type(traffic_simulator_msgs::build().type(type)) + .subtype(traffic_simulator_msgs::build().value( + traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN)) + .time(0.0) + .name(name) + .bounding_box(bbox) + .action_status(traffic_simulator::helper::constructActionStatus(speed, 0.0, 0.0, 0.0)) + .pose(pose) + .lanelet_pose(traffic_simulator_msgs::msg::LaneletPose{}) + .lanelet_pose_valid(false); +} + +auto makeCanonicalizedEntityStatus( + std::shared_ptr hdmap_utils, + traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, + traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, + const std::string name = "default_entity_name", + const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) + -> traffic_simulator::entity_status::CanonicalizedEntityStatus +{ + return traffic_simulator::entity_status::CanonicalizedEntityStatus( + makeEntityStatus(hdmap_utils, pose, bbox, speed, name, type), hdmap_utils); +} + +auto makeCanonicalizedEntityStatus( + std::shared_ptr hdmap_utils, geometry_msgs::msg::Pose pose, + traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, + const std::string name = "default_entity_name", + const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) + -> traffic_simulator::entity_status::CanonicalizedEntityStatus +{ + return traffic_simulator::entity_status::CanonicalizedEntityStatus( + makeEntityStatus(hdmap_utils, pose, bbox, speed, name, type), hdmap_utils); +} + +#endif // TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ \ No newline at end of file diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp index a60f9c5a3de..f3526cd6aed 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp @@ -668,11 +668,10 @@ TEST(TrafficLight, Bulb_make) */ TEST(TrafficLight, Bulb_trafficLightMessageConversion) { - using TrafficLight = traffic_simulator::TrafficLight; - using Color = TrafficLight::Color; - using Status = TrafficLight::Status; - using Shape = TrafficLight::Shape; - using Bulb = TrafficLight::Bulb; + using Color = traffic_simulator::TrafficLight::Color; + using Status = traffic_simulator::TrafficLight::Status; + using Shape = traffic_simulator::TrafficLight::Shape; + using Bulb = traffic_simulator::TrafficLight::Bulb; { constexpr auto bulb = Bulb(Color::red, Status::flashing, Shape::circle); @@ -730,8 +729,7 @@ TEST(TrafficLight, Bulb_trafficLightMessageConversion) TEST(TrafficLight, Color_make_wrong) { using Color = traffic_simulator::TrafficLight::Color; - Color color; - EXPECT_THROW(color = Color::make("wrong_color"), std::runtime_error); + EXPECT_THROW([[maybe_unused]] Color color = Color::make("wrong_color"), common::SyntaxError); } /** @@ -740,8 +738,7 @@ TEST(TrafficLight, Color_make_wrong) TEST(TrafficLight, Shape_make_wrong) { using Shape = traffic_simulator::TrafficLight::Shape; - Shape shape; - EXPECT_THROW(shape = Shape::make("wrong_shape"), std::runtime_error); + EXPECT_THROW([[maybe_unused]] Shape shape = Shape::make("wrong_shape"), common::SyntaxError); } /** @@ -750,8 +747,7 @@ TEST(TrafficLight, Shape_make_wrong) TEST(TrafficLight, Status_make_wrong) { using Status = traffic_simulator::TrafficLight::Status; - Status status; - EXPECT_THROW(status = Status::make("wrong_status"), std::runtime_error); + EXPECT_THROW([[maybe_unused]] Status status = Status::make("wrong_status"), common::SyntaxError); } /** @@ -761,8 +757,8 @@ TEST(TrafficLight, Bulb_make_wrong) { using Bulb = traffic_simulator::TrafficLight::Bulb; - EXPECT_THROW(Bulb("red flashing wrong_shape"), std::runtime_error); - EXPECT_THROW(Bulb("red wrong_status circle"), std::runtime_error); - EXPECT_THROW(Bulb("wrong_color flashing circle"), std::runtime_error); - EXPECT_THROW(Bulb("wrong_color wrong_status wrong_shape"), std::runtime_error); + EXPECT_THROW(Bulb("red flashing wrong_shape"), common::SyntaxError); + EXPECT_THROW(Bulb("red wrong_status circle"), common::SyntaxError); + EXPECT_THROW(Bulb("wrong_color flashing circle"), common::SyntaxError); + EXPECT_THROW(Bulb("wrong_color wrong_status wrong_shape"), common::SyntaxError); } From 66a3762e98a5f5b5c7a2fabb278d27cb3b5f5a49 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 17 Jun 2024 14:45:05 +0200 Subject: [PATCH 082/118] add newlines --- simulation/traffic_simulator/test/src/behavior/CMakeLists.txt | 2 ++ simulation/traffic_simulator/test/src/helper_functions.hpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt b/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt index 63631e745ca..15071521634 100644 --- a/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/behavior/CMakeLists.txt @@ -1,6 +1,8 @@ ament_add_gtest(test_behavior test_behavior.cpp) target_link_libraries(test_behavior traffic_simulator) + ament_add_gtest(test_route_planner test_route_planner.cpp) target_link_libraries(test_route_planner traffic_simulator) + ament_add_gtest(test_longitudinal_speed_planner test_longitudinal_speed_planner.cpp) target_link_libraries(test_longitudinal_speed_planner traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index f8f73560d0a..7b786a006e9 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -142,4 +142,4 @@ auto makeCanonicalizedEntityStatus( makeEntityStatus(hdmap_utils, pose, bbox, speed, name, type), hdmap_utils); } -#endif // TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ \ No newline at end of file +#endif // TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ From 03b72c9c1976f7f08780c69458afd83be5847f09 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 17 Jun 2024 15:10:40 +0200 Subject: [PATCH 083/118] remove unecessary assignment --- .../test/src/traffic_lights/test_traffic_light.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp index f3526cd6aed..b77ec381cab 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp @@ -728,8 +728,7 @@ TEST(TrafficLight, Bulb_trafficLightMessageConversion) */ TEST(TrafficLight, Color_make_wrong) { - using Color = traffic_simulator::TrafficLight::Color; - EXPECT_THROW([[maybe_unused]] Color color = Color::make("wrong_color"), common::SyntaxError); + EXPECT_THROW(traffic_simulator::TrafficLight::Color::make("wrong_color"), common::SyntaxError); } /** @@ -737,8 +736,7 @@ TEST(TrafficLight, Color_make_wrong) */ TEST(TrafficLight, Shape_make_wrong) { - using Shape = traffic_simulator::TrafficLight::Shape; - EXPECT_THROW([[maybe_unused]] Shape shape = Shape::make("wrong_shape"), common::SyntaxError); + EXPECT_THROW(traffic_simulator::TrafficLight::Shape::make("wrong_shape"), common::SyntaxError); } /** @@ -746,8 +744,7 @@ TEST(TrafficLight, Shape_make_wrong) */ TEST(TrafficLight, Status_make_wrong) { - using Status = traffic_simulator::TrafficLight::Status; - EXPECT_THROW([[maybe_unused]] Status status = Status::make("wrong_status"), common::SyntaxError); + EXPECT_THROW(traffic_simulator::TrafficLight::Status::make("wrong_status"), common::SyntaxError); } /** From a4dd832ecd180af989461ca72441cd7472c84314 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 17 Jun 2024 16:44:55 +0200 Subject: [PATCH 084/118] fix typo --- .../test/src/simulation_clock/test_simulation_clock.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp index 1994230fbda..96e57887d90 100644 --- a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp +++ b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp @@ -59,7 +59,7 @@ TEST(SimulationClock, getCurrentRosTime) } /** - * @note Test basic functionality used in API. Test scenario time calculation correctness with initialized obejct, + * @note Test basic functionality used in API. Test scenario time calculation correctness with initialized object, * npc logic started after several update() calls and additional update() calls after starting npc logic. */ TEST(SimulationClock, getCurrentScenarioTime) From 38fb9bba90f5c0bfd4e6ae7bca986d8714346215 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 17 Jun 2024 17:04:35 +0200 Subject: [PATCH 085/118] resolve conflict --- simulation/traffic_simulator/test/src/helper_functions.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index 7b786a006e9..4b68528b152 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -16,6 +16,7 @@ #define TRAFFIC_SIMULATOR__TEST__HELPER_FUNCTIONS_HPP_ #include +#include #include #include #include @@ -46,7 +47,7 @@ auto makeSmallBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion { - return quaternion_operation::convertEulerAngleToQuaternion( + return math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z(yaw)); } From ce42ebf8d3efdd09e2b86b3608031af0d97d062c Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 17 Jun 2024 17:16:25 +0200 Subject: [PATCH 086/118] resolve conflict --- simulation/traffic_simulator/test/src/helper_functions.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index 7b786a006e9..4b68528b152 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -16,6 +16,7 @@ #define TRAFFIC_SIMULATOR__TEST__HELPER_FUNCTIONS_HPP_ #include +#include #include #include #include @@ -46,7 +47,7 @@ auto makeSmallBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion { - return quaternion_operation::convertEulerAngleToQuaternion( + return math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z(yaw)); } From ec4c74f5fbf0cbfd033883d9bbf1a513abfdc788 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 17 Jun 2024 23:28:42 +0000 Subject: [PATCH 087/118] Bump urllib3 from 2.2.1 to 2.2.2 Bumps [urllib3](https://github.com/urllib3/urllib3) from 2.2.1 to 2.2.2. - [Release notes](https://github.com/urllib3/urllib3/releases) - [Changelog](https://github.com/urllib3/urllib3/blob/main/CHANGES.rst) - [Commits](https://github.com/urllib3/urllib3/compare/2.2.1...2.2.2) --- updated-dependencies: - dependency-name: urllib3 dependency-type: indirect ... Signed-off-by: dependabot[bot] --- poetry.lock | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/poetry.lock b/poetry.lock index 75b31ee16f2..2ae6f713e48 100644 --- a/poetry.lock +++ b/poetry.lock @@ -515,6 +515,7 @@ optional = false python-versions = ">=3.6" files = [ {file = "mkdocs-redirects-1.2.1.tar.gz", hash = "sha256:9420066d70e2a6bb357adf86e67023dcdca1857f97f07c7fe450f8f1fb42f861"}, + {file = "mkdocs_redirects-1.2.1-py3-none-any.whl", hash = "sha256:497089f9e0219e7389304cffefccdfa1cac5ff9509f2cb706f4c9b221726dffb"}, ] [package.dependencies] @@ -925,13 +926,13 @@ files = [ [[package]] name = "urllib3" -version = "2.2.1" +version = "2.2.2" description = "HTTP library with thread-safe connection pooling, file post, and more." optional = false python-versions = ">=3.8" files = [ - {file = "urllib3-2.2.1-py3-none-any.whl", hash = "sha256:450b20ec296a467077128bff42b73080516e71b56ff59a60a02bef2232c4fa9d"}, - {file = "urllib3-2.2.1.tar.gz", hash = "sha256:d0570876c61ab9e520d776c38acbbb5b05a776d3f9ff98a5c8fd5162a444cf19"}, + {file = "urllib3-2.2.2-py3-none-any.whl", hash = "sha256:a448b2f64d686155468037e1ace9f2d2199776e17f0a46610480d311f73e3472"}, + {file = "urllib3-2.2.2.tar.gz", hash = "sha256:dd505485549a7a552833da5e6063639d0d177c04f23bc3864e41e5dc5f612168"}, ] [package.extras] From f35ed44bb36c451bd3e3f47cf714a3316194e764 Mon Sep 17 00:00:00 2001 From: Release Bot Date: Tue, 18 Jun 2024 01:16:09 +0000 Subject: [PATCH 088/118] Bump version of scenario_simulator_v2 from version 2.1.4 to version 2.1.5 --- common/math/arithmetic/CHANGELOG.rst | 3 +++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 3 +++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 3 +++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 3 +++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 3 +++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 3 +++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 3 +++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 3 +++ map/kashiwanoha_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 3 +++ mock/cpp_mock_scenarios/package.xml | 2 +- openscenario/openscenario_experimental_catalog/CHANGELOG.rst | 3 +++ openscenario/openscenario_experimental_catalog/package.xml | 2 +- openscenario/openscenario_interpreter/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter/package.xml | 2 +- openscenario/openscenario_interpreter_example/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter_example/package.xml | 2 +- openscenario/openscenario_interpreter_msgs/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter_msgs/package.xml | 2 +- openscenario/openscenario_preprocessor/CHANGELOG.rst | 3 +++ openscenario/openscenario_preprocessor/package.xml | 2 +- openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst | 3 +++ openscenario/openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 3 +++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 3 +++ openscenario/openscenario_validator/package.xml | 2 +- rviz_plugins/openscenario_visualization/CHANGELOG.rst | 3 +++ rviz_plugins/openscenario_visualization/package.xml | 2 +- .../real_time_factor_control_rviz_plugin/CHANGELOG.rst | 3 +++ rviz_plugins/real_time_factor_control_rviz_plugin/package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 3 +++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 3 +++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 3 +++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 3 +++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 3 +++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 3 +++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 3 +++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 3 +++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 3 +++ test_runner/scenario_test_runner/package.xml | 2 +- 56 files changed, 112 insertions(+), 28 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index feafa5ad3c0..1e89aa9bfbf 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package arithmetic * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 510d19f289e..113ec8b97db 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 2.1.4 + 2.1.5 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index 8df31047d01..9e4c2781989 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package geometry * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge pull request `#1281 `_ from tier4/fix/remove_quaternion_operation diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 2e7dfc41e90..271113aaec8 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 2.1.4 + 2.1.5 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 617fb68ab54..e08d57bceb5 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package scenario_simulator_exception * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 3bafdfff009..e5b11f869ca 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 2.1.4 + 2.1.5 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index de122be8880..68c51ffc578 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package junit_exporter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 29fb0dad2db..d5a7e93f2a3 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 2.1.4 + 2.1.5 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index 7ae2845dffa..7bd1e5fcfe1 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package status_monitor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index b664a5dd99d..583b6c602f5 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 2.1.4 + 2.1.5 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 4f241fa2f75..6a83afa09f4 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package concealer * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 681173a9da6..6c23966a62a 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 2.1.4 + 2.1.5 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 919128b14b5..6f46e6be762 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -16,6 +16,9 @@ Changelog for package embree_vendor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 9090a547e3e..2393e2c0268 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 2.1.4 + 2.1.5 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 6da844d4b5b..499246a4c3e 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package kashiwanoha_map * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 78060893748..3afb7b1ee10 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 2.1.4 + 2.1.5 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index 5c60d2e8f33..d77729d27e7 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package cpp_mock_scenarios * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge pull request `#1281 `_ from tier4/fix/remove_quaternion_operation diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 332e5e276e3..48c540c3f28 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 2.1.4 + 2.1.5 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 3b2ddc18c48..9ff694ba031 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_experimental_catalog * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index d25fdfd58ed..aec0de1e767 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 2.1.4 + 2.1.5 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 41bbdc9b134..b9bf643a4d9 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -14,6 +14,9 @@ Changelog for package openscenario_interpreter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge pull request `#1281 `_ from tier4/fix/remove_quaternion_operation diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 57f832fa6ac..60d16dfd4e5 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 2.1.4 + 2.1.5 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 2af2bc3a476..60d99ebe17e 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_interpreter_example * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 393fca871f3..1961eb41961 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 2.1.4 + 2.1.5 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index 1c9316e048a..3bda709a798 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_interpreter_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 43552341374..b2816751677 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 2.1.4 + 2.1.5 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index 26c6d3fec13..0bf18ebbb51 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_preprocessor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 728d319fd8c..60082dee4ee 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 2.1.4 + 2.1.5 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index d9307bbaa5a..b5238d48e29 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_preprocessor_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index b920c262800..ebb997d4f83 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 2.1.4 + 2.1.5 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 49e98af902f..09c19469ae3 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -16,6 +16,9 @@ Changelog for package openscenario_utility * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index bb68a5dcf5c..d1e23e3e103 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 2.1.4 + 2.1.5 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index a4c1eefb9da..f98881261ea 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package openscenario_validator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index c2f3beeaefc..197ba699512 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 2.1.4 + 2.1.5 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index a35b5c8e9bd..ae19eb7c728 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_visualization * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 1dc52a9beb4..0a49e25ae55 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 2.1.4 + 2.1.5 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index a1b09e6aefb..2adc0442a8f 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index d39a9ccd9f8..03cd53f54a1 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 2.1.4 + 2.1.5 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 43a76ae05f7..ff18ebd18ac 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package scenario_simulator_v2 * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 67897e18872..1afb763dbcd 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 2.1.4 + 2.1.5 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 9c26b4e96fa..462aa6670b7 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package behavior_tree_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge pull request `#1281 `_ from tier4/fix/remove_quaternion_operation diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 30c07969c7c..78cc80d80e5 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 2.1.4 + 2.1.5 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 3b59574c869..6d5f3b75be7 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package do_nothing_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge pull request `#1281 `_ from tier4/fix/remove_quaternion_operation diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 1f50295bd35..691c210fa18 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 2.1.4 + 2.1.5 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 96e64e595bc..9db7fd8927f 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package simple_sensor_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge pull request `#1281 `_ from tier4/fix/remove_quaternion_operation diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 4ad80262459..7ec61d0df84 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 2.1.4 + 2.1.5 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index baa6af4f01a..6b6cd7f1034 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package simulation_interface * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 1e4e8b46a50..d297ed10153 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 2.1.4 + 2.1.5 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index 1533e609f2b..4a68c52738f 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package traffic_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge pull request `#1281 `_ from tier4/fix/remove_quaternion_operation diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index a9f99a9a156..cbd9caad0be 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 2.1.4 + 2.1.5 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 493c562fefb..9ac138642c9 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index b388fec2d43..ee4c359fa40 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 2.1.4 + 2.1.5 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index da110aa6480..429a7bc8a87 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package random_test_runner * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 142eb007044..2132d95f0d7 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 2.1.4 + 2.1.5 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 4a92a68fcb8..3f995c14ae0 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -23,6 +23,9 @@ Changelog for package scenario_test_runner * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.5 (2024-06-18) +------------------ + 2.1.4 (2024-06-14) ------------------ * Merge branch 'master' into fix/remove_quaternion_operation diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index a4ac02a4822..eddfbd3f505 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 2.1.4 + 2.1.5 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From b095a3ca33aea78604117341d72ef0fdb75be224 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Tue, 18 Jun 2024 12:39:32 +0900 Subject: [PATCH 089/118] Revert "Optimize `EntityManager::broadcastEntityTransform` to Execute Only Once" --- simulation/traffic_simulator/src/entity/entity_manager.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index d834314b489..571adc2dedf 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -39,13 +39,6 @@ namespace entity { void EntityManager::broadcastEntityTransform() { - static bool isSend; - if (isSend) { - return; - } else { - isSend = true; - } - using math::geometry::operator/; using math::geometry::operator*; using math::geometry::operator+=; From bd366bce147e65d5991b62db333cf35153dd96fb Mon Sep 17 00:00:00 2001 From: Release Bot Date: Tue, 18 Jun 2024 06:29:24 +0000 Subject: [PATCH 090/118] Bump version of scenario_simulator_v2 from version 2.1.5 to version 2.1.6 --- common/math/arithmetic/CHANGELOG.rst | 3 +++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 3 +++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 3 +++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 3 +++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 3 +++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 3 +++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 3 +++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 3 +++ map/kashiwanoha_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 3 +++ mock/cpp_mock_scenarios/package.xml | 2 +- .../openscenario_experimental_catalog/CHANGELOG.rst | 3 +++ openscenario/openscenario_experimental_catalog/package.xml | 2 +- openscenario/openscenario_interpreter/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter/package.xml | 2 +- .../openscenario_interpreter_example/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter_example/package.xml | 2 +- openscenario/openscenario_interpreter_msgs/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter_msgs/package.xml | 2 +- openscenario/openscenario_preprocessor/CHANGELOG.rst | 3 +++ openscenario/openscenario_preprocessor/package.xml | 2 +- openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst | 3 +++ openscenario/openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 3 +++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 3 +++ openscenario/openscenario_validator/package.xml | 2 +- rviz_plugins/openscenario_visualization/CHANGELOG.rst | 3 +++ rviz_plugins/openscenario_visualization/package.xml | 2 +- .../real_time_factor_control_rviz_plugin/CHANGELOG.rst | 3 +++ .../real_time_factor_control_rviz_plugin/package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 3 +++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 3 +++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 3 +++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 3 +++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 3 +++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 7 +++++++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 3 +++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 3 +++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 3 +++ test_runner/scenario_test_runner/package.xml | 2 +- 56 files changed, 116 insertions(+), 28 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 1e89aa9bfbf..fe6e4fc23ab 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package arithmetic * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 113ec8b97db..743117634cc 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 2.1.5 + 2.1.6 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index 9e4c2781989..b4ad57fd22c 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package geometry * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 271113aaec8..f9275a2d653 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 2.1.5 + 2.1.6 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index e08d57bceb5..13010d66dcc 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package scenario_simulator_exception * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index e5b11f869ca..3bad578cbf4 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 2.1.5 + 2.1.6 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index 68c51ffc578..018674606a6 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package junit_exporter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index d5a7e93f2a3..9f0d73e05ce 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 2.1.5 + 2.1.6 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index 7bd1e5fcfe1..b641784f65b 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package status_monitor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 583b6c602f5..4b4ab3c172a 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 2.1.5 + 2.1.6 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 6a83afa09f4..679f76600c6 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package concealer * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 6c23966a62a..52060c1ee4b 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 2.1.5 + 2.1.6 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 6f46e6be762..2f873ad07c3 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -16,6 +16,9 @@ Changelog for package embree_vendor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 2393e2c0268..2e68b1959b8 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 2.1.5 + 2.1.6 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 499246a4c3e..bab69cdd6a6 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package kashiwanoha_map * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 3afb7b1ee10..de166898365 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 2.1.5 + 2.1.6 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index d77729d27e7..46acd42b47f 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package cpp_mock_scenarios * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 48c540c3f28..2e6bd56cd94 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 2.1.5 + 2.1.6 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 9ff694ba031..8a20eddbd4f 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_experimental_catalog * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index aec0de1e767..92ea64d2fdb 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 2.1.5 + 2.1.6 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index b9bf643a4d9..71c2330ead3 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -14,6 +14,9 @@ Changelog for package openscenario_interpreter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 60d16dfd4e5..36c0ba54bbb 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 2.1.5 + 2.1.6 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 60d99ebe17e..08c14706900 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_interpreter_example * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 1961eb41961..3bc024b2c08 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 2.1.5 + 2.1.6 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index 3bda709a798..bbd47166497 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_interpreter_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index b2816751677..0991b966377 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 2.1.5 + 2.1.6 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index 0bf18ebbb51..c22f3f24c90 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_preprocessor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 60082dee4ee..5b5d5fb9979 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 2.1.5 + 2.1.6 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index b5238d48e29..2507a9b8660 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_preprocessor_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index ebb997d4f83..f0898209dcb 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 2.1.5 + 2.1.6 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 09c19469ae3..187c3ee6451 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -16,6 +16,9 @@ Changelog for package openscenario_utility * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index d1e23e3e103..33ca0f13fb2 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 2.1.5 + 2.1.6 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index f98881261ea..f14553dfa30 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package openscenario_validator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 197ba699512..9ddafb4f4f0 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 2.1.5 + 2.1.6 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index ae19eb7c728..5543ce346ae 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_visualization * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 0a49e25ae55..6d1b47cf762 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 2.1.5 + 2.1.6 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 2adc0442a8f..d9383eb68c4 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index 03cd53f54a1..8bc3a9bde2a 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 2.1.5 + 2.1.6 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index ff18ebd18ac..437e937e422 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package scenario_simulator_v2 * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 1afb763dbcd..9f5c0aa94bf 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 2.1.5 + 2.1.6 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 462aa6670b7..fbb093871c6 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package behavior_tree_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 78cc80d80e5..bed1468bbda 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 2.1.5 + 2.1.6 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 6d5f3b75be7..3c21518202f 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package do_nothing_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 691c210fa18..e8dda243cfb 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 2.1.5 + 2.1.6 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 9db7fd8927f..dab533b60bc 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package simple_sensor_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 7ec61d0df84..3bff2ed2e31 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 2.1.5 + 2.1.6 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 6b6cd7f1034..e530849b7d7 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package simulation_interface * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index d297ed10153..7cab5a458e3 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 2.1.5 + 2.1.6 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index 4a68c52738f..6e1dd7c6247 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -13,6 +13,13 @@ Changelog for package traffic_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ +* Merge pull request `#1289 `_ from tier4/revert-1284-fix/issue1276 + Revert "Optimize `EntityManager::broadcastEntityTransform` to Execute Only Once" +* Revert "Optimize `EntityManager::broadcastEntityTransform` to Execute Only Once" +* Contributors: Masaya Kataoka + 2.1.5 (2024-06-18) ------------------ diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index cbd9caad0be..2279d7ea6fe 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 2.1.5 + 2.1.6 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 9ac138642c9..f78d02ada1f 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index ee4c359fa40..503165019e7 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 2.1.5 + 2.1.6 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 429a7bc8a87..41cd8bc49c4 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package random_test_runner * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 2132d95f0d7..d9a662a6493 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 2.1.5 + 2.1.6 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 3f995c14ae0..16e0486d814 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -23,6 +23,9 @@ Changelog for package scenario_test_runner * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.6 (2024-06-18) +------------------ + 2.1.5 (2024-06-18) ------------------ diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index eddfbd3f505..f5255866d4e 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 2.1.5 + 2.1.6 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 66138a1d749c4d97c177daf975b5afdee138e4d4 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Tue, 18 Jun 2024 10:15:21 +0200 Subject: [PATCH 091/118] getParameter -> getROS2Parameter Signed-off-by: Mateusz Palczuk --- mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp | 2 +- .../include/openscenario_interpreter/simulator_core.hpp | 6 +++--- .../traffic_simulator/include/traffic_simulator/api/api.hpp | 2 +- simulation/traffic_simulator/src/api/api.cpp | 4 ++-- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index b339e781252..c7d1b5fc030 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -101,7 +101,7 @@ void CppScenarioNode::spawnEgoEntity( api_.attachOccupancyGridSensor([this] { simulation_api_schema::OccupancyGridSensorConfiguration configuration; // clang-format off - configuration.set_architecture_type(api_.getParameter("architecture_type", "awf/universe")); + configuration.set_architecture_type(api_.getROS2Parameter("architecture_type", "awf/universe")); configuration.set_entity("ego"); configuration.set_filter_by_range(true); configuration.set_height(200); diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index a40ab21c812..927d9374031 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -339,7 +339,7 @@ class SimulatorCore core->attachDetectionSensor([&]() { simulation_api_schema::DetectionSensorConfiguration configuration; // clang-format off - configuration.set_architecture_type(core->getParameter("architecture_type", "awf/universe")); + configuration.set_architecture_type(core->getROS2Parameter("architecture_type", "awf/universe")); configuration.set_entity(entity_ref); configuration.set_detect_all_objects_in_range(controller.properties.template get("isClairvoyant")); configuration.set_object_recognition_delay(controller.properties.template get("detectedObjectPublishingDelay")); @@ -356,7 +356,7 @@ class SimulatorCore core->attachOccupancyGridSensor([&]() { simulation_api_schema::OccupancyGridSensorConfiguration configuration; // clang-format off - configuration.set_architecture_type(core->getParameter("architecture_type", "awf/universe")); + configuration.set_architecture_type(core->getROS2Parameter("architecture_type", "awf/universe")); configuration.set_entity(entity_ref); configuration.set_filter_by_range(controller.properties.template get("isClairvoyant")); configuration.set_height(200); @@ -371,7 +371,7 @@ class SimulatorCore core->attachPseudoTrafficLightDetector([&]() { simulation_api_schema::PseudoTrafficLightDetectorConfiguration configuration; configuration.set_architecture_type( - core->getParameter("architecture_type", "awf/universe")); + core->getROS2Parameter("architecture_type", "awf/universe")); return configuration; }()); diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index d3e6bc4e1f4..1bf4f3e33be 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -389,7 +389,7 @@ class API #undef FORWARD_TO_ENTITY_MANAGER template - auto getParameter(const std::string & name, Ts &&... xs) const -> decltype(auto) + auto getROS2Parameter(const std::string & name, Ts &&... xs) const -> decltype(auto) { return node_parameter_handler_->getParameter(name, std::forward(xs)...); } diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 09ad9ec99a2..fbf565729d6 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -218,7 +218,7 @@ bool API::attachDetectionSensor( double object_recognition_delay) { return attachDetectionSensor(helper::constructDetectionSensorConfiguration( - entity_name, getParameter("architecture_type", "awf/universe"), 0.1, + entity_name, getROS2Parameter("architecture_type", "awf/universe"), 0.1, detection_sensor_range, detect_all_objects_in_range, pos_noise_stddev, random_seed, probability_of_lost, object_recognition_delay)); } @@ -251,7 +251,7 @@ bool API::attachLidarSensor( const helper::LidarType lidar_type) { return attachLidarSensor(helper::constructLidarConfiguration( - lidar_type, entity_name, getParameter("architecture_type", "awf/universe"), + lidar_type, entity_name, getROS2Parameter("architecture_type", "awf/universe"), lidar_sensor_delay)); } From 5a462a14ffb9171090f6046d94b1af736ac92126 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 18 Jun 2024 10:36:34 +0200 Subject: [PATCH 092/118] file structure --- simulation/simple_sensor_simulator/test/CMakeLists.txt | 3 ++- .../src/{vehicle_simulation => }/test_scenario_simulator.cpp | 2 +- .../test/src/vehicle_simulation/CMakeLists.txt | 2 -- 3 files changed, 3 insertions(+), 4 deletions(-) rename simulation/simple_sensor_simulator/test/src/{vehicle_simulation => }/test_scenario_simulator.cpp (99%) delete mode 100644 simulation/simple_sensor_simulator/test/src/vehicle_simulation/CMakeLists.txt diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt index ff941219a36..caa6e54495c 100644 --- a/simulation/simple_sensor_simulator/test/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/CMakeLists.txt @@ -10,4 +10,5 @@ target_link_libraries(test_vertex simple_sensor_simulator_component) ament_add_gtest(test_raycaster src/lidar/test_raycaster.cpp) target_link_libraries(test_raycaster simple_sensor_simulator_component ${Protobuf_LIBRARIES}) -add_subdirectory(src/vehicle_simulation) \ No newline at end of file +ament_add_gtest(test_scenario_simulator src/test_scenario_simulator.cpp) +target_link_libraries(test_scenario_simulator simple_sensor_simulator_component) diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_scenario_simulator.cpp b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp similarity index 99% rename from simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_scenario_simulator.cpp rename to simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp index c9c593a356f..2b3d04d0990 100644 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_scenario_simulator.cpp +++ b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp @@ -24,7 +24,7 @@ #include #include -#include "../utils/expect_eq_macros.hpp" +#include "utils/expect_eq_macros.hpp" auto makeInitializeRequest() -> simulation_api_schema::InitializeRequest { diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/CMakeLists.txt deleted file mode 100644 index 4c46707be29..00000000000 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -ament_add_gtest(test_scenario_simulator test_scenario_simulator.cpp) -target_link_libraries(test_scenario_simulator simple_sensor_simulator_component) From 55b686f5098a3b5a35a6fa03588183f95a9ed028 Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Tue, 18 Jun 2024 18:03:50 +0900 Subject: [PATCH 093/118] add API::clearRoute() --- .../include/traffic_simulator/api/api.hpp | 1 + .../include/traffic_simulator/entity/ego_entity.hpp | 2 ++ .../include/traffic_simulator/entity/entity_base.hpp | 2 ++ .../include/traffic_simulator/entity/entity_manager.hpp | 1 + simulation/traffic_simulator/src/entity/ego_entity.cpp | 2 ++ simulation/traffic_simulator/src/entity/entity_base.cpp | 7 +++++++ 6 files changed, 15 insertions(+) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index d50990b863a..a3b5968721e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -364,6 +364,7 @@ class API FORWARD_TO_ENTITY_MANAGER(requestFollowTrajectory); FORWARD_TO_ENTITY_MANAGER(requestSpeedChange); FORWARD_TO_ENTITY_MANAGER(requestWalkStraight); + FORWARD_TO_ENTITY_MANAGER(requestClearRoute); FORWARD_TO_ENTITY_MANAGER(resetBehaviorPlugin); FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate); FORWARD_TO_ENTITY_MANAGER(resetV2ITrafficLightPublishRate); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index fdf0aa0d171..79a12413ca2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -110,6 +110,8 @@ class EgoEntity : public VehicleEntity const speed_change::RelativeTargetSpeed &, const speed_change::Transition, const speed_change::Constraint, const bool continuous) -> void override; + void requestClearRoute() override; + auto isControlledBySimulator() const -> bool override; auto setControlledBySimulator(bool state) -> void override diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index 47cb91d6ea4..8d771799cbb 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -166,6 +166,8 @@ class EntityBase virtual void requestSpeedChange(const speed_change::RelativeTargetSpeed &, bool); + virtual void requestClearRoute(); + virtual auto isControlledBySimulator() const -> bool; virtual auto setControlledBySimulator(bool) -> void; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index f7f1af6764e..3aef3085447 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -309,6 +309,7 @@ class EntityManager FORWARD_TO_ENTITY(requestFollowTrajectory, ); FORWARD_TO_ENTITY(requestLaneChange, ); FORWARD_TO_ENTITY(requestWalkStraight, ); + FORWARD_TO_ENTITY(requestClearRoute, ); FORWARD_TO_ENTITY(setAcceleration, ); FORWARD_TO_ENTITY(setAccelerationLimit, ); FORWARD_TO_ENTITY(setAccelerationRateLimit, ); diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index ca5e551b472..5b640fbd3c8 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -260,6 +260,8 @@ auto EgoEntity::requestSpeedChange( "purposes only."); } +void EgoEntity::requestClearRoute() { field_operator_application->clearRoute(); } + auto EgoEntity::getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints & { diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 6f4a71d5850..0e47bd8009f 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -271,6 +271,13 @@ void EntityBase::requestSpeedChangeWithConstantAcceleration( } } +void EntityBase::requestClearRoute() +{ + THROW_SEMANTIC_ERROR( + "requestClearRoute is only supported for EgoEntity. The specified Entity is not an EgoEntity. " + "Please check the scenario carefully."); +} + void EntityBase::requestSpeedChangeWithTimeConstraint( const double target_speed, const speed_change::Transition transition, double acceleration_time) { From f4db270be6d63f05cee19b81f49bb5a4ccebd454 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Tue, 18 Jun 2024 12:08:09 +0200 Subject: [PATCH 094/118] test: [RJD-937] to Implement Unit tests on simple_sensor_simulator - Corrected directory structure - Fixed an issue with a mistake --- simulation/simple_sensor_simulator/test/CMakeLists.txt | 6 +++--- .../src/{utils => sensor_simulation}/expect_eq_macros.hpp | 0 .../test/src/{ => sensor_simulation}/lidar/CMakeLists.txt | 0 .../src/{ => sensor_simulation}/lidar/test_raycaster.cpp | 0 .../src/{ => sensor_simulation}/lidar/test_raycaster.hpp | 0 .../{ => sensor_simulation}/occupancy_grid/CMakeLists.txt | 0 .../occupancy_grid/test_grid_traversal.cpp | 2 +- .../occupancy_grid/test_grid_traversal.hpp | 0 .../src/{ => sensor_simulation}/primitives/CMakeLists.txt | 0 .../src/{ => sensor_simulation}/primitives/test_box.cpp | 2 +- .../src/{ => sensor_simulation}/primitives/test_vertex.cpp | 2 +- 11 files changed, 6 insertions(+), 6 deletions(-) rename simulation/simple_sensor_simulator/test/src/{utils => sensor_simulation}/expect_eq_macros.hpp (100%) rename simulation/simple_sensor_simulator/test/src/{ => sensor_simulation}/lidar/CMakeLists.txt (100%) rename simulation/simple_sensor_simulator/test/src/{ => sensor_simulation}/lidar/test_raycaster.cpp (100%) rename simulation/simple_sensor_simulator/test/src/{ => sensor_simulation}/lidar/test_raycaster.hpp (100%) rename simulation/simple_sensor_simulator/test/src/{ => sensor_simulation}/occupancy_grid/CMakeLists.txt (100%) rename simulation/simple_sensor_simulator/test/src/{ => sensor_simulation}/occupancy_grid/test_grid_traversal.cpp (98%) rename simulation/simple_sensor_simulator/test/src/{ => sensor_simulation}/occupancy_grid/test_grid_traversal.hpp (100%) rename simulation/simple_sensor_simulator/test/src/{ => sensor_simulation}/primitives/CMakeLists.txt (100%) rename simulation/simple_sensor_simulator/test/src/{ => sensor_simulation}/primitives/test_box.cpp (98%) rename simulation/simple_sensor_simulator/test/src/{ => sensor_simulation}/primitives/test_vertex.cpp (98%) diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt index 1dab2d4f167..e7f6401dfef 100644 --- a/simulation/simple_sensor_simulator/test/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/CMakeLists.txt @@ -1,3 +1,3 @@ -add_subdirectory(src/lidar) -add_subdirectory(src/primitives) -add_subdirectory(src/occupancy_grid) +add_subdirectory(src/sensor_simulation/lidar) +add_subdirectory(src/sensor_simulation/primitives) +add_subdirectory(src/sensor_simulation/occupancy_grid) diff --git a/simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/expect_eq_macros.hpp similarity index 100% rename from simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp rename to simulation/simple_sensor_simulator/test/src/sensor_simulation/expect_eq_macros.hpp diff --git a/simulation/simple_sensor_simulator/test/src/lidar/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt similarity index 100% rename from simulation/simple_sensor_simulator/test/src/lidar/CMakeLists.txt rename to simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt diff --git a/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.cpp similarity index 100% rename from simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.cpp rename to simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.cpp diff --git a/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.hpp similarity index 100% rename from simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.hpp rename to simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.hpp diff --git a/simulation/simple_sensor_simulator/test/src/occupancy_grid/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt similarity index 100% rename from simulation/simple_sensor_simulator/test/src/occupancy_grid/CMakeLists.txt rename to simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt diff --git a/simulation/simple_sensor_simulator/test/src/occupancy_grid/test_grid_traversal.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_grid_traversal.cpp similarity index 98% rename from simulation/simple_sensor_simulator/test/src/occupancy_grid/test_grid_traversal.cpp rename to simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_grid_traversal.cpp index 8abe59e511f..1b55750f75d 100644 --- a/simulation/simple_sensor_simulator/test/src/occupancy_grid/test_grid_traversal.cpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_grid_traversal.cpp @@ -41,7 +41,7 @@ TEST_F(GridTraversalTest, end) } /** - * @note Test basic functionality. Test iterator dereferencing correcntess with a sample iterator. + * @note Test basic functionality. Test iterator dereferencing correctness with a sample iterator. */ TEST_F(GridTraversalTest, iterator_operator_dereference) { diff --git a/simulation/simple_sensor_simulator/test/src/occupancy_grid/test_grid_traversal.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_grid_traversal.hpp similarity index 100% rename from simulation/simple_sensor_simulator/test/src/occupancy_grid/test_grid_traversal.hpp rename to simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_grid_traversal.hpp diff --git a/simulation/simple_sensor_simulator/test/src/primitives/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt similarity index 100% rename from simulation/simple_sensor_simulator/test/src/primitives/CMakeLists.txt rename to simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt diff --git a/simulation/simple_sensor_simulator/test/src/primitives/test_box.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp similarity index 98% rename from simulation/simple_sensor_simulator/test/src/primitives/test_box.cpp rename to simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp index 5eac7d465de..cd243afa1be 100644 --- a/simulation/simple_sensor_simulator/test/src/primitives/test_box.cpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp @@ -19,7 +19,7 @@ #include #include -#include "../utils/expect_eq_macros.hpp" +#include "../expect_eq_macros.hpp" using namespace simple_sensor_simulator; using namespace simple_sensor_simulator::primitives; diff --git a/simulation/simple_sensor_simulator/test/src/primitives/test_vertex.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp similarity index 98% rename from simulation/simple_sensor_simulator/test/src/primitives/test_vertex.cpp rename to simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp index a631601ad46..66da446cec3 100644 --- a/simulation/simple_sensor_simulator/test/src/primitives/test_vertex.cpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp @@ -19,7 +19,7 @@ #include #include -#include "../utils/expect_eq_macros.hpp" +#include "../expect_eq_macros.hpp" using namespace simple_sensor_simulator; From bdfb1271a0822c52b0467d6182d492320ec0e5fa Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Tue, 18 Jun 2024 13:30:35 +0200 Subject: [PATCH 095/118] test: [RJD-937] to Implement Unit tests on simple_sensor_simulator - Fixed an issue with missing package --- simulation/simple_sensor_simulator/test/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt index e7f6401dfef..16f47547ef3 100644 --- a/simulation/simple_sensor_simulator/test/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/CMakeLists.txt @@ -1,3 +1,5 @@ +find_package(quaternion_operation REQUIRED) + add_subdirectory(src/sensor_simulation/lidar) add_subdirectory(src/sensor_simulation/primitives) add_subdirectory(src/sensor_simulation/occupancy_grid) From 86ac45605b0dbb20af3f945906c0bb79bc60aef9 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 18 Jun 2024 13:49:02 +0200 Subject: [PATCH 096/118] merge conflicts --- .../simple_sensor_simulator/CMakeLists.txt | 10 -- .../test/CMakeLists.txt | 6 + .../test/src/lidar/test_raycaster.cpp | 152 ------------------ .../test/src/lidar/test_raycaster.hpp | 77 --------- .../test/src/primitives/test_box.cpp | 70 -------- .../test/src/primitives/test_vertex.cpp | 123 -------------- .../test/src/test_scenario_simulator.cpp | 2 +- .../test/src/utils/expect_eq_macros.hpp | 68 -------- 8 files changed, 7 insertions(+), 501 deletions(-) delete mode 100644 simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.cpp delete mode 100644 simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.hpp delete mode 100644 simulation/simple_sensor_simulator/test/src/primitives/test_box.cpp delete mode 100644 simulation/simple_sensor_simulator/test/src/primitives/test_vertex.cpp delete mode 100644 simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt index 284768558a1..692063321aa 100644 --- a/simulation/simple_sensor_simulator/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/CMakeLists.txt @@ -86,16 +86,6 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) ament_lint_auto_find_test_dependencies() -<<<<<<< HEAD - - add_subdirectory(test) - find_package(Protobuf REQUIRED) - find_package(rclcpp REQUIRED) - include_directories(${Protobuf_INCLUDE_DIRS}) - target_link_libraries(simple_sensor_simulator_component ${Protobuf_LIBRARIES}) -endif() -======= ->>>>>>> f4db270be6d63f05cee19b81f49bb5a4ccebd454 add_subdirectory(test) endif() diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt index e7f6401dfef..6f86259750e 100644 --- a/simulation/simple_sensor_simulator/test/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/CMakeLists.txt @@ -1,3 +1,9 @@ add_subdirectory(src/sensor_simulation/lidar) add_subdirectory(src/sensor_simulation/primitives) add_subdirectory(src/sensor_simulation/occupancy_grid) + +find_package(Protobuf REQUIRED) +include_directories(${Protobuf_INCLUDE_DIRS}) + +ament_add_gtest(test_scenario_simulator src/test_scenario_simulator.cpp) +target_link_libraries(test_scenario_simulator simple_sensor_simulator_component ${Protobuf_LIBRARIES}) diff --git a/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.cpp b/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.cpp deleted file mode 100644 index 57a7308a6e8..00000000000 --- a/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.cpp +++ /dev/null @@ -1,152 +0,0 @@ -// 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_raycaster.hpp" - -/** - * @note Test basic functionality - * - * Test adding a primitive to scene correctness with a Box. - */ -TEST_F(RaycasterTest, addPrimitive_box) -{ - EXPECT_NO_THROW(raycaster_->addPrimitive( - box_name_, box_depth_, box_width_, box_height_, box_pose_)); -} - -/** - * @note Test function behavior when adding two Boxes with identical names to a scene. - */ -TEST_F(RaycasterTest, addPrimitive_twoIdenticalNames) -{ - raycaster_->addPrimitive( - box_name_, box_depth_, box_width_, box_height_, box_pose_); - - EXPECT_THROW( - raycaster_->addPrimitive( - box_name_, box_depth_, box_width_, box_height_, box_pose_), - std::runtime_error); -} - -/** - * @note Test basic functionality - * - * Test raycasting correctness with an empty scene. - */ -TEST_F(RaycasterTest, raycast_empty) -{ - auto cloud = raycaster_->raycast(frame_id_, stamp_, origin_); - - EXPECT_EQ(cloud.width, 0); - EXPECT_EQ(cloud.header.frame_id, frame_id_); - EXPECT_EQ(cloud.header.stamp, stamp_); -} - -/** - * @note Test basic functionality - * - * Test raycasting correctness with one box on the scene. - */ -TEST_F(RaycasterTest, raycast_box) -{ - raycaster_->addPrimitive( - box_name_, box_depth_, box_width_, box_height_, box_pose_); - - auto cloud = raycaster_->raycast(frame_id_, stamp_, origin_); - - EXPECT_GT(cloud.width, 0); - EXPECT_EQ(cloud.header.frame_id, frame_id_); - EXPECT_EQ(cloud.header.stamp, stamp_); -} - -/** - * @note Test basic functionality - * - * Test setting ray directions with a lidar configuration that has one ray which intersects with the - * only box on the scene. - */ -TEST_F(RaycasterTest, setDirection_oneBox) -{ - raycaster_->addPrimitive( - box_name_, box_depth_, box_width_, box_height_, box_pose_); - - simulation_api_schema::LidarConfiguration config; - config.add_vertical_angles(0.0); // Only one vertical angle for a horizontal ring - config.set_horizontal_resolution(degToRad(1.0)); // Set horizontal resolution to 1 degree - - raycaster_->setDirection(config); - - auto cloud = raycaster_->raycast(frame_id_, stamp_, origin_); - - EXPECT_GT(cloud.width, 0); - EXPECT_EQ(cloud.header.frame_id, frame_id_); - EXPECT_EQ(cloud.header.stamp, stamp_); -} - -/** - * @note Test basic functionality - * - * Test setting ray directions with a lidar configuration that has a ring of horizontal rays which - * intersect with boxes positioned on the ring so that they intersect with the rays. - */ -TEST_F(RaycasterTest, setDirection_manyBoxes) -{ - constexpr double radius = 5.0; - constexpr int num_boxes = 10; - constexpr double angle_increment = 2 * M_PI / num_boxes; - - for (int i = 0; i < num_boxes; ++i) { - const double angle = i * angle_increment; - const auto box_pose = - geometry_msgs::build() - .position(geometry_msgs::build() - .x(radius * cos(angle)) - .y(radius * sin(angle)) - .z(0)) - .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); - - std::string name = "box" + std::to_string(i); - raycaster_->addPrimitive(name, box_depth_, box_width_, box_height_, box_pose); - } - - simulation_api_schema::LidarConfiguration config; - config.add_vertical_angles(0.0); // Only one vertical angle for a horizontal ring - config.set_horizontal_resolution(degToRad(5)); - - raycaster_->setDirection(config); - - auto cloud = raycaster_->raycast(frame_id_, stamp_, origin_); - - EXPECT_GT(cloud.width, 0); - EXPECT_EQ(cloud.header.frame_id, frame_id_); - EXPECT_EQ(cloud.header.stamp, stamp_); -} - -/** - * @note Test basic functionality - * - * Test detected objects obtaining from the statuses list containing Ego. - */ -TEST_F(RaycasterTest, getDetectedObjects) -{ - raycaster_->addPrimitive( - box_name_, box_depth_, box_width_, box_height_, box_pose_); - - raycaster_->raycast(frame_id_, stamp_, origin_); - - const auto & detected_objects = raycaster_->getDetectedObject(); - - ASSERT_FALSE(detected_objects.empty()); - EXPECT_EQ(detected_objects[0], box_name_); -} diff --git a/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.hpp b/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.hpp deleted file mode 100644 index 37fad304554..00000000000 --- a/simulation/simple_sensor_simulator/test/src/lidar/test_raycaster.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// 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_RAYCASTER_HPP_ -#define SIMPLE_SENSOR_SIMULATOR__TEST__TEST_RAYCASTER_HPP_ - -#include - -#include -#include -#include -#include -#include - -using namespace simple_sensor_simulator; -using namespace geometry_msgs::msg; - -constexpr static double degToRad(double deg) { return deg * M_PI / 180.0; } - -class RaycasterTest : public ::testing::Test -{ -protected: - void SetUp() override - { - raycaster_ = std::make_unique(); - configureLidar(); - - origin_ = - geometry_msgs::build() - .position(geometry_msgs::build().x(0).y(0).z(0)) - .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); - box_pose_ = - geometry_msgs::build() - .position(geometry_msgs::build().x(5.0).y(0).z(0)) - .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); - } - - std::unique_ptr raycaster_; - simulation_api_schema::LidarConfiguration config_; - - const float box_depth_{1.0f}; - const float box_width_{1.0f}; - const float box_height_{1.0f}; - const std::string box_name_{"box"}; - - const rclcpp::Time stamp_{0}; - const std::string frame_id_{"frame_id"}; - - geometry_msgs::msg::Pose origin_; - geometry_msgs::msg::Pose box_pose_; - -private: - auto configureLidar() -> void - { - // Setting vertical angles from -15 to +15 degrees in 2 degree steps - for (double angle = -15.0; angle <= 15.0; angle += 2.0) { - config_.add_vertical_angles(degToRad(angle)); - } - // Setting horizontal resolution to 0.5 degrees - config_.set_horizontal_resolution(degToRad(0.5)); - - raycaster_->setDirection(config_); - } -}; - -#endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_RAYCASTER_HPP_ diff --git a/simulation/simple_sensor_simulator/test/src/primitives/test_box.cpp b/simulation/simple_sensor_simulator/test/src/primitives/test_box.cpp deleted file mode 100644 index 5543e200cba..00000000000 --- a/simulation/simple_sensor_simulator/test/src/primitives/test_box.cpp +++ /dev/null @@ -1,70 +0,0 @@ -// 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 -#include - -#include -#include -#include - -#include "../utils/expect_eq_macros.hpp" - -using namespace simple_sensor_simulator; -using namespace simple_sensor_simulator::primitives; - -/** - * @note Test initialization correctness - * - * The goal is to test whether the vertices and indices are initialized to create a box shape (can - * use get2DConvexHull or other specially created const reference accessors). - */ -TEST(BoxTest, Box) -{ - const std::vector expected_vertices = { - {-0.5, -0.5, -0.5}, {-0.5, -0.5, 0.5}, {-0.5, 0.5, -0.5}, {-0.5, 0.5, 0.5}, - {0.5, -0.5, -0.5}, {0.5, -0.5, 0.5}, {0.5, 0.5, -0.5}, {0.5, 0.5, 0.5}}; - - // Indices - const std::vector expected_triangles = {{0, 1, 2}, {1, 3, 2}, {4, 6, 5}, {5, 6, 7}, - {0, 4, 1}, {1, 4, 5}, {2, 3, 6}, {3, 7, 6}, - {0, 2, 4}, {2, 6, 4}, {1, 5, 3}, {3, 5, 7}}; - - const auto pose = - geometry_msgs::build() - .position(geometry_msgs::build().x(0).y(0).z(0)) - .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); - - Box box(1.0, 1.0, 1.0, pose); - - auto vertices = box.getVertex(); - auto triangles = box.getTriangles(); - - ASSERT_EQ(triangles.size(), expected_triangles.size()); - ASSERT_EQ(vertices.size(), expected_vertices.size()); - - for (size_t i = 0; i < triangles.size(); ++i) { - EXPECT_TRIANGLE_EQ(triangles[i], expected_triangles[i]); - } - - for (size_t i = 0; i < vertices.size(); ++i) { - EXPECT_VERTEX_EQ(vertices[i], expected_vertices[i]); - } -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/primitives/test_vertex.cpp b/simulation/simple_sensor_simulator/test/src/primitives/test_vertex.cpp deleted file mode 100644 index 353507175c5..00000000000 --- a/simulation/simple_sensor_simulator/test/src/primitives/test_vertex.cpp +++ /dev/null @@ -1,123 +0,0 @@ -// 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 - -#include -#include -#include -#include - -#include "../utils/expect_eq_macros.hpp" - -using namespace simple_sensor_simulator; - -/** - * @note Test basic functionality - * - * Test to vertex conversion correctness with a sample point. - */ -TEST(VertexTest, toVertex_onePoint) -{ - const auto point = geometry_msgs::build().x(1.0).y(2.0).z(3.0); - - Vertex vertex = toVertex(point); - - EXPECT_VERTEX_AND_POINT_EQ(vertex, point); -} - -/** - * @note Test basic functionality - * - * Test to vertex conversion correctness with a vector containing multiple sample points. - */ -TEST(VertexTest, toVertex_manyPoints) -{ - const std::vector points = { - geometry_msgs::build().x(1.0).y(2.0).z(3.0), - geometry_msgs::build().x(4.0).y(5.0).z(6.0), - geometry_msgs::build().x(7.0).y(8.0).z(9.0)}; - - std::vector vertices = toVertex(points); - - ASSERT_EQ(vertices.size(), points.size()); - for (size_t i = 0; i < vertices.size(); ++i) { - EXPECT_VERTEX_AND_POINT_EQ(vertices[i], points[i]); - } -} - -/** - * @note Test function behavior when an empty vector is passed - * - * The goal is to get an empty vector. - */ -TEST(VertexTest, toVertex_empty) -{ - std::vector points; - - std::vector vertices = toVertex(points); - - EXPECT_TRUE(vertices.empty()); -} - -/** - * @note Test basic functionality - * - * Test to point conversion correctness with a sample vertex. - */ -TEST(VertexTest, toPoint_oneVertex) -{ - const Vertex vertex{1.0, 2.0, 3.0}; - - geometry_msgs::msg::Point point = toPoint(vertex); - - EXPECT_VERTEX_AND_POINT_EQ(point, vertex); -} - -/** - * @note Test basic functionality - * - * Test to point conversion correctness with a vector containing multiple sample vertices. - */ -TEST(VertexTest, toPoints_manyVertices) -{ - const std::vector vertices = {{1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}, {7.0, 8.0, 9.0}}; - - std::vector points = toPoints(vertices); - - ASSERT_EQ(points.size(), vertices.size()); - for (size_t i = 0; i < points.size(); ++i) { - EXPECT_VERTEX_AND_POINT_EQ(points[i], vertices[i]); - } -} - -/** - * @note Test function behavior when an empty vector is passed - * - * The goal is to get an empty vector. - */ -TEST(VertexTest, toPoints_empty) -{ - std::vector vertices; - - std::vector points = toPoints(vertices); - - EXPECT_TRUE(points.empty()); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp index 2b3d04d0990..ad3ce51f038 100644 --- a/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp +++ b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp @@ -24,7 +24,7 @@ #include #include -#include "utils/expect_eq_macros.hpp" +#include "sensor_simulation/expect_eq_macros.hpp" auto makeInitializeRequest() -> simulation_api_schema::InitializeRequest { diff --git a/simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp b/simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp deleted file mode 100644 index c93387428cc..00000000000 --- a/simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp +++ /dev/null @@ -1,68 +0,0 @@ -// 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__UTILS__EXPECT_EQ_MACROS_HPP_ -#define SIMPLE_SENSOR_SIMULATOR__TEST__UTILS__EXPECT_EQ_MACROS_HPP_ - -#include - -#include -#include - -#define EXPECT_POINT_EQ(DATA0, DATA1) \ - EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x); \ - EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \ - EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); - -#define EXPECT_POINT_NEAR(DATA0, DATA1, TOLERANCE) \ - EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \ - EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \ - EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE); - -#define EXPECT_QUATERNION_EQ(DATA0, DATA1) \ - EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x); \ - EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \ - EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); \ - EXPECT_DOUBLE_EQ(DATA0.w, DATA1.w); - -#define EXPECT_QUATERNION_NEAR(DATA0, DATA1, TOLERANCE) \ - EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \ - EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \ - EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE); \ - EXPECT_NEAR(DATA0.w, DATA1.w, TOLERANCE); - -#define EXPECT_POSE_EQ(DATA0, DATA1) \ - EXPECT_POINT_EQ(DATA0.position, DATA1.position); \ - EXPECT_QUATERNION_EQ(DATA0.orientation, DATA1.orientation); - -#define EXPECT_POSE_NEAR(DATA0, DATA1, TOLERANCE) \ - EXPECT_POINT_NEAR(DATA0.position, DATA1.position, TOLERANCE); \ - EXPECT_QUATERNION_NEAR(DATA0.orientation, DATA1.orientation, TOLERANCE); - -#define EXPECT_VERTEX_EQ(DATA0, DATA1) \ - EXPECT_FLOAT_EQ(DATA0.x, DATA1.x); \ - EXPECT_FLOAT_EQ(DATA0.y, DATA1.y); \ - EXPECT_FLOAT_EQ(DATA0.z, DATA1.z); - -#define EXPECT_TRIANGLE_EQ(DATA0, DATA1) \ - EXPECT_EQ(DATA0.v0, DATA1.v0); \ - EXPECT_EQ(DATA0.v1, DATA1.v1); \ - EXPECT_EQ(DATA0.v2, DATA1.v2); - -#define EXPECT_VERTEX_AND_POINT_EQ(VERTEX, POINT) \ - EXPECT_FLOAT_EQ(VERTEX.x, POINT.x); \ - EXPECT_FLOAT_EQ(VERTEX.y, POINT.y); \ - EXPECT_FLOAT_EQ(VERTEX.z, POINT.z); - -#endif // SIMPLE_SENSOR_SIMULATOR__TEST__UTILS__EXPECT_EQ_MACROS_HPP_ From 2278a52c039ef27fabc4286dd55b23998ff2d309 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 18 Jun 2024 16:18:42 +0200 Subject: [PATCH 097/118] port old tests --- .../test/CMakeLists.txt | 3 + .../test_sensor_simulation.cpp | 118 ++++++++++++++++++ 2 files changed, 121 insertions(+) create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt index 892b425d41c..1f8cbf3a6ab 100644 --- a/simulation/simple_sensor_simulator/test/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/CMakeLists.txt @@ -9,3 +9,6 @@ include_directories(${Protobuf_INCLUDE_DIRS}) ament_add_gtest(test_scenario_simulator src/test_scenario_simulator.cpp) target_link_libraries(test_scenario_simulator simple_sensor_simulator_component ${Protobuf_LIBRARIES}) + +ament_add_gtest(test_sensor_simulation src/sensor_simulation/test_sensor_simulation.cpp) +target_link_libraries(test_sensor_simulation simple_sensor_simulator_component ${Protobuf_LIBRARIES}) \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp new file mode 100644 index 00000000000..c34fa998db0 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp @@ -0,0 +1,118 @@ +// 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 + +#include + +/** + * @note Test function behavior when called with a configuration architecture type other than + * "awf/universe" - the goal is to test error throwing. + */ +TEST(SensorSimulation, attachLidarSensor_wrongArchitecture) +{ + auto configuration = simulation_api_schema::LidarConfiguration{}; + configuration.set_architecture_type("wrong/architecture"); + + auto node = rclcpp::Node{"name", rclcpp::NodeOptions{}}; + + EXPECT_THROW( + simple_sensor_simulator::SensorSimulation().attachLidarSensor(0.0, configuration, node), + std::runtime_error); +} + +/** + * @note Test basic functionality. Test attaching a lidar sensor correctness with a sample lidar configuration. + */ +TEST(SensorSimulation, attachLidarSensor_correctConfiguration) +{ + auto configuration = simulation_api_schema::LidarConfiguration{}; + configuration.set_architecture_type("awf/universe"); + + auto node = rclcpp::Node{"name", rclcpp::NodeOptions{}}; + + EXPECT_NO_THROW( + simple_sensor_simulator::SensorSimulation().attachLidarSensor(0.0, configuration, node)); +} + +/** + * @note Test function behavior when called with a configuration architecture type other than + * "awf/universe" - the goal is to test error throwing. + */ +TEST(SensorSimulation, attachDetectionSensor_wrongArchitecture) +{ + auto configuration = simulation_api_schema::DetectionSensorConfiguration{}; + configuration.set_architecture_type("wrong/architecture"); + + auto node = rclcpp::Node{"name", rclcpp::NodeOptions{}}; + + EXPECT_THROW( + simple_sensor_simulator::SensorSimulation().attachDetectionSensor(0.0, configuration, node), + std::runtime_error); +} + +/** + * @note Test basic functionality. Test attaching a detection sensor correctness + * with a sample detection sensor configuration. + */ +TEST(SensorSimulation, attachDetectionSensor_correctConfiguration) +{ + auto configuration = simulation_api_schema::DetectionSensorConfiguration{}; + configuration.set_architecture_type("awf/universe"); + + auto node = rclcpp::Node{"name", rclcpp::NodeOptions{}}; + + EXPECT_NO_THROW( + simple_sensor_simulator::SensorSimulation().attachDetectionSensor(0.0, configuration, node)); +} + +/** + * @note Test function behavior when called with a configuration architecture type other than + * "awf/universe" - the goal is to test error throwing. + */ +TEST(SensorSimulation, attachOccupancyGridSensor_wrongArchitecture) +{ + auto configuration = simulation_api_schema::OccupancyGridSensorConfiguration{}; + configuration.set_architecture_type("wrong/architecture"); + + auto node = rclcpp::Node{"name", rclcpp::NodeOptions{}}; + + EXPECT_THROW( + simple_sensor_simulator::SensorSimulation().attachOccupancyGridSensor(0.0, configuration, node), + std::runtime_error); +} + +/** + * @note Test basic functionality. Test attaching a occupancy grid sensor correctness + * with a sample occupancy grid sensor configuration. + */ +TEST(SensorSimulation, attachOccupancyGridSensor_correctConfiguration) +{ + auto configuration = simulation_api_schema::OccupancyGridSensorConfiguration{}; + configuration.set_architecture_type("awf/universe"); + + auto node = rclcpp::Node{"name", rclcpp::NodeOptions{}}; + + EXPECT_NO_THROW(simple_sensor_simulator::SensorSimulation().attachOccupancyGridSensor( + 0.0, configuration, node)); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(0, nullptr); + testing::InitGoogleTest(&argc, argv); + const int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} From 2a0cb5aa63e5a7c6a8a9de5e389d8631dfd3f2aa Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Tue, 18 Jun 2024 16:28:26 +0200 Subject: [PATCH 098/118] Convert parameterHandler class to free function getParameter used with parameters interface Signed-off-by: Mateusz Palczuk --- .../include/traffic_simulator/api/api.hpp | 15 +++--- .../traffic_simulator/entity/ego_entity.hpp | 6 +-- .../entity/entity_manager.hpp | 12 ++--- .../utils/node_parameter_handler.hpp | 54 ------------------- .../utils/node_parameters.hpp | 42 +++++++++++++++ .../src/entity/ego_entity.cpp | 22 ++++---- 6 files changed, 69 insertions(+), 82 deletions(-) delete mode 100644 simulation/traffic_simulator/include/traffic_simulator/utils/node_parameter_handler.hpp create mode 100644 simulation/traffic_simulator/include/traffic_simulator/utils/node_parameters.hpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 1bf4f3e33be..b6e0747389d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -65,9 +65,10 @@ class API template , typename... Ts> explicit API(NodeT && node, const Configuration & configuration, Ts &&... xs) : configuration(configuration), - node_parameter_handler_(std::make_shared(node)), + node_parameters_( + rclcpp::node_interfaces::get_node_parameters_interface(std::forward(node))), entity_manager_ptr_( - std::make_shared(node, configuration, node_parameter_handler_)), + std::make_shared(node, configuration, node_parameters_)), traffic_controller_ptr_(std::make_shared( entity_manager_ptr_->getHdmapUtils(), [this]() { return API::getEntityNames(); }, [this](const auto & name) { return API::getMapPose(name); }, @@ -133,7 +134,7 @@ class API if (behavior == VehicleBehavior::autoware()) { return entity_manager_ptr_->entityExists(name) or entity_manager_ptr_->spawnEntity( - name, pose, parameters, configuration, node_parameter_handler_); + name, pose, parameters, configuration, node_parameters_); } else { return entity_manager_ptr_->spawnEntity( name, pose, parameters, behavior); @@ -388,10 +389,10 @@ class API public: #undef FORWARD_TO_ENTITY_MANAGER - template - auto getROS2Parameter(const std::string & name, Ts &&... xs) const -> decltype(auto) + template + auto getROS2Parameter(Ts &&... xs) const -> decltype(auto) { - return node_parameter_handler_->getParameter(name, std::forward(xs)...); + return getParameter(node_parameters_, std::forward(xs)...); } auto canonicalize(const LaneletPose & maybe_non_canonicalized_lanelet_pose) const @@ -411,7 +412,7 @@ class API const Configuration configuration; - const std::shared_ptr node_parameter_handler_; + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; const std::shared_ptr entity_manager_ptr_; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index 1f5998c2ea8..a31b0b47ab4 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include #include @@ -38,7 +38,7 @@ class EgoEntity : public VehicleEntity const std::unique_ptr field_operator_application; static auto makeFieldOperatorApplication( - const Configuration &, const std::shared_ptr &) + const Configuration &, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &) -> std::unique_ptr; bool is_controlled_by_simulator_{false}; @@ -53,7 +53,7 @@ class EgoEntity : public VehicleEntity const std::string & name, const CanonicalizedEntityStatus &, const std::shared_ptr &, const traffic_simulator_msgs::msg::VehicleParameters &, const Configuration &, - const std::shared_ptr &); + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &); explicit EgoEntity(EgoEntity &&) = delete; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index bd08db62729..0662052f101 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include #include @@ -74,7 +74,7 @@ class EntityManager Configuration configuration; std::shared_ptr node_topics_interface; - const std::shared_ptr node_parameter_handler_; + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; tf2_ros::StaticTransformBroadcaster broadcaster_; tf2_ros::TransformBroadcaster base_link_broadcaster_; @@ -133,7 +133,7 @@ class EntityManager auto makeV2ITrafficLightPublisher(Ts &&... xs) -> std::shared_ptr { if (const auto architecture_type = - node_parameter_handler_->getParameter("architecture_type", "awf/universe"); + getParameter(node_parameters_, "architecture_type", "awf/universe"); architecture_type.find("awf/universe") != std::string::npos) { return std::make_shared< TrafficLightPublisher>( @@ -148,10 +148,10 @@ class EntityManager template > explicit EntityManager( NodeT && node, const Configuration & configuration, - const std::shared_ptr & node_parameter_handler) + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters) : configuration(configuration), node_topics_interface(rclcpp::node_interfaces::get_node_topics_interface(node)), - node_parameter_handler_(node_parameter_handler), + node_parameters_(node_parameters), broadcaster_(node), base_link_broadcaster_(node), clock_ptr_(node->get_clock()), @@ -499,7 +499,7 @@ class EntityManager entity_status.lanelet_pose = *lanelet_pose; entity_status.lanelet_pose_valid = true; /// @note fix z, roll and pitch to fitting to the lanelet - if (node_parameter_handler_->getParameter("consider_pose_by_road_slope", false)) { + if (getParameter(node_parameters_, "consider_pose_by_road_slope", false)) { entity_status.pose = hdmap_utils_ptr_->toMapPose(*lanelet_pose).pose; } else { entity_status.pose = pose; diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameter_handler.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameter_handler.hpp deleted file mode 100644 index 9cbf2f11722..00000000000 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameter_handler.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2024 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 TRAFFIC_SIMULATOR__UTILS__NODE_PARAMETER_HANDLER_HPP_ -#define TRAFFIC_SIMULATOR__UTILS__NODE_PARAMETER_HANDLER_HPP_ - -#include -#include - -namespace traffic_simulator -{ -class NodeParameterHandler -{ -public: - template - explicit NodeParameterHandler(NodeT && node) - : node_parameters_interface_( - rclcpp::node_interfaces::get_node_parameters_interface(std::forward(node))) - { - } - - /** - * Get parameter or declare it if it has not been declared before. Declare it with a default value. - * @param name The name of the parameter - * @param default_value The default value of the parameter - * @return The value of the parameter - */ - template - auto getParameter(const std::string & name, const ParameterT & default_value = {}) const - -> ParameterT - { - if (not node_parameters_interface_->has_parameter(name)) { - node_parameters_interface_->declare_parameter(name, rclcpp::ParameterValue(default_value)); - } - return node_parameters_interface_->get_parameter(name).get_value(); - } - -private: - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface_; -}; -} // namespace traffic_simulator - -#endif //TRAFFIC_SIMULATOR__UTILS__NODE_PARAMETER_HANDLER_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameters.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameters.hpp new file mode 100644 index 00000000000..5114d9997af --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameters.hpp @@ -0,0 +1,42 @@ +// Copyright 2024 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 TRAFFIC_SIMULATOR__UTILS__NODE_PARAMETERS_HPP_ +#define TRAFFIC_SIMULATOR__UTILS__NODE_PARAMETERS_HPP_ + +#include +#include + +namespace traffic_simulator +{ +/** + * Get parameter or declare it if it has not been declared before. Declare it with a default value. + * @param node_parameters The node parameters interface pointer, it will be used to access the parameters + * @param name The name of the parameter + * @param default_value The default value of the parameter + * @return The value of the parameter + */ +template +auto getParameter( + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + const std::string & name, const ParameterT & default_value = {}) -> ParameterT +{ + if (not node_parameters->has_parameter(name)) { + node_parameters->declare_parameter(name, rclcpp::ParameterValue(default_value)); + } + return node_parameters->get_parameter(name).get_value(); +} +} // namespace traffic_simulator + +#endif //TRAFFIC_SIMULATOR__UTILS__NODE_PARAMETERS_HPP_ diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 42f26e3b5eb..ec548631114 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -34,25 +34,23 @@ namespace entity { auto EgoEntity::makeFieldOperatorApplication( const Configuration & configuration, - const std::shared_ptr & node_parameter_handler) + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters) -> std::unique_ptr { if (const auto architecture_type = - node_parameter_handler->getParameter("architecture_type", "awf/universe"); + getParameter(node_parameters, "architecture_type", "awf/universe"); architecture_type.find("awf/universe") != std::string::npos) { - std::string rviz_config = node_parameter_handler->getParameter("rviz_config", ""); - return node_parameter_handler->getParameter("launch_autoware", true) + std::string rviz_config = getParameter(node_parameters, "rviz_config", ""); + return getParameter(node_parameters, "launch_autoware", true) ? std::make_unique< concealer::FieldOperatorApplicationFor>( - node_parameter_handler->getParameter("autoware_launch_package"), - node_parameter_handler->getParameter("autoware_launch_file"), + getParameter(node_parameters, "autoware_launch_package"), + getParameter(node_parameters, "autoware_launch_file"), "map_path:=" + configuration.map_path.string(), "lanelet2_map_file:=" + configuration.getLanelet2MapFile(), "pointcloud_map_file:=" + configuration.getPointCloudMapFile(), - "sensor_model:=" + - node_parameter_handler->getParameter("sensor_model"), - "vehicle_model:=" + - node_parameter_handler->getParameter("vehicle_model"), + "sensor_model:=" + getParameter(node_parameters, "sensor_model"), + "vehicle_model:=" + getParameter(node_parameters, "vehicle_model"), "rviz_config:=" + ((rviz_config == "") ? configuration.rviz_config_path.string() : Configuration::Pathname(rviz_config).string()), @@ -72,9 +70,9 @@ EgoEntity::EgoEntity( const std::shared_ptr & hdmap_utils_ptr, const traffic_simulator_msgs::msg::VehicleParameters & parameters, const Configuration & configuration, - const std::shared_ptr & node_parameter_handler) + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters) : VehicleEntity(name, entity_status, hdmap_utils_ptr, parameters), - field_operator_application(makeFieldOperatorApplication(configuration, node_parameter_handler)) + field_operator_application(makeFieldOperatorApplication(configuration, node_parameters)) { } From afeffb5cf26941404ff79bd6d5d9b2c86432a78f Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Tue, 18 Jun 2024 16:58:12 +0200 Subject: [PATCH 099/118] test: [RJD-937] to Implement Unit tests on simple_sensor_simulator - Fixed an issue with missed header file --- simulation/simple_sensor_simulator/test/CMakeLists.txt | 2 -- .../test/src/sensor_simulation/primitives/test_box.cpp | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt index 16f47547ef3..e7f6401dfef 100644 --- a/simulation/simple_sensor_simulator/test/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/CMakeLists.txt @@ -1,5 +1,3 @@ -find_package(quaternion_operation REQUIRED) - add_subdirectory(src/sensor_simulation/lidar) add_subdirectory(src/sensor_simulation/primitives) add_subdirectory(src/sensor_simulation/occupancy_grid) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp index cd243afa1be..c5b501075d7 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp @@ -13,9 +13,9 @@ // limitations under the License. #include -#include #include +#include #include #include From bf6a962e14e3e85627fca226574120cdba30080e Mon Sep 17 00:00:00 2001 From: Release Bot Date: Wed, 19 Jun 2024 07:30:39 +0000 Subject: [PATCH 100/118] Bump version of scenario_simulator_v2 from version 2.1.6 to version 2.1.7 --- common/math/arithmetic/CHANGELOG.rst | 6 +++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 6 +++++ common/math/geometry/package.xml | 2 +- .../CHANGELOG.rst | 6 +++++ .../scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 6 +++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 6 +++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 6 +++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 6 +++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 6 +++++ map/kashiwanoha_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 13 +++++++++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../CHANGELOG.rst | 6 +++++ .../package.xml | 2 +- .../openscenario_interpreter/CHANGELOG.rst | 13 +++++++++++ .../openscenario_interpreter/package.xml | 2 +- .../CHANGELOG.rst | 6 +++++ .../package.xml | 2 +- .../CHANGELOG.rst | 6 +++++ .../openscenario_interpreter_msgs/package.xml | 2 +- .../openscenario_preprocessor/CHANGELOG.rst | 6 +++++ .../openscenario_preprocessor/package.xml | 2 +- .../CHANGELOG.rst | 6 +++++ .../package.xml | 2 +- .../openscenario_utility/CHANGELOG.rst | 6 +++++ openscenario/openscenario_utility/package.xml | 2 +- .../openscenario_validator/CHANGELOG.rst | 6 +++++ .../openscenario_validator/package.xml | 2 +- .../openscenario_visualization/CHANGELOG.rst | 6 +++++ .../openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 6 +++++ .../package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 6 +++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 6 +++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 6 +++++ simulation/do_nothing_plugin/package.xml | 2 +- .../simple_sensor_simulator/CHANGELOG.rst | 6 +++++ .../simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 6 +++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 23 +++++++++++++++++++ simulation/traffic_simulator/package.xml | 2 +- .../traffic_simulator_msgs/CHANGELOG.rst | 6 +++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 6 +++++ test_runner/random_test_runner/package.xml | 2 +- .../scenario_test_runner/CHANGELOG.rst | 6 +++++ test_runner/scenario_test_runner/package.xml | 2 +- 56 files changed, 227 insertions(+), 28 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index fe6e4fc23ab..9deea1f5a57 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package arithmetic * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 743117634cc..cbda1e06db6 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 2.1.6 + 2.1.7 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index b4ad57fd22c..2fef972efdf 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package geometry * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index f9275a2d653..33a53ffebe2 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 2.1.6 + 2.1.7 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 13010d66dcc..e43733599dd 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package scenario_simulator_exception * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 3bad578cbf4..d775df3d2ac 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 2.1.6 + 2.1.7 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index 018674606a6..72d792d3e3e 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package junit_exporter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 9f0d73e05ce..d26a4845977 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 2.1.6 + 2.1.7 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index b641784f65b..fabf2422b54 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package status_monitor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 4b4ab3c172a..2c5a649c247 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 2.1.6 + 2.1.7 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 679f76600c6..ffe281bd402 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package concealer * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 52060c1ee4b..df4a0d58c47 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 2.1.6 + 2.1.7 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 2f873ad07c3..c5635764d39 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -16,6 +16,12 @@ Changelog for package embree_vendor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 2e68b1959b8..c83e13bf2df 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 2.1.6 + 2.1.7 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index bab69cdd6a6..f1254f86ca8 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package kashiwanoha_map * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index de166898365..3aa114f33f0 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 2.1.6 + 2.1.7 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index 46acd42b47f..45f5ed9e69d 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -13,6 +13,19 @@ Changelog for package cpp_mock_scenarios * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge pull request `#1275 `_ from tier4/feature/improve-ros-parameter-handling + Feature: improve ROS parameter handling +* getParameter -> getROS2Parameter +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Revert changes adding parameter checking + After thic change the code is functionally the same as in the beginning +* Merge branch 'master' into feature/improve-ros-parameter-handling +* ref(ParameterManager): rename to NodeParameterHandler, improve +* Apply API getParameter function where possible +* Contributors: Dawid Moszynski, Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 2e6bd56cd94..d2de246fa95 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 2.1.6 + 2.1.7 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 8a20eddbd4f..7d732774ad2 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_experimental_catalog * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 92ea64d2fdb..d7d40291637 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 2.1.6 + 2.1.7 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 71c2330ead3..a2c94658b4a 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -14,6 +14,19 @@ Changelog for package openscenario_interpreter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge pull request `#1275 `_ from tier4/feature/improve-ros-parameter-handling + Feature: improve ROS parameter handling +* getParameter -> getROS2Parameter +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Revert changes adding parameter checking + After thic change the code is functionally the same as in the beginning +* Merge branch 'master' into feature/improve-ros-parameter-handling +* ref(ParameterManager): rename to NodeParameterHandler, improve +* Apply API getParameter function where possible +* Contributors: Dawid Moszynski, Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 36c0ba54bbb..23c2b6d72de 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 2.1.6 + 2.1.7 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 08c14706900..3c7585bfd04 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_interpreter_example * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 3bc024b2c08..6e1d60ff3d2 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 2.1.6 + 2.1.7 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index bbd47166497..722d6c52414 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_interpreter_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 0991b966377..76fd9b2644a 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 2.1.6 + 2.1.7 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index c22f3f24c90..642beaae459 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_preprocessor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 5b5d5fb9979..606dcebb3e4 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 2.1.6 + 2.1.7 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 2507a9b8660..c4036e19a10 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_preprocessor_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index f0898209dcb..a055a7e2601 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 2.1.6 + 2.1.7 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 187c3ee6451..23411560857 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -16,6 +16,12 @@ Changelog for package openscenario_utility * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 33ca0f13fb2..048acf3f72f 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 2.1.6 + 2.1.7 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index f14553dfa30..34529344df9 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package openscenario_validator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 9ddafb4f4f0..45e9b50e53d 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 2.1.6 + 2.1.7 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 5543ce346ae..68eaf3457a1 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_visualization * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 6d1b47cf762..cde6936cff2 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 2.1.6 + 2.1.7 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index d9383eb68c4..e72288b9ead 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index 8bc3a9bde2a..b6914388dde 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 2.1.6 + 2.1.7 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 437e937e422..2cc48bef587 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package scenario_simulator_v2 * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 9f5c0aa94bf..1ab02509b02 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 2.1.6 + 2.1.7 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index fbb093871c6..f123b83193b 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package behavior_tree_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index bed1468bbda..14d4c9970e8 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 2.1.6 + 2.1.7 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 3c21518202f..86dee7ed816 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package do_nothing_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index e8dda243cfb..2cc5655e0a1 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 2.1.6 + 2.1.7 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index dab533b60bc..8bf43b48c9b 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package simple_sensor_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 3bff2ed2e31..b6231e2cb71 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 2.1.6 + 2.1.7 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index e530849b7d7..566d4755bc2 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package simulation_interface * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 7cab5a458e3..30292fa588f 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 2.1.6 + 2.1.7 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index 6e1dd7c6247..2252b2710d5 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -13,6 +13,29 @@ Changelog for package traffic_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge pull request `#1275 `_ from tier4/feature/improve-ros-parameter-handling + Feature: improve ROS parameter handling +* Convert parameterHandler class to free function getParameter used with parameters interface +* getParameter -> getROS2Parameter +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Revert changes adding parameter checking + After thic change the code is functionally the same as in the beginning +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Declare parameters before getting values +* ref(ParameterManager): rename to NodeParameterHandler, improve +* Make ParameterManager explicit +* Remove getParameter from EntityManager +* Remove getParameter from EgoEntity +* Use ParameterManager in EgoEntity +* Use const for getParameter +* Add getParameter forwarding in API +* Add ParameterManager to API +* Move ParameterManager +* Prototype ParameterManager +* Contributors: Dawid Moszynski, Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ * Merge pull request `#1289 `_ from tier4/revert-1284-fix/issue1276 diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 2279d7ea6fe..826d273d550 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 2.1.6 + 2.1.7 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index f78d02ada1f..418fbb74852 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 503165019e7..2d17165f834 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 2.1.6 + 2.1.7 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 41cd8bc49c4..531a5dd17af 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package random_test_runner * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index d9a662a6493..ee139252a6c 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 2.1.6 + 2.1.7 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 16e0486d814..3ea78095fda 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -23,6 +23,12 @@ Changelog for package scenario_test_runner * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.7 (2024-06-19) +------------------ +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Merge branch 'master' into feature/improve-ros-parameter-handling +* Contributors: Masaya Kataoka, Mateusz Palczuk + 2.1.6 (2024-06-18) ------------------ diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index f5255866d4e..1bf1f8ed87b 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 2.1.6 + 2.1.7 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 2c3aa1440a55d7a5424d98277e57cc5a591fc454 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 19 Jun 2024 17:46:10 +0200 Subject: [PATCH 101/118] add tests --- .../occupancy_grid/CMakeLists.txt | 6 + .../test_occupancy_grid_builder.cpp | 225 ++++++++++++++++++ .../test_occupancy_grid_sensor.cpp | 24 ++ .../test/src/test_scenario_simulator.cpp | 103 +++++++- 4 files changed, 357 insertions(+), 1 deletion(-) create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt index bbd7ab28815..cf2ed3de1e2 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt @@ -1,2 +1,8 @@ ament_add_gtest(test_grid_traversal test_grid_traversal.cpp) target_link_libraries(test_grid_traversal simple_sensor_simulator_component) + +ament_add_gtest(test_occupancy_grid_builder test_occupancy_grid_builder.cpp) +target_link_libraries(test_occupancy_grid_builder simple_sensor_simulator_component) + +ament_add_gtest(test_occupancy_grid_sensor test_occupancy_grid_sensor.cpp) +target_link_libraries(test_occupancy_grid_sensor simple_sensor_simulator_component) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp new file mode 100644 index 00000000000..1b94d0a44cb --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp @@ -0,0 +1,225 @@ +// 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 + +#include +#include +#include +#include +#include + +auto printGrid(const simple_sensor_simulator::OccupancyGridBuilder & builder) -> void +{ + const auto & grid = builder.get(); + for (size_t i = static_cast(0); i < builder.height; ++i) { + for (size_t j = static_cast(0); j < builder.width; ++j) { + std::cout << static_cast(grid.at(i * builder.width + j)) << ", "; + } + std::cout << std::endl; + } +} + +auto makePose(const double x, const double y) -> geometry_msgs::msg::Pose +{ + return geometry_msgs::build() + .position(geometry_msgs::build().x(x).y(y).z(0.0)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0)); +} + +auto makeBox(const float size, const double x, const double y) + -> simple_sensor_simulator::primitives::Box +{ + return simple_sensor_simulator::primitives::Box(size, size, size, makePose(x, y)); +} + +/** + * @note Test function behavior when called after the limit has been reached. + * The number of elements added is equal to int16_t max. + */ +TEST(OccupancyGridBuilder, add_overLimit) +{ + auto builder = simple_sensor_simulator::OccupancyGridBuilder( + 0.1, static_cast(20), static_cast(40)); + for (int i = 0; i < static_cast(std::numeric_limits::max()); ++i) { + builder.add(makeBox(5.0f, 0.0, 0.0)); + } + EXPECT_THROW(builder.add(makeBox(5.0f, 0.0, 0.0)), std::runtime_error); +} + +/** + * @note Test basic functionality. Test adding a primitive with a sample primitive (e.g. box) + * positioned fully inside the grid, but not in the center. + */ +TEST(OccupancyGridBuilder, add_primitiveInside) +{ + auto builder = simple_sensor_simulator::OccupancyGridBuilder( + 1.0, static_cast(10), static_cast(20), static_cast(2), + static_cast(1)); + builder.add(makeBox(5.0f, 4.0, 0.0)); + builder.add(makeBox(2.0f, -5.0, 0.0)); + builder.build(); + const auto & result_obtained = builder.get(); + const auto result_expected = std::vector{ + // clang-format off + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 2, 2, 2, 2, 2, 2, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 2, 2, 2, 2, 2, 2, 1, 1, 1, + 1, 1, 1, 1, 2, 2, 2, 0, 0, 0, 0, 2, 2, 2, 2, 2, 2, 1, 1, 1, + 1, 1, 1, 1, 2, 2, 2, 0, 0, 0, 0, 2, 2, 2, 2, 2, 2, 1, 1, 1, + 1, 1, 1, 1, 2, 2, 2, 0, 0, 0, 0, 2, 2, 2, 2, 2, 2, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 2, 2, 2, 2, 2, 2, 1, 1, 1, + 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, + // clang-format on + }; + EXPECT_EQ(result_expected, result_obtained); +} + +/** + * @note Test basic functionality. Test adding a primitive with a sample primitive (e.g. box) + * positioned in such a way that it protrudes from the grid area. + */ +TEST(OccupancyGridBuilder, add_primitiveProtruding) +{ + auto builder = simple_sensor_simulator::OccupancyGridBuilder( + 1.0, static_cast(10), static_cast(20), static_cast(2), + static_cast(1)); + builder.add(makeBox(5.0f, 9.0, -4.0)); + builder.add(makeBox(2.0f, -10.0, 3.0)); + builder.build(); + const auto & result_obtained = builder.get(); + const auto result_expected = std::vector{ + // clang-format off + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 2, 2, 2, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 2, 2, 2, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 2, 2, 2, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 2, 2, 2, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 2, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 2, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 2, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + // clang-format on + }; + EXPECT_EQ(result_expected, result_obtained); +} + +/** + * @note Test basic functionality. Test adding a primitive with a sample primitive (e.g. box) + * covering the center of the grid - the goal is to get whole grid as invisible. + */ +TEST(OccupancyGridBuilder, add_primitiveInCenter) +{ + auto builder = simple_sensor_simulator::OccupancyGridBuilder( + 1.0, static_cast(10), static_cast(20), static_cast(2), + static_cast(1)); + builder.add(makeBox(5.0f, 0.0, 0.0)); + builder.build(); + const auto & result_obtained = builder.get(); + const auto result_expected = std::vector{ + // clang-format off + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + // clang-format on + }; + EXPECT_EQ(result_expected, result_obtained); +} + +/** + * @note Test basic functionality. Test adding a primitive with a sample primitive (e.g. box) + * covering the center of the grid - the goal is to get whole grid as invisible. + */ +TEST(OccupancyGridBuilder, reset) +{ + auto builder = simple_sensor_simulator::OccupancyGridBuilder( + 1.0, static_cast(10), static_cast(20), static_cast(2), + static_cast(1)); + builder.add(makeBox(5.0f, 0.0, 0.0)); + builder.reset(makePose(0.0, 0.0)); + builder.build(); + const auto & result_obtained = builder.get(); + const auto result_expected = std::vector{ + // clang-format off + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + // clang-format on + }; + EXPECT_EQ(result_expected, result_obtained); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +/* +Methods .add(), .build(), .get() will inherently interleave each other. +Can't test them separately. + + +Why does only the method .reset(POSE) have an ability to change the pose? +It seems weird. + + +WEIRD ZEROS ON THE RIGHT +builder.add(makeBox(5.0f, 0.0, 0.0)); + +1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 +1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 +1 1 1 1 1 1 1 2 2 2 2 2 2 1 1 0 0 0 0 0 +1 1 1 1 1 1 1 2 2 2 2 2 2 1 0 0 0 0 0 0 +1 1 1 1 1 1 1 2 2 2 2 2 2 0 0 0 0 0 0 0 +1 1 1 1 1 1 1 2 2 2 2 2 2 0 0 0 0 0 0 0 +1 1 1 1 1 1 1 2 2 2 2 2 2 0 0 0 0 0 0 0 +1 1 1 1 1 1 1 2 2 2 2 2 2 0 0 0 0 0 0 0 +1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 +1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 + + +WEIRD ONE IN THE MIDDLE +builder.add(makeBox(5.0f, 4.0, 0.0)); +builder.add(makeBox(2.0f, -5.0, 0.0)); + +0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 +0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 +1 1 1 0 0 0 0 0 0 0 0 2 2 2 2 2 2 1 1 1 +1 1 1 1 1 1 1 1 0 0 0 2 2 2 2 2 2 1 1 1 +1 1 1 1 2 2 2 0 0 0 0 2 2 2 2 2 2 1 1 1 +1 1 1 1 2 2 2 0 0 0 0 2 2 2 2 2 2 1 1 1 +1 1 1 1 2 2 2 0 0 0 0 2 2 2 2 2 2 1 1 1 +1 1 1 1 1 1 1 0 0 0 0 2 2 2 2 2 2 1 1 1 +1 1 1 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 +0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 +*/ \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp new file mode 100644 index 00000000000..2f180c0e0a2 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp @@ -0,0 +1,24 @@ +// 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 + +#include +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp index ad3ce51f038..72d91df4bd4 100644 --- a/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp +++ b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -37,7 +38,8 @@ auto makeInitializeRequest() -> simulation_api_schema::InitializeRequest auto makeUpdateFrameRequest() -> simulation_api_schema::UpdateFrameRequest { auto request = simulation_api_schema::UpdateFrameRequest(); - // to be filled up + request.set_current_scenario_time(1.0); + request.set_current_simulation_time(1.0); return request; } @@ -80,6 +82,39 @@ auto makeSpawnMiscObjectEntityRequest(const std::string name = "name") return request; } +auto makeAttachDetectionSensorRequest() -> simulation_api_schema::AttachDetectionSensorRequest +{ + auto request = simulation_api_schema::AttachDetectionSensorRequest(); + auto configuration = traffic_simulator::helper::constructDetectionSensorConfiguration( + "entity_name", "awf/universe", 1.0); + *request.mutable_configuration() = configuration; + return request; +} + +auto makeAttachLidarSensorRequest() -> simulation_api_schema::AttachLidarSensorRequest +{ + auto request = simulation_api_schema::AttachLidarSensorRequest(); + auto configuration = traffic_simulator::helper::constructLidarConfiguration( + traffic_simulator::helper::LidarType::VLP16, "entity_name", "awf/universe", 1.0); + *request.mutable_configuration() = configuration; + return request; +} + +auto makeAttachOccupancyGridSensorRequest() + -> simulation_api_schema::AttachOccupancyGridSensorRequest +{ + auto request = simulation_api_schema::AttachOccupancyGridSensorRequest(); + auto configuration = simulation_api_schema::OccupancyGridSensorConfiguration(); + + configuration.set_entity("entity_name"); + configuration.set_architecture_type("awf/universe"); + configuration.set_update_duration(1.0); + configuration.set_range(300.0); + configuration.set_resolution(1.0); + *request.mutable_configuration() = configuration; + return request; +} + /** * @note Test initialization correctness with a sample request with the default port (5555). */ @@ -348,6 +383,72 @@ TEST(ScenarioSimulator, despawnEntity_invalidName) server.join(); } +/** + * @note Test attaching detection sensor with a request to attach a sensor with a valid configuration. + */ +TEST(ScenarioSimulator, attachDetectionSensor) +{ + auto server = std::thread([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + }); + + auto multi_client = + zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); + + EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(multi_client.call(makeAttachDetectionSensorRequest()).result().success()); + + rclcpp::shutdown(); + server.join(); +} + +/** + * @note Test attaching lidar sensor with a request to attach a sensor with a valid configuration. + */ +TEST(ScenarioSimulator, attachLidarSensor) +{ + auto server = std::thread([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + }); + + auto multi_client = + zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); + + EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(multi_client.call(makeAttachLidarSensorRequest()).result().success()); + + rclcpp::shutdown(); + server.join(); +} + +/** + * @note Test attaching occupancy grid sensor with a request to attach a sensor with a valid configuration. + */ +TEST(ScenarioSimulator, attachOccupancyGridSensor) +{ + auto server = std::thread([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + }); + + auto multi_client = + zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); + + EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(multi_client.call(makeAttachOccupancyGridSensorRequest()).result().success()); + + rclcpp::shutdown(); + server.join(); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 1ee4828af70b68a2f45fc62dd92426a581cc813d Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Thu, 20 Jun 2024 17:39:27 +0900 Subject: [PATCH 102/118] Optimize entity frame computation. --- .../src/entity/entity_manager.cpp | 31 +++++++++++++------ 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 571adc2dedf..b25b03d62e4 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -39,6 +39,9 @@ namespace entity { void EntityManager::broadcastEntityTransform() { + static bool is_send = false; + static geometry_msgs::msg::Pose pose; + using math::geometry::operator/; using math::geometry::operator*; using math::geometry::operator+=; @@ -52,31 +55,39 @@ void EntityManager::broadcastEntityTransform() if (isEgoSpawned()) { const auto ego_name = getEgoName(); if (entityExists(ego_name)) { + if (!is_send) { + pose = getMapPose(ego_name); + is_send = true; + } broadcastTransform( geometry_msgs::build() /** * @note This is the intended implementation. * It is easier to create rviz config if the name "ego" is fixed, - * so the frame_id “ego” is issued regardless of the name of the ego entity. + * so the frame_id "ego" is issued regardless of the name of the ego entity. */ .header(std_msgs::build().stamp(clock_ptr_->now()).frame_id("ego")) - .pose(getMapPose(ego_name)), + .pose(pose), true); } } if (!names.empty()) { + if (!is_send) { + pose = geometry_msgs::build() + .position(std::accumulate( + names.begin(), names.end(), geometry_msgs::msg::Point(), + [this, names](geometry_msgs::msg::Point point, const std::string & name) { + point += getMapPose(name).position * (1.0 / static_cast(names.size())); + return point; + })) + .orientation(geometry_msgs::msg::Quaternion()); + is_send = true; + } broadcastTransform( geometry_msgs::build() .header( std_msgs::build().stamp(clock_ptr_->now()).frame_id("entities")) - .pose(geometry_msgs::build() - .position(std::accumulate( - names.begin(), names.end(), geometry_msgs::msg::Point(), - [this, names](geometry_msgs::msg::Point point, const std::string & name) { - point += getMapPose(name).position * (1.0 / static_cast(names.size())); - return point; - })) - .orientation(geometry_msgs::msg::Quaternion())), + .pose(pose), true); } } From 2aae71ba748811da6b0f4a4ae397e346af058a57 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Thu, 20 Jun 2024 10:47:38 +0200 Subject: [PATCH 103/118] Moved ament_cmake_gtest to package.xml --- simulation/simple_sensor_simulator/CMakeLists.txt | 1 - simulation/simple_sensor_simulator/package.xml | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt index 692063321aa..4e3d828ec97 100644 --- a/simulation/simple_sensor_simulator/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/CMakeLists.txt @@ -84,7 +84,6 @@ EXPORT export_simple_sensor_simulator_component if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - find_package(ament_cmake_gtest REQUIRED) ament_lint_auto_find_test_dependencies() add_subdirectory(test) diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index b6231e2cb71..dff923ae37b 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -57,6 +57,7 @@ ament_cmake_lint_cmake ament_cmake_pep257 ament_cmake_xmllint + ament_cmake_gtest From 1eb6144b66209937e90197a0b206438fcee0d368 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 20 Jun 2024 11:01:46 +0200 Subject: [PATCH 104/118] use test fixture --- .../test_sensor_simulation.cpp | 31 +-- .../test/src/test_scenario_simulator.cpp | 239 +++++------------- 2 files changed, 75 insertions(+), 195 deletions(-) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp index c34fa998db0..dbc905f15ef 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/test_sensor_simulation.cpp @@ -16,17 +16,22 @@ #include +class SensorSimulationTest : public testing::Test +{ +protected: + SensorSimulationTest() : node(rclcpp::Node{"name", rclcpp::NodeOptions{}}) {} + rclcpp::Node node; +}; + /** * @note Test function behavior when called with a configuration architecture type other than * "awf/universe" - the goal is to test error throwing. */ -TEST(SensorSimulation, attachLidarSensor_wrongArchitecture) +TEST_F(SensorSimulationTest, attachLidarSensor_wrongArchitecture) { auto configuration = simulation_api_schema::LidarConfiguration{}; configuration.set_architecture_type("wrong/architecture"); - auto node = rclcpp::Node{"name", rclcpp::NodeOptions{}}; - EXPECT_THROW( simple_sensor_simulator::SensorSimulation().attachLidarSensor(0.0, configuration, node), std::runtime_error); @@ -35,13 +40,11 @@ TEST(SensorSimulation, attachLidarSensor_wrongArchitecture) /** * @note Test basic functionality. Test attaching a lidar sensor correctness with a sample lidar configuration. */ -TEST(SensorSimulation, attachLidarSensor_correctConfiguration) +TEST_F(SensorSimulationTest, attachLidarSensor_correctConfiguration) { auto configuration = simulation_api_schema::LidarConfiguration{}; configuration.set_architecture_type("awf/universe"); - auto node = rclcpp::Node{"name", rclcpp::NodeOptions{}}; - EXPECT_NO_THROW( simple_sensor_simulator::SensorSimulation().attachLidarSensor(0.0, configuration, node)); } @@ -50,13 +53,11 @@ TEST(SensorSimulation, attachLidarSensor_correctConfiguration) * @note Test function behavior when called with a configuration architecture type other than * "awf/universe" - the goal is to test error throwing. */ -TEST(SensorSimulation, attachDetectionSensor_wrongArchitecture) +TEST_F(SensorSimulationTest, attachDetectionSensor_wrongArchitecture) { auto configuration = simulation_api_schema::DetectionSensorConfiguration{}; configuration.set_architecture_type("wrong/architecture"); - auto node = rclcpp::Node{"name", rclcpp::NodeOptions{}}; - EXPECT_THROW( simple_sensor_simulator::SensorSimulation().attachDetectionSensor(0.0, configuration, node), std::runtime_error); @@ -66,13 +67,11 @@ TEST(SensorSimulation, attachDetectionSensor_wrongArchitecture) * @note Test basic functionality. Test attaching a detection sensor correctness * with a sample detection sensor configuration. */ -TEST(SensorSimulation, attachDetectionSensor_correctConfiguration) +TEST_F(SensorSimulationTest, attachDetectionSensor_correctConfiguration) { auto configuration = simulation_api_schema::DetectionSensorConfiguration{}; configuration.set_architecture_type("awf/universe"); - auto node = rclcpp::Node{"name", rclcpp::NodeOptions{}}; - EXPECT_NO_THROW( simple_sensor_simulator::SensorSimulation().attachDetectionSensor(0.0, configuration, node)); } @@ -81,13 +80,11 @@ TEST(SensorSimulation, attachDetectionSensor_correctConfiguration) * @note Test function behavior when called with a configuration architecture type other than * "awf/universe" - the goal is to test error throwing. */ -TEST(SensorSimulation, attachOccupancyGridSensor_wrongArchitecture) +TEST_F(SensorSimulationTest, attachOccupancyGridSensor_wrongArchitecture) { auto configuration = simulation_api_schema::OccupancyGridSensorConfiguration{}; configuration.set_architecture_type("wrong/architecture"); - auto node = rclcpp::Node{"name", rclcpp::NodeOptions{}}; - EXPECT_THROW( simple_sensor_simulator::SensorSimulation().attachOccupancyGridSensor(0.0, configuration, node), std::runtime_error); @@ -97,13 +94,11 @@ TEST(SensorSimulation, attachOccupancyGridSensor_wrongArchitecture) * @note Test basic functionality. Test attaching a occupancy grid sensor correctness * with a sample occupancy grid sensor configuration. */ -TEST(SensorSimulation, attachOccupancyGridSensor_correctConfiguration) +TEST_F(SensorSimulationTest, attachOccupancyGridSensor_correctConfiguration) { auto configuration = simulation_api_schema::OccupancyGridSensorConfiguration{}; configuration.set_architecture_type("awf/universe"); - auto node = rclcpp::Node{"name", rclcpp::NodeOptions{}}; - EXPECT_NO_THROW(simple_sensor_simulator::SensorSimulation().attachOccupancyGridSensor( 0.0, configuration, node)); } diff --git a/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp index 72d91df4bd4..43272c42e6b 100644 --- a/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp +++ b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp @@ -15,15 +15,12 @@ #include #include -#include #include #include #include #include #include #include -#include -#include #include "sensor_simulation/expect_eq_macros.hpp" @@ -115,6 +112,23 @@ auto makeAttachOccupancyGridSensorRequest() return request; } +class ScenarioSimulatorTest : public testing::Test +{ +protected: + ScenarioSimulatorTest() + : server([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + }), + client(simulation_interface::TransportProtocol::TCP, "localhost", 5555U) + { + } + std::thread server; + zeromq::MultiClient client; +}; + /** * @note Test initialization correctness with a sample request with the default port (5555). */ @@ -127,10 +141,10 @@ TEST(ScenarioSimulator, initialize_defaultPort) rclcpp::spin(component); }); - auto multi_client = + auto client = zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); - EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); rclcpp::shutdown(); server.join(); @@ -150,10 +164,10 @@ TEST(ScenarioSimulator, initialize_customPort) rclcpp::spin(component); }); - auto multi_client = + auto client = zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 1234U); - EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); rclcpp::shutdown(); server.join(); @@ -162,20 +176,10 @@ TEST(ScenarioSimulator, initialize_customPort) /** * @note Test updating frame correctness with a sample request. */ -TEST(ScenarioSimulator, updateFrame_correct) +TEST_F(ScenarioSimulatorTest, updateFrame_correct) { - auto server = std::thread([] { - rclcpp::init(0, nullptr); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - }); - - auto multi_client = - zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); - - EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); - EXPECT_TRUE(multi_client.call(makeUpdateFrameRequest()).result().success()); + EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(client.call(makeUpdateFrameRequest()).result().success()); rclcpp::shutdown(); server.join(); @@ -184,20 +188,11 @@ TEST(ScenarioSimulator, updateFrame_correct) /** * @note Test updating frame correctness with a sample request before requesting initialization. */ -TEST(ScenarioSimulator, updateFrame_noInitialize) +TEST_F(ScenarioSimulatorTest, updateFrame_noInitialize) { // undefined behaviour !!! there is missing "initialized_(false)," in the ScenarioSimulator constructor - auto server = std::thread([] { - rclcpp::init(0, nullptr); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - }); - - auto multi_client = - zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); - EXPECT_FALSE(multi_client.call(makeUpdateFrameRequest()).result().success()); + EXPECT_FALSE(client.call(makeUpdateFrameRequest()).result().success()); rclcpp::shutdown(); server.join(); @@ -207,20 +202,10 @@ TEST(ScenarioSimulator, updateFrame_noInitialize) * @note Test spawning vehicle entity correctness with a request to spawn * Ego vehicle when Ego vehicle is not spawned yet. */ -TEST(ScenarioSimulator, spawnVehicleEntity_firstEgo) +TEST_F(ScenarioSimulatorTest, spawnVehicleEntity_firstEgo) { - auto server = std::thread([] { - rclcpp::init(0, nullptr); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - }); - - auto multi_client = - zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); - - EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); - EXPECT_TRUE(multi_client.call(makeSpawnVehicleEntityRequest("ego", true)).result().success()); + EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(client.call(makeSpawnVehicleEntityRequest("ego", true)).result().success()); rclcpp::shutdown(); server.join(); @@ -229,20 +214,10 @@ TEST(ScenarioSimulator, spawnVehicleEntity_firstEgo) /** * @note Test spawning vehicle entity correctness with a request to spawn a NPC vehicle. */ -TEST(ScenarioSimulator, spawnVehicleEntity_npc) +TEST_F(ScenarioSimulatorTest, spawnVehicleEntity_npc) { - auto server = std::thread([] { - rclcpp::init(0, nullptr); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - }); - - auto multi_client = - zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); - - EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); - EXPECT_TRUE(multi_client.call(makeSpawnVehicleEntityRequest("npc", false)).result().success()); + EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(client.call(makeSpawnVehicleEntityRequest("npc", false)).result().success()); rclcpp::shutdown(); server.join(); @@ -251,20 +226,10 @@ TEST(ScenarioSimulator, spawnVehicleEntity_npc) /** * @note Test spawning pedestrian entity correctness with a request to spawn a NPC pedestrian. */ -TEST(ScenarioSimulator, spawnPedestrianEntity) +TEST_F(ScenarioSimulatorTest, spawnPedestrianEntity) { - auto server = std::thread([] { - rclcpp::init(0, nullptr); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - }); - - auto multi_client = - zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); - - EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); - EXPECT_TRUE(multi_client.call(makeSpawnPedestrianEntityRequest("bob")).result().success()); + EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(client.call(makeSpawnPedestrianEntityRequest("bob")).result().success()); rclcpp::shutdown(); server.join(); @@ -273,20 +238,10 @@ TEST(ScenarioSimulator, spawnPedestrianEntity) /** * @note Test spawning misc object entity correctness with a request to spawn a misc object. */ -TEST(ScenarioSimulator, spawnMiscObjectEntity) +TEST_F(ScenarioSimulatorTest, spawnMiscObjectEntity) { - auto server = std::thread([] { - rclcpp::init(0, nullptr); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - }); - - auto multi_client = - zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); - - EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); - EXPECT_TRUE(multi_client.call(makeSpawnMiscObjectEntityRequest("blob")).result().success()); + EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(client.call(makeSpawnMiscObjectEntityRequest("blob")).result().success()); rclcpp::shutdown(); server.join(); @@ -295,21 +250,11 @@ TEST(ScenarioSimulator, spawnMiscObjectEntity) /** * @note Test despawning an entity with a request to despawn an existing vehicle. */ -TEST(ScenarioSimulator, despawnEntity_vehicle) +TEST_F(ScenarioSimulatorTest, despawnEntity_vehicle) { - auto server = std::thread([] { - rclcpp::init(0, nullptr); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - }); - - auto multi_client = - zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); - - EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); - EXPECT_TRUE(multi_client.call(makeSpawnVehicleEntityRequest("npc", false)).result().success()); - EXPECT_TRUE(multi_client.call(makeDespawnEntityRequest("npc")).result().success()); + EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(client.call(makeSpawnVehicleEntityRequest("npc", false)).result().success()); + EXPECT_TRUE(client.call(makeDespawnEntityRequest("npc")).result().success()); rclcpp::shutdown(); server.join(); @@ -318,21 +263,11 @@ TEST(ScenarioSimulator, despawnEntity_vehicle) /** * @note Test despawning an entity with a request to despawn an existing pedestrian. */ -TEST(ScenarioSimulator, despawnEntity_pedestrian) +TEST_F(ScenarioSimulatorTest, despawnEntity_pedestrian) { - auto server = std::thread([] { - rclcpp::init(0, nullptr); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - }); - - auto multi_client = - zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); - - EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); - EXPECT_TRUE(multi_client.call(makeSpawnPedestrianEntityRequest("bob")).result().success()); - EXPECT_TRUE(multi_client.call(makeDespawnEntityRequest("bob")).result().success()); + EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(client.call(makeSpawnPedestrianEntityRequest("bob")).result().success()); + EXPECT_TRUE(client.call(makeDespawnEntityRequest("bob")).result().success()); rclcpp::shutdown(); server.join(); @@ -341,21 +276,11 @@ TEST(ScenarioSimulator, despawnEntity_pedestrian) /** * @note Test despawning an entity with a request to despawn an existing misc object. */ -TEST(ScenarioSimulator, despawnEntity_miscObject) +TEST_F(ScenarioSimulatorTest, despawnEntity_miscObject) { - auto server = std::thread([] { - rclcpp::init(0, nullptr); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - }); - - auto multi_client = - zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); - - EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); - EXPECT_TRUE(multi_client.call(makeSpawnMiscObjectEntityRequest("blob")).result().success()); - EXPECT_TRUE(multi_client.call(makeDespawnEntityRequest("blob")).result().success()); + EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(client.call(makeSpawnMiscObjectEntityRequest("blob")).result().success()); + EXPECT_TRUE(client.call(makeDespawnEntityRequest("blob")).result().success()); rclcpp::shutdown(); server.join(); @@ -364,20 +289,10 @@ TEST(ScenarioSimulator, despawnEntity_miscObject) /** * @note Test despawning an entity with a request to despawn an entity that does not exist. */ -TEST(ScenarioSimulator, despawnEntity_invalidName) +TEST_F(ScenarioSimulatorTest, despawnEntity_invalidName) { - auto server = std::thread([] { - rclcpp::init(0, nullptr); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - }); - - auto multi_client = - zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); - - EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); - EXPECT_FALSE(multi_client.call(makeDespawnEntityRequest("invalid")).result().success()); + EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); + EXPECT_FALSE(client.call(makeDespawnEntityRequest("invalid")).result().success()); rclcpp::shutdown(); server.join(); @@ -386,20 +301,10 @@ TEST(ScenarioSimulator, despawnEntity_invalidName) /** * @note Test attaching detection sensor with a request to attach a sensor with a valid configuration. */ -TEST(ScenarioSimulator, attachDetectionSensor) +TEST_F(ScenarioSimulatorTest, attachDetectionSensor) { - auto server = std::thread([] { - rclcpp::init(0, nullptr); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - }); - - auto multi_client = - zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); - - EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); - EXPECT_TRUE(multi_client.call(makeAttachDetectionSensorRequest()).result().success()); + EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(client.call(makeAttachDetectionSensorRequest()).result().success()); rclcpp::shutdown(); server.join(); @@ -408,20 +313,10 @@ TEST(ScenarioSimulator, attachDetectionSensor) /** * @note Test attaching lidar sensor with a request to attach a sensor with a valid configuration. */ -TEST(ScenarioSimulator, attachLidarSensor) +TEST_F(ScenarioSimulatorTest, attachLidarSensor) { - auto server = std::thread([] { - rclcpp::init(0, nullptr); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - }); - - auto multi_client = - zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); - - EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); - EXPECT_TRUE(multi_client.call(makeAttachLidarSensorRequest()).result().success()); + EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(client.call(makeAttachLidarSensorRequest()).result().success()); rclcpp::shutdown(); server.join(); @@ -430,20 +325,10 @@ TEST(ScenarioSimulator, attachLidarSensor) /** * @note Test attaching occupancy grid sensor with a request to attach a sensor with a valid configuration. */ -TEST(ScenarioSimulator, attachOccupancyGridSensor) +TEST_F(ScenarioSimulatorTest, attachOccupancyGridSensor) { - auto server = std::thread([] { - rclcpp::init(0, nullptr); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - }); - - auto multi_client = - zeromq::MultiClient(simulation_interface::TransportProtocol::TCP, "localhost", 5555U); - - EXPECT_TRUE(multi_client.call(makeInitializeRequest()).result().success()); - EXPECT_TRUE(multi_client.call(makeAttachOccupancyGridSensorRequest()).result().success()); + EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); + EXPECT_TRUE(client.call(makeAttachOccupancyGridSensorRequest()).result().success()); rclcpp::shutdown(); server.join(); From 3d8101d7c042677ecf2c756d6fd9f6c92b19b40f Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 20 Jun 2024 11:46:33 +0200 Subject: [PATCH 105/118] update comments, newlines --- .../test/CMakeLists.txt | 2 +- .../occupancy_grid/CMakeLists.txt | 3 -- .../test_occupancy_grid_builder.cpp | 52 +++++-------------- .../test_occupancy_grid_sensor.cpp | 24 --------- .../test/src/test_scenario_simulator.cpp | 5 +- 5 files changed, 15 insertions(+), 71 deletions(-) delete mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt index c4913bedad5..02739fd3cdc 100644 --- a/simulation/simple_sensor_simulator/test/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/CMakeLists.txt @@ -9,4 +9,4 @@ ament_add_gtest(test_scenario_simulator src/test_scenario_simulator.cpp) target_link_libraries(test_scenario_simulator simple_sensor_simulator_component ${Protobuf_LIBRARIES}) ament_add_gtest(test_sensor_simulation src/sensor_simulation/test_sensor_simulation.cpp) -target_link_libraries(test_sensor_simulation simple_sensor_simulator_component ${Protobuf_LIBRARIES}) \ No newline at end of file +target_link_libraries(test_sensor_simulation simple_sensor_simulator_component ${Protobuf_LIBRARIES}) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt index cf2ed3de1e2..a8dad91304f 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt @@ -3,6 +3,3 @@ target_link_libraries(test_grid_traversal simple_sensor_simulator_component) ament_add_gtest(test_occupancy_grid_builder test_occupancy_grid_builder.cpp) target_link_libraries(test_occupancy_grid_builder simple_sensor_simulator_component) - -ament_add_gtest(test_occupancy_grid_sensor test_occupancy_grid_sensor.cpp) -target_link_libraries(test_occupancy_grid_sensor simple_sensor_simulator_component) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp index 1b94d0a44cb..f74400a3c6f 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp @@ -144,6 +144,18 @@ TEST(OccupancyGridBuilder, add_primitiveInCenter) 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // clang-format on + /* This is how the algorithm's output looks, zeros on the right are unexpected. + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, + 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 1, 1, 0, 0, 0, 0, 0, + 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 1, 0, 0, 0, 0, 0, 0, + 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 0, 0, 0, 0, 0, 0, 0, + 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 0, 0, 0, 0, 0, 0, 0, + 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 0, 0, 0, 0, 0, 0, 0, + 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 0, 0, 0, 0, 0, 0, 0, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, + */ }; EXPECT_EQ(result_expected, result_obtained); } @@ -183,43 +195,3 @@ int main(int argc, char ** argv) testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } - -/* -Methods .add(), .build(), .get() will inherently interleave each other. -Can't test them separately. - - -Why does only the method .reset(POSE) have an ability to change the pose? -It seems weird. - - -WEIRD ZEROS ON THE RIGHT -builder.add(makeBox(5.0f, 0.0, 0.0)); - -1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 -1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 -1 1 1 1 1 1 1 2 2 2 2 2 2 1 1 0 0 0 0 0 -1 1 1 1 1 1 1 2 2 2 2 2 2 1 0 0 0 0 0 0 -1 1 1 1 1 1 1 2 2 2 2 2 2 0 0 0 0 0 0 0 -1 1 1 1 1 1 1 2 2 2 2 2 2 0 0 0 0 0 0 0 -1 1 1 1 1 1 1 2 2 2 2 2 2 0 0 0 0 0 0 0 -1 1 1 1 1 1 1 2 2 2 2 2 2 0 0 0 0 0 0 0 -1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 -1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 - - -WEIRD ONE IN THE MIDDLE -builder.add(makeBox(5.0f, 4.0, 0.0)); -builder.add(makeBox(2.0f, -5.0, 0.0)); - -0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 -0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 -1 1 1 0 0 0 0 0 0 0 0 2 2 2 2 2 2 1 1 1 -1 1 1 1 1 1 1 1 0 0 0 2 2 2 2 2 2 1 1 1 -1 1 1 1 2 2 2 0 0 0 0 2 2 2 2 2 2 1 1 1 -1 1 1 1 2 2 2 0 0 0 0 2 2 2 2 2 2 1 1 1 -1 1 1 1 2 2 2 0 0 0 0 2 2 2 2 2 2 1 1 1 -1 1 1 1 1 1 1 0 0 0 0 2 2 2 2 2 2 1 1 1 -1 1 1 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 -0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 -*/ \ No newline at end of file diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp deleted file mode 100644 index 2f180c0e0a2..00000000000 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_sensor.cpp +++ /dev/null @@ -1,24 +0,0 @@ -// 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 - -#include -#include - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp index 43272c42e6b..154a8c88778 100644 --- a/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp +++ b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp @@ -190,8 +190,7 @@ TEST_F(ScenarioSimulatorTest, updateFrame_correct) */ TEST_F(ScenarioSimulatorTest, updateFrame_noInitialize) { - // undefined behaviour !!! there is missing "initialized_(false)," in the ScenarioSimulator constructor - + // UB is entered during this test. ScenarioSimulator constructor is missing "initialized_(false)," EXPECT_FALSE(client.call(makeUpdateFrameRequest()).result().success()); rclcpp::shutdown(); @@ -338,4 +337,4 @@ int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} From 0dbbc2811d3b5e2ac01e98643056b2e7159e4b3f Mon Sep 17 00:00:00 2001 From: Release Bot Date: Thu, 20 Jun 2024 12:01:05 +0000 Subject: [PATCH 106/118] Bump version of scenario_simulator_v2 from version 2.1.7 to version 2.1.8 --- common/math/arithmetic/CHANGELOG.rst | 6 ++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 6 ++++ common/math/geometry/package.xml | 2 +- .../CHANGELOG.rst | 6 ++++ .../scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 6 ++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 6 ++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 6 ++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 6 ++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 6 ++++ map/kashiwanoha_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 6 ++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../CHANGELOG.rst | 6 ++++ .../package.xml | 2 +- .../openscenario_interpreter/CHANGELOG.rst | 6 ++++ .../openscenario_interpreter/package.xml | 2 +- .../CHANGELOG.rst | 6 ++++ .../package.xml | 2 +- .../CHANGELOG.rst | 6 ++++ .../openscenario_interpreter_msgs/package.xml | 2 +- .../openscenario_preprocessor/CHANGELOG.rst | 6 ++++ .../openscenario_preprocessor/package.xml | 2 +- .../CHANGELOG.rst | 6 ++++ .../package.xml | 2 +- .../openscenario_utility/CHANGELOG.rst | 6 ++++ openscenario/openscenario_utility/package.xml | 2 +- .../openscenario_validator/CHANGELOG.rst | 6 ++++ .../openscenario_validator/package.xml | 2 +- .../openscenario_visualization/CHANGELOG.rst | 6 ++++ .../openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 6 ++++ .../package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 6 ++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 6 ++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 6 ++++ simulation/do_nothing_plugin/package.xml | 2 +- .../simple_sensor_simulator/CHANGELOG.rst | 32 +++++++++++++++++++ .../simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 6 ++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 6 ++++ simulation/traffic_simulator/package.xml | 2 +- .../traffic_simulator_msgs/CHANGELOG.rst | 6 ++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 6 ++++ test_runner/random_test_runner/package.xml | 2 +- .../scenario_test_runner/CHANGELOG.rst | 6 ++++ test_runner/scenario_test_runner/package.xml | 2 +- 56 files changed, 222 insertions(+), 28 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 9deea1f5a57..11e1fa3a014 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package arithmetic * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index cbda1e06db6..0fba5c0bbbe 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 2.1.7 + 2.1.8 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index 2fef972efdf..e5eeb0d64f1 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package geometry * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 33a53ffebe2..ed5422bd89f 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 2.1.7 + 2.1.8 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index e43733599dd..1780d2722a2 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package scenario_simulator_exception * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index d775df3d2ac..ddffee8182d 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 2.1.7 + 2.1.8 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index 72d792d3e3e..8747dd30cc0 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package junit_exporter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index d26a4845977..82ae848e777 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 2.1.7 + 2.1.8 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index fabf2422b54..d6edadcb875 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package status_monitor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 2c5a649c247..8c225fe70b0 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 2.1.7 + 2.1.8 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index ffe281bd402..6744fbee224 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package concealer * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/external/concealer/package.xml b/external/concealer/package.xml index df4a0d58c47..18241c6c15d 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 2.1.7 + 2.1.8 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index c5635764d39..b491555134b 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -16,6 +16,12 @@ Changelog for package embree_vendor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index c83e13bf2df..4ac0edb6620 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 2.1.7 + 2.1.8 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index f1254f86ca8..05bf18e7d9e 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package kashiwanoha_map * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 3aa114f33f0..cf2314b7c9b 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 2.1.7 + 2.1.8 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index 45f5ed9e69d..2b307f81cb4 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package cpp_mock_scenarios * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge pull request `#1275 `_ from tier4/feature/improve-ros-parameter-handling diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index d2de246fa95..0a20573a470 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 2.1.7 + 2.1.8 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 7d732774ad2..5e47590cb52 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_experimental_catalog * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index d7d40291637..395c678d5f4 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 2.1.7 + 2.1.8 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index a2c94658b4a..1b2d50612c9 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -14,6 +14,12 @@ Changelog for package openscenario_interpreter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge pull request `#1275 `_ from tier4/feature/improve-ros-parameter-handling diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 23c2b6d72de..e3ae867a80f 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 2.1.7 + 2.1.8 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 3c7585bfd04..5ecf3d386cb 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_interpreter_example * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 6e1d60ff3d2..ccd1ec9fe07 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 2.1.7 + 2.1.8 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index 722d6c52414..303155de75f 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_interpreter_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 76fd9b2644a..25525f2821c 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 2.1.7 + 2.1.8 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index 642beaae459..d343134493a 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_preprocessor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 606dcebb3e4..7c88b485345 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 2.1.7 + 2.1.8 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index c4036e19a10..cde933c0d81 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_preprocessor_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index a055a7e2601..4cb164009ec 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 2.1.7 + 2.1.8 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 23411560857..8b5624a5b82 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -16,6 +16,12 @@ Changelog for package openscenario_utility * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 048acf3f72f..f4eabc98759 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 2.1.7 + 2.1.8 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index 34529344df9..cc68e668841 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package openscenario_validator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 45e9b50e53d..77f15a0c906 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 2.1.7 + 2.1.8 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 68eaf3457a1..5cfc0e50646 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_visualization * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index cde6936cff2..ca828bcbda2 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 2.1.7 + 2.1.8 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index e72288b9ead..97a6e5df448 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index b6914388dde..70cdf29b301 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 2.1.7 + 2.1.8 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 2cc48bef587..da730858713 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package scenario_simulator_v2 * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 1ab02509b02..204ae5d70d7 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 2.1.7 + 2.1.8 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index f123b83193b..da4f5cf2edb 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package behavior_tree_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 14d4c9970e8..df1b803add3 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 2.1.7 + 2.1.8 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 86dee7ed816..d66d557ee80 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package do_nothing_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 2cc5655e0a1..ad3512650fd 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 2.1.7 + 2.1.8 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 8bf43b48c9b..210c79a9392 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -13,6 +13,38 @@ Changelog for package simple_sensor_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge pull request `#1291 `_ from tier4/feature/simple_sensor_simulator_unit_test + Feature/simple sensor simulator unit test +* Moved ament_cmake_gtest to package.xml +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* test: [RJD-937] to Implement Unit tests on simple_sensor_simulator + - Fixed an issue with missed header file +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* test: [RJD-937] to Implement Unit tests on simple_sensor_simulator + - Fixed an issue with missing package +* test: [RJD-937] to Implement Unit tests on simple_sensor_simulator + - Corrected directory structure + - Fixed an issue with a mistake +* test: [RJD-937] to Implement Unit tests on simple_sensor_simulator + - Replaced constructors with SetUp: RaycasterTest and GridTraversalTest + - Refactored Box, GridTraversal and Vertex unit tests +* test: [RJD-937] to Implement Unit tests on simple_sensor_simulator + - Fixed an issue with Raycaster tests - pointcloud size checking + - Updated test descriptions + - Added 'const' to specific variables + - Ensured a new line is added at the end of each file +* test: [RJD-937] to Implement Unit tests on simple_sensor_simulator + - Added CMakeLists.txt +* test: [RJD-937] to Implement Unit tests on simple_sensor_simulator + - Added unit tests to GridTraversal +* test: [RJD-937] to Implement Unit tests on simple_sensor_simulator + - Added unit tests to Box and Vertex classes + - Added unit tests to Raycaster class + - Added set of macros used in unit tests +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index dff923ae37b..e3c14edbb7d 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 2.1.7 + 2.1.8 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 566d4755bc2..af5f51fc0f5 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package simulation_interface * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 30292fa588f..0ae0ff65fd9 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 2.1.7 + 2.1.8 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index 2252b2710d5..efc3979d64c 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package traffic_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge pull request `#1275 `_ from tier4/feature/improve-ros-parameter-handling diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 826d273d550..791782a002e 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 2.1.7 + 2.1.8 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 418fbb74852..994f32f3485 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package openscenario_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 2d17165f834..e260652502d 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 2.1.7 + 2.1.8 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 531a5dd17af..d61aa672c17 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -13,6 +13,12 @@ Changelog for package random_test_runner * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index ee139252a6c..8048afce27c 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 2.1.7 + 2.1.8 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 3ea78095fda..07fdfeab3e5 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -23,6 +23,12 @@ Changelog for package scenario_test_runner * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.8 (2024-06-20) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Merge branch 'master' into feature/simple_sensor_simulator_unit_test +* Contributors: Kotaro Yoshimoto, SzymonParapura + 2.1.7 (2024-06-19) ------------------ * Merge branch 'master' into feature/improve-ros-parameter-handling diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 1bf1f8ed87b..6d04eff5cd1 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 2.1.7 + 2.1.8 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 6e919da05dea256e5b228685af88a03d33f81cd9 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Mon, 24 Jun 2024 11:19:27 +0900 Subject: [PATCH 107/118] remove arm build test Signed-off-by: Masaya Kataoka --- .github/workflows/Docker.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/Docker.yaml b/.github/workflows/Docker.yaml index 86c80c2c8c4..47d8950d84b 100644 --- a/.github/workflows/Docker.yaml +++ b/.github/workflows/Docker.yaml @@ -26,7 +26,8 @@ jobs: strategy: matrix: rosdistro: [humble] - arch: [amd64, arm64] + arch: [amd64] + # arch: [amd64, arm64] steps: - name: Free Disk Space (Ubuntu) uses: jlumbroso/free-disk-space@main From 8b6b47b3dbf50c19c49dead9a929baf8c2eba0bd Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Mon, 24 Jun 2024 11:22:17 +0900 Subject: [PATCH 108/118] update Docker.yaml Signed-off-by: Masaya Kataoka --- .github/workflows/Docker.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/Docker.yaml b/.github/workflows/Docker.yaml index 47d8950d84b..9055fc47db1 100644 --- a/.github/workflows/Docker.yaml +++ b/.github/workflows/Docker.yaml @@ -27,6 +27,9 @@ jobs: matrix: rosdistro: [humble] arch: [amd64] + # Build test for arm64 CPU is broken. + # This is a temporary solution and will be repaired in the future. + # See also https://github.com/tier4/scenario_simulator_v2/pull/1295 # arch: [amd64, arm64] steps: - name: Free Disk Space (Ubuntu) From ab97cc35e6609df2b5e30d05fa4c3b9dc64ec71e Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Mon, 24 Jun 2024 11:23:08 +0900 Subject: [PATCH 109/118] update lockfile Signed-off-by: Masaya Kataoka --- poetry.lock | 280 ++++++++++++++++++++++++++-------------------------- 1 file changed, 141 insertions(+), 139 deletions(-) diff --git a/poetry.lock b/poetry.lock index 2ae6f713e48..536d6227fad 100644 --- a/poetry.lock +++ b/poetry.lock @@ -1,4 +1,4 @@ -# This file is automatically @generated by Poetry 1.8.2 and should not be changed by hand. +# This file is automatically @generated by Poetry 1.8.3 and should not be changed by hand. [[package]] name = "babel" @@ -47,13 +47,13 @@ test = ["flake8", "flake8-blind-except", "flake8-builtins", "flake8-class-newlin [[package]] name = "certifi" -version = "2024.2.2" +version = "2024.6.2" description = "Python package for providing Mozilla's CA Bundle." optional = false python-versions = ">=3.6" files = [ - {file = "certifi-2024.2.2-py3-none-any.whl", hash = "sha256:dc383c07b76109f368f6106eee2b593b04a011ea4d55f652c6ca24a754d1cdd1"}, - {file = "certifi-2024.2.2.tar.gz", hash = "sha256:0569859f95fc761b18b45ef421b1290a0f65f147e92a1e5eb3e635f9a5e4e66f"}, + {file = "certifi-2024.6.2-py3-none-any.whl", hash = "sha256:ddc6c8ce995e6987e7faf5e3f1b02b302836a0e5d98ece18392cb1a36c72ad56"}, + {file = "certifi-2024.6.2.tar.gz", hash = "sha256:3cd43f1c6fa7dedc5899d69d3ad0398fd018ad1a17fba83ddaf78aa46c747516"}, ] [[package]] @@ -452,13 +452,13 @@ pygments = ">=2.9.0" [[package]] name = "mkdocs-git-revision-date-localized-plugin" -version = "1.2.5" +version = "1.2.6" description = "Mkdocs plugin that enables displaying the localized date of the last git modification of a markdown file." optional = false python-versions = ">=3.8" files = [ - {file = "mkdocs_git_revision_date_localized_plugin-1.2.5-py3-none-any.whl", hash = "sha256:d796a18b07cfcdb154c133e3ec099d2bb5f38389e4fd54d3eb516a8a736815b8"}, - {file = "mkdocs_git_revision_date_localized_plugin-1.2.5.tar.gz", hash = "sha256:0c439816d9d0dba48e027d9d074b2b9f1d7cd179f74ba46b51e4da7bb3dc4b9b"}, + {file = "mkdocs_git_revision_date_localized_plugin-1.2.6-py3-none-any.whl", hash = "sha256:f015cb0f3894a39b33447b18e270ae391c4e25275cac5a626e80b243784e2692"}, + {file = "mkdocs_git_revision_date_localized_plugin-1.2.6.tar.gz", hash = "sha256:e432942ce4ee8aa9b9f4493e993dee9d2cc08b3ea2b40a3d6b03ca0f2a4bcaa2"}, ] [package.dependencies] @@ -543,13 +543,13 @@ icu = ["PyICU (>=1.0.0)"] [[package]] name = "packaging" -version = "24.0" +version = "24.1" description = "Core utilities for Python packages" optional = false -python-versions = ">=3.7" +python-versions = ">=3.8" files = [ - {file = "packaging-24.0-py3-none-any.whl", hash = "sha256:2ddfb553fdf02fb784c234c7ba6ccc288296ceabec964ad2eae3777778130bc5"}, - {file = "packaging-24.0.tar.gz", hash = "sha256:eb82c5e3e56209074766e6885bb04b8c38a0c015d0a30036ebe7ece34c9989e9"}, + {file = "packaging-24.1-py3-none-any.whl", hash = "sha256:5b8f2217dbdbd2f7f384c41c628544e6d52f2d0f53c6d0c3ea61aa5d1d7ff124"}, + {file = "packaging-24.1.tar.gz", hash = "sha256:026ed72c8ed3fcce5bf8950572258698927fd1dbda10a5e981cdf0ac37f4f002"}, ] [[package]] @@ -588,13 +588,13 @@ httplib2 = "*" [[package]] name = "plantuml-markdown" -version = "3.9.6" +version = "3.9.7" description = "A PlantUML plugin for Markdown" optional = false python-versions = "*" files = [ - {file = "plantuml-markdown-3.9.6.tar.gz", hash = "sha256:65bfa22c030e5250374f54d3f3e342fdd98314d64ab2869b7d106a0b1dbb2fed"}, - {file = "plantuml_markdown-3.9.6-py3-none-any.whl", hash = "sha256:46ec054c652c4026dd37060df9bdec57e2677a33b53b3d363186fd2fe6457060"}, + {file = "plantuml-markdown-3.9.7.tar.gz", hash = "sha256:8ee5b08ad40ebfa33e5f768ca0108e707b67abcbafb3060a9a6794c4fc0fca99"}, + {file = "plantuml_markdown-3.9.7-py3-none-any.whl", hash = "sha256:beff4f9f33d3718159077a8b3e10804c7c26599c9a621bf769cd71a3d5059cc0"}, ] [package.dependencies] @@ -604,13 +604,13 @@ six = "*" [[package]] name = "platformdirs" -version = "4.2.1" +version = "4.2.2" description = "A small Python package for determining appropriate platform-specific dirs, e.g. a `user data dir`." optional = false python-versions = ">=3.8" files = [ - {file = "platformdirs-4.2.1-py3-none-any.whl", hash = "sha256:17d5a1161b3fd67b390023cb2d3b026bbd40abde6fdb052dfbd3a29c3ba22ee1"}, - {file = "platformdirs-4.2.1.tar.gz", hash = "sha256:031cd18d4ec63ec53e82dceaac0417d218a6863f7745dfcc9efe7793b7039bdf"}, + {file = "platformdirs-4.2.2-py3-none-any.whl", hash = "sha256:2d7a1657e36a80ea911db832a8a6ece5ee53d8de21edd5cc5879af6530b1bfee"}, + {file = "platformdirs-4.2.2.tar.gz", hash = "sha256:38b7b51f512eed9e84a22788b4bce1de17c0adb134d6becb09836e37d8654cd3"}, ] [package.extras] @@ -779,101 +779,101 @@ pyyaml = "*" [[package]] name = "regex" -version = "2024.4.28" +version = "2024.5.15" description = "Alternative regular expression module, to replace re." optional = false python-versions = ">=3.8" files = [ - {file = "regex-2024.4.28-cp310-cp310-macosx_10_9_universal2.whl", hash = "sha256:cd196d056b40af073d95a2879678585f0b74ad35190fac04ca67954c582c6b61"}, - {file = "regex-2024.4.28-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:8bb381f777351bd534462f63e1c6afb10a7caa9fa2a421ae22c26e796fe31b1f"}, - {file = "regex-2024.4.28-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:47af45b6153522733aa6e92543938e97a70ce0900649ba626cf5aad290b737b6"}, - {file = "regex-2024.4.28-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:99d6a550425cc51c656331af0e2b1651e90eaaa23fb4acde577cf15068e2e20f"}, - {file = "regex-2024.4.28-cp310-cp310-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:bf29304a8011feb58913c382902fde3395957a47645bf848eea695839aa101b7"}, - {file = "regex-2024.4.28-cp310-cp310-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:92da587eee39a52c91aebea8b850e4e4f095fe5928d415cb7ed656b3460ae79a"}, - {file = "regex-2024.4.28-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:6277d426e2f31bdbacb377d17a7475e32b2d7d1f02faaecc48d8e370c6a3ff31"}, - {file = "regex-2024.4.28-cp310-cp310-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:28e1f28d07220c0f3da0e8fcd5a115bbb53f8b55cecf9bec0c946eb9a059a94c"}, - {file = "regex-2024.4.28-cp310-cp310-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_12_x86_64.manylinux2010_x86_64.whl", hash = "sha256:aaa179975a64790c1f2701ac562b5eeb733946eeb036b5bcca05c8d928a62f10"}, - {file = "regex-2024.4.28-cp310-cp310-musllinux_1_1_aarch64.whl", hash = "sha256:6f435946b7bf7a1b438b4e6b149b947c837cb23c704e780c19ba3e6855dbbdd3"}, - {file = "regex-2024.4.28-cp310-cp310-musllinux_1_1_i686.whl", hash = "sha256:19d6c11bf35a6ad077eb23852827f91c804eeb71ecb85db4ee1386825b9dc4db"}, - {file = "regex-2024.4.28-cp310-cp310-musllinux_1_1_ppc64le.whl", hash = "sha256:fdae0120cddc839eb8e3c15faa8ad541cc6d906d3eb24d82fb041cfe2807bc1e"}, - {file = "regex-2024.4.28-cp310-cp310-musllinux_1_1_s390x.whl", hash = "sha256:e672cf9caaf669053121f1766d659a8813bd547edef6e009205378faf45c67b8"}, - {file = "regex-2024.4.28-cp310-cp310-musllinux_1_1_x86_64.whl", hash = "sha256:f57515750d07e14743db55d59759893fdb21d2668f39e549a7d6cad5d70f9fea"}, - {file = "regex-2024.4.28-cp310-cp310-win32.whl", hash = "sha256:a1409c4eccb6981c7baabc8888d3550df518add6e06fe74fa1d9312c1838652d"}, - {file = "regex-2024.4.28-cp310-cp310-win_amd64.whl", hash = "sha256:1f687a28640f763f23f8a9801fe9e1b37338bb1ca5d564ddd41619458f1f22d1"}, - {file = "regex-2024.4.28-cp311-cp311-macosx_10_9_universal2.whl", hash = "sha256:84077821c85f222362b72fdc44f7a3a13587a013a45cf14534df1cbbdc9a6796"}, - {file = "regex-2024.4.28-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:b45d4503de8f4f3dc02f1d28a9b039e5504a02cc18906cfe744c11def942e9eb"}, - {file = "regex-2024.4.28-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:457c2cd5a646dd4ed536c92b535d73548fb8e216ebee602aa9f48e068fc393f3"}, - {file = "regex-2024.4.28-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:2b51739ddfd013c6f657b55a508de8b9ea78b56d22b236052c3a85a675102dc6"}, - {file = "regex-2024.4.28-cp311-cp311-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:459226445c7d7454981c4c0ce0ad1a72e1e751c3e417f305722bbcee6697e06a"}, - {file = "regex-2024.4.28-cp311-cp311-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:670fa596984b08a4a769491cbdf22350431970d0112e03d7e4eeaecaafcd0fec"}, - {file = "regex-2024.4.28-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:fe00f4fe11c8a521b173e6324d862ee7ee3412bf7107570c9b564fe1119b56fb"}, - {file = "regex-2024.4.28-cp311-cp311-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:36f392dc7763fe7924575475736bddf9ab9f7a66b920932d0ea50c2ded2f5636"}, - {file = "regex-2024.4.28-cp311-cp311-musllinux_1_1_aarch64.whl", hash = "sha256:23a412b7b1a7063f81a742463f38821097b6a37ce1e5b89dd8e871d14dbfd86b"}, - {file = "regex-2024.4.28-cp311-cp311-musllinux_1_1_i686.whl", hash = "sha256:f1d6e4b7b2ae3a6a9df53efbf199e4bfcff0959dbdb5fd9ced34d4407348e39a"}, - {file = "regex-2024.4.28-cp311-cp311-musllinux_1_1_ppc64le.whl", hash = "sha256:499334ad139557de97cbc4347ee921c0e2b5e9c0f009859e74f3f77918339257"}, - {file = "regex-2024.4.28-cp311-cp311-musllinux_1_1_s390x.whl", hash = "sha256:0940038bec2fe9e26b203d636c44d31dd8766abc1fe66262da6484bd82461ccf"}, - {file = "regex-2024.4.28-cp311-cp311-musllinux_1_1_x86_64.whl", hash = "sha256:66372c2a01782c5fe8e04bff4a2a0121a9897e19223d9eab30c54c50b2ebeb7f"}, - {file = "regex-2024.4.28-cp311-cp311-win32.whl", hash = "sha256:c77d10ec3c1cf328b2f501ca32583625987ea0f23a0c2a49b37a39ee5c4c4630"}, - {file = "regex-2024.4.28-cp311-cp311-win_amd64.whl", hash = "sha256:fc0916c4295c64d6890a46e02d4482bb5ccf33bf1a824c0eaa9e83b148291f90"}, - {file = "regex-2024.4.28-cp312-cp312-macosx_10_9_universal2.whl", hash = "sha256:08a1749f04fee2811c7617fdd46d2e46d09106fa8f475c884b65c01326eb15c5"}, - {file = "regex-2024.4.28-cp312-cp312-macosx_10_9_x86_64.whl", hash = "sha256:b8eb28995771c087a73338f695a08c9abfdf723d185e57b97f6175c5051ff1ae"}, - {file = "regex-2024.4.28-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:dd7ef715ccb8040954d44cfeff17e6b8e9f79c8019daae2fd30a8806ef5435c0"}, - {file = "regex-2024.4.28-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:fb0315a2b26fde4005a7c401707c5352df274460f2f85b209cf6024271373013"}, - {file = "regex-2024.4.28-cp312-cp312-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:f2fc053228a6bd3a17a9b0a3f15c3ab3cf95727b00557e92e1cfe094b88cc662"}, - {file = "regex-2024.4.28-cp312-cp312-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:7fe9739a686dc44733d52d6e4f7b9c77b285e49edf8570754b322bca6b85b4cc"}, - {file = "regex-2024.4.28-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:a74fcf77d979364f9b69fcf8200849ca29a374973dc193a7317698aa37d8b01c"}, - {file = "regex-2024.4.28-cp312-cp312-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:965fd0cf4694d76f6564896b422724ec7b959ef927a7cb187fc6b3f4e4f59833"}, - {file = "regex-2024.4.28-cp312-cp312-musllinux_1_1_aarch64.whl", hash = "sha256:2fef0b38c34ae675fcbb1b5db760d40c3fc3612cfa186e9e50df5782cac02bcd"}, - {file = "regex-2024.4.28-cp312-cp312-musllinux_1_1_i686.whl", hash = "sha256:bc365ce25f6c7c5ed70e4bc674f9137f52b7dd6a125037f9132a7be52b8a252f"}, - {file = "regex-2024.4.28-cp312-cp312-musllinux_1_1_ppc64le.whl", hash = "sha256:ac69b394764bb857429b031d29d9604842bc4cbfd964d764b1af1868eeebc4f0"}, - {file = "regex-2024.4.28-cp312-cp312-musllinux_1_1_s390x.whl", hash = "sha256:144a1fc54765f5c5c36d6d4b073299832aa1ec6a746a6452c3ee7b46b3d3b11d"}, - {file = "regex-2024.4.28-cp312-cp312-musllinux_1_1_x86_64.whl", hash = "sha256:2630ca4e152c221072fd4a56d4622b5ada876f668ecd24d5ab62544ae6793ed6"}, - {file = "regex-2024.4.28-cp312-cp312-win32.whl", hash = "sha256:7f3502f03b4da52bbe8ba962621daa846f38489cae5c4a7b5d738f15f6443d17"}, - {file = "regex-2024.4.28-cp312-cp312-win_amd64.whl", hash = "sha256:0dd3f69098511e71880fb00f5815db9ed0ef62c05775395968299cb400aeab82"}, - {file = "regex-2024.4.28-cp38-cp38-macosx_10_9_universal2.whl", hash = "sha256:374f690e1dd0dbdcddea4a5c9bdd97632cf656c69113f7cd6a361f2a67221cb6"}, - {file = "regex-2024.4.28-cp38-cp38-macosx_10_9_x86_64.whl", hash = "sha256:25f87ae6b96374db20f180eab083aafe419b194e96e4f282c40191e71980c666"}, - {file = "regex-2024.4.28-cp38-cp38-macosx_11_0_arm64.whl", hash = "sha256:5dbc1bcc7413eebe5f18196e22804a3be1bfdfc7e2afd415e12c068624d48247"}, - {file = "regex-2024.4.28-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:f85151ec5a232335f1be022b09fbbe459042ea1951d8a48fef251223fc67eee1"}, - {file = "regex-2024.4.28-cp38-cp38-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:57ba112e5530530fd175ed550373eb263db4ca98b5f00694d73b18b9a02e7185"}, - {file = "regex-2024.4.28-cp38-cp38-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:224803b74aab56aa7be313f92a8d9911dcade37e5f167db62a738d0c85fdac4b"}, - {file = "regex-2024.4.28-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:0a54a047b607fd2d2d52a05e6ad294602f1e0dec2291152b745870afc47c1397"}, - {file = "regex-2024.4.28-cp38-cp38-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:0a2a512d623f1f2d01d881513af9fc6a7c46e5cfffb7dc50c38ce959f9246c94"}, - {file = "regex-2024.4.28-cp38-cp38-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_12_x86_64.manylinux2010_x86_64.whl", hash = "sha256:c06bf3f38f0707592898428636cbb75d0a846651b053a1cf748763e3063a6925"}, - {file = "regex-2024.4.28-cp38-cp38-musllinux_1_1_aarch64.whl", hash = "sha256:1031a5e7b048ee371ab3653aad3030ecfad6ee9ecdc85f0242c57751a05b0ac4"}, - {file = "regex-2024.4.28-cp38-cp38-musllinux_1_1_i686.whl", hash = "sha256:d7a353ebfa7154c871a35caca7bfd8f9e18666829a1dc187115b80e35a29393e"}, - {file = "regex-2024.4.28-cp38-cp38-musllinux_1_1_ppc64le.whl", hash = "sha256:7e76b9cfbf5ced1aca15a0e5b6f229344d9b3123439ffce552b11faab0114a02"}, - {file = "regex-2024.4.28-cp38-cp38-musllinux_1_1_s390x.whl", hash = "sha256:5ce479ecc068bc2a74cb98dd8dba99e070d1b2f4a8371a7dfe631f85db70fe6e"}, - {file = "regex-2024.4.28-cp38-cp38-musllinux_1_1_x86_64.whl", hash = "sha256:7d77b6f63f806578c604dca209280e4c54f0fa9a8128bb8d2cc5fb6f99da4150"}, - {file = "regex-2024.4.28-cp38-cp38-win32.whl", hash = "sha256:d84308f097d7a513359757c69707ad339da799e53b7393819ec2ea36bc4beb58"}, - {file = "regex-2024.4.28-cp38-cp38-win_amd64.whl", hash = "sha256:2cc1b87bba1dd1a898e664a31012725e48af826bf3971e786c53e32e02adae6c"}, - {file = "regex-2024.4.28-cp39-cp39-macosx_10_9_universal2.whl", hash = "sha256:7413167c507a768eafb5424413c5b2f515c606be5bb4ef8c5dee43925aa5718b"}, - {file = "regex-2024.4.28-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:108e2dcf0b53a7c4ab8986842a8edcb8ab2e59919a74ff51c296772e8e74d0ae"}, - {file = "regex-2024.4.28-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:f1c5742c31ba7d72f2dedf7968998730664b45e38827637e0f04a2ac7de2f5f1"}, - {file = "regex-2024.4.28-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:ecc6148228c9ae25ce403eade13a0961de1cb016bdb35c6eafd8e7b87ad028b1"}, - {file = "regex-2024.4.28-cp39-cp39-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:b7d893c8cf0e2429b823ef1a1d360a25950ed11f0e2a9df2b5198821832e1947"}, - {file = "regex-2024.4.28-cp39-cp39-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:4290035b169578ffbbfa50d904d26bec16a94526071ebec3dadbebf67a26b25e"}, - {file = "regex-2024.4.28-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:44a22ae1cfd82e4ffa2066eb3390777dc79468f866f0625261a93e44cdf6482b"}, - {file = "regex-2024.4.28-cp39-cp39-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:fd24fd140b69f0b0bcc9165c397e9b2e89ecbeda83303abf2a072609f60239e2"}, - {file = "regex-2024.4.28-cp39-cp39-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_12_x86_64.manylinux2010_x86_64.whl", hash = "sha256:39fb166d2196413bead229cd64a2ffd6ec78ebab83fff7d2701103cf9f4dfd26"}, - {file = "regex-2024.4.28-cp39-cp39-musllinux_1_1_aarch64.whl", hash = "sha256:9301cc6db4d83d2c0719f7fcda37229691745168bf6ae849bea2e85fc769175d"}, - {file = "regex-2024.4.28-cp39-cp39-musllinux_1_1_i686.whl", hash = "sha256:7c3d389e8d76a49923683123730c33e9553063d9041658f23897f0b396b2386f"}, - {file = "regex-2024.4.28-cp39-cp39-musllinux_1_1_ppc64le.whl", hash = "sha256:99ef6289b62042500d581170d06e17f5353b111a15aa6b25b05b91c6886df8fc"}, - {file = "regex-2024.4.28-cp39-cp39-musllinux_1_1_s390x.whl", hash = "sha256:b91d529b47798c016d4b4c1d06cc826ac40d196da54f0de3c519f5a297c5076a"}, - {file = "regex-2024.4.28-cp39-cp39-musllinux_1_1_x86_64.whl", hash = "sha256:43548ad74ea50456e1c68d3c67fff3de64c6edb85bcd511d1136f9b5376fc9d1"}, - {file = "regex-2024.4.28-cp39-cp39-win32.whl", hash = "sha256:05d9b6578a22db7dedb4df81451f360395828b04f4513980b6bd7a1412c679cc"}, - {file = "regex-2024.4.28-cp39-cp39-win_amd64.whl", hash = "sha256:3986217ec830c2109875be740531feb8ddafe0dfa49767cdcd072ed7e8927962"}, - {file = "regex-2024.4.28.tar.gz", hash = "sha256:83ab366777ea45d58f72593adf35d36ca911ea8bd838483c1823b883a121b0e4"}, + {file = "regex-2024.5.15-cp310-cp310-macosx_10_9_universal2.whl", hash = "sha256:a81e3cfbae20378d75185171587cbf756015ccb14840702944f014e0d93ea09f"}, + {file = "regex-2024.5.15-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:7b59138b219ffa8979013be7bc85bb60c6f7b7575df3d56dc1e403a438c7a3f6"}, + {file = "regex-2024.5.15-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:a0bd000c6e266927cb7a1bc39d55be95c4b4f65c5be53e659537537e019232b1"}, + {file = "regex-2024.5.15-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:5eaa7ddaf517aa095fa8da0b5015c44d03da83f5bd49c87961e3c997daed0de7"}, + {file = "regex-2024.5.15-cp310-cp310-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:ba68168daedb2c0bab7fd7e00ced5ba90aebf91024dea3c88ad5063c2a562cca"}, + {file = "regex-2024.5.15-cp310-cp310-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:6e8d717bca3a6e2064fc3a08df5cbe366369f4b052dcd21b7416e6d71620dca1"}, + {file = "regex-2024.5.15-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:1337b7dbef9b2f71121cdbf1e97e40de33ff114801263b275aafd75303bd62b5"}, + {file = "regex-2024.5.15-cp310-cp310-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:f9ebd0a36102fcad2f03696e8af4ae682793a5d30b46c647eaf280d6cfb32796"}, + {file = "regex-2024.5.15-cp310-cp310-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_12_x86_64.manylinux2010_x86_64.whl", hash = "sha256:9efa1a32ad3a3ea112224897cdaeb6aa00381627f567179c0314f7b65d354c62"}, + {file = "regex-2024.5.15-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:1595f2d10dff3d805e054ebdc41c124753631b6a471b976963c7b28543cf13b0"}, + {file = "regex-2024.5.15-cp310-cp310-musllinux_1_2_i686.whl", hash = "sha256:b802512f3e1f480f41ab5f2cfc0e2f761f08a1f41092d6718868082fc0d27143"}, + {file = "regex-2024.5.15-cp310-cp310-musllinux_1_2_ppc64le.whl", hash = "sha256:a0981022dccabca811e8171f913de05720590c915b033b7e601f35ce4ea7019f"}, + {file = "regex-2024.5.15-cp310-cp310-musllinux_1_2_s390x.whl", hash = "sha256:19068a6a79cf99a19ccefa44610491e9ca02c2be3305c7760d3831d38a467a6f"}, + {file = "regex-2024.5.15-cp310-cp310-musllinux_1_2_x86_64.whl", hash = "sha256:1b5269484f6126eee5e687785e83c6b60aad7663dafe842b34691157e5083e53"}, + {file = "regex-2024.5.15-cp310-cp310-win32.whl", hash = "sha256:ada150c5adfa8fbcbf321c30c751dc67d2f12f15bd183ffe4ec7cde351d945b3"}, + {file = "regex-2024.5.15-cp310-cp310-win_amd64.whl", hash = "sha256:ac394ff680fc46b97487941f5e6ae49a9f30ea41c6c6804832063f14b2a5a145"}, + {file = "regex-2024.5.15-cp311-cp311-macosx_10_9_universal2.whl", hash = "sha256:f5b1dff3ad008dccf18e652283f5e5339d70bf8ba7c98bf848ac33db10f7bc7a"}, + {file = "regex-2024.5.15-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:c6a2b494a76983df8e3d3feea9b9ffdd558b247e60b92f877f93a1ff43d26656"}, + {file = "regex-2024.5.15-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:a32b96f15c8ab2e7d27655969a23895eb799de3665fa94349f3b2fbfd547236f"}, + {file = "regex-2024.5.15-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:10002e86e6068d9e1c91eae8295ef690f02f913c57db120b58fdd35a6bb1af35"}, + {file = "regex-2024.5.15-cp311-cp311-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:ec54d5afa89c19c6dd8541a133be51ee1017a38b412b1321ccb8d6ddbeb4cf7d"}, + {file = "regex-2024.5.15-cp311-cp311-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:10e4ce0dca9ae7a66e6089bb29355d4432caed736acae36fef0fdd7879f0b0cb"}, + {file = "regex-2024.5.15-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:3e507ff1e74373c4d3038195fdd2af30d297b4f0950eeda6f515ae3d84a1770f"}, + {file = "regex-2024.5.15-cp311-cp311-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:d1f059a4d795e646e1c37665b9d06062c62d0e8cc3c511fe01315973a6542e40"}, + {file = "regex-2024.5.15-cp311-cp311-musllinux_1_2_aarch64.whl", hash = "sha256:0721931ad5fe0dda45d07f9820b90b2148ccdd8e45bb9e9b42a146cb4f695649"}, + {file = "regex-2024.5.15-cp311-cp311-musllinux_1_2_i686.whl", hash = "sha256:833616ddc75ad595dee848ad984d067f2f31be645d603e4d158bba656bbf516c"}, + {file = "regex-2024.5.15-cp311-cp311-musllinux_1_2_ppc64le.whl", hash = "sha256:287eb7f54fc81546346207c533ad3c2c51a8d61075127d7f6d79aaf96cdee890"}, + {file = "regex-2024.5.15-cp311-cp311-musllinux_1_2_s390x.whl", hash = "sha256:19dfb1c504781a136a80ecd1fff9f16dddf5bb43cec6871778c8a907a085bb3d"}, + {file = "regex-2024.5.15-cp311-cp311-musllinux_1_2_x86_64.whl", hash = "sha256:119af6e56dce35e8dfb5222573b50c89e5508d94d55713c75126b753f834de68"}, + {file = "regex-2024.5.15-cp311-cp311-win32.whl", hash = "sha256:1c1c174d6ec38d6c8a7504087358ce9213d4332f6293a94fbf5249992ba54efa"}, + {file = "regex-2024.5.15-cp311-cp311-win_amd64.whl", hash = "sha256:9e717956dcfd656f5055cc70996ee2cc82ac5149517fc8e1b60261b907740201"}, + {file = "regex-2024.5.15-cp312-cp312-macosx_10_9_universal2.whl", hash = "sha256:632b01153e5248c134007209b5c6348a544ce96c46005d8456de1d552455b014"}, + {file = "regex-2024.5.15-cp312-cp312-macosx_10_9_x86_64.whl", hash = "sha256:e64198f6b856d48192bf921421fdd8ad8eb35e179086e99e99f711957ffedd6e"}, + {file = "regex-2024.5.15-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:68811ab14087b2f6e0fc0c2bae9ad689ea3584cad6917fc57be6a48bbd012c49"}, + {file = "regex-2024.5.15-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:f8ec0c2fea1e886a19c3bee0cd19d862b3aa75dcdfb42ebe8ed30708df64687a"}, + {file = "regex-2024.5.15-cp312-cp312-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:d0c0c0003c10f54a591d220997dd27d953cd9ccc1a7294b40a4be5312be8797b"}, + {file = "regex-2024.5.15-cp312-cp312-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:2431b9e263af1953c55abbd3e2efca67ca80a3de8a0437cb58e2421f8184717a"}, + {file = "regex-2024.5.15-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:4a605586358893b483976cffc1723fb0f83e526e8f14c6e6614e75919d9862cf"}, + {file = "regex-2024.5.15-cp312-cp312-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:391d7f7f1e409d192dba8bcd42d3e4cf9e598f3979cdaed6ab11288da88cb9f2"}, + {file = "regex-2024.5.15-cp312-cp312-musllinux_1_2_aarch64.whl", hash = "sha256:9ff11639a8d98969c863d4617595eb5425fd12f7c5ef6621a4b74b71ed8726d5"}, + {file = "regex-2024.5.15-cp312-cp312-musllinux_1_2_i686.whl", hash = "sha256:4eee78a04e6c67e8391edd4dad3279828dd66ac4b79570ec998e2155d2e59fd5"}, + {file = "regex-2024.5.15-cp312-cp312-musllinux_1_2_ppc64le.whl", hash = "sha256:8fe45aa3f4aa57faabbc9cb46a93363edd6197cbc43523daea044e9ff2fea83e"}, + {file = "regex-2024.5.15-cp312-cp312-musllinux_1_2_s390x.whl", hash = "sha256:d0a3d8d6acf0c78a1fff0e210d224b821081330b8524e3e2bc5a68ef6ab5803d"}, + {file = "regex-2024.5.15-cp312-cp312-musllinux_1_2_x86_64.whl", hash = "sha256:c486b4106066d502495b3025a0a7251bf37ea9540433940a23419461ab9f2a80"}, + {file = "regex-2024.5.15-cp312-cp312-win32.whl", hash = "sha256:c49e15eac7c149f3670b3e27f1f28a2c1ddeccd3a2812cba953e01be2ab9b5fe"}, + {file = "regex-2024.5.15-cp312-cp312-win_amd64.whl", hash = "sha256:673b5a6da4557b975c6c90198588181029c60793835ce02f497ea817ff647cb2"}, + {file = "regex-2024.5.15-cp38-cp38-macosx_10_9_universal2.whl", hash = "sha256:87e2a9c29e672fc65523fb47a90d429b70ef72b901b4e4b1bd42387caf0d6835"}, + {file = "regex-2024.5.15-cp38-cp38-macosx_10_9_x86_64.whl", hash = "sha256:c3bea0ba8b73b71b37ac833a7f3fd53825924165da6a924aec78c13032f20850"}, + {file = "regex-2024.5.15-cp38-cp38-macosx_11_0_arm64.whl", hash = "sha256:bfc4f82cabe54f1e7f206fd3d30fda143f84a63fe7d64a81558d6e5f2e5aaba9"}, + {file = "regex-2024.5.15-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:e5bb9425fe881d578aeca0b2b4b3d314ec88738706f66f219c194d67179337cb"}, + {file = "regex-2024.5.15-cp38-cp38-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:64c65783e96e563103d641760664125e91bd85d8e49566ee560ded4da0d3e704"}, + {file = "regex-2024.5.15-cp38-cp38-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:cf2430df4148b08fb4324b848672514b1385ae3807651f3567871f130a728cc3"}, + {file = "regex-2024.5.15-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:5397de3219a8b08ae9540c48f602996aa6b0b65d5a61683e233af8605c42b0f2"}, + {file = "regex-2024.5.15-cp38-cp38-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:455705d34b4154a80ead722f4f185b04c4237e8e8e33f265cd0798d0e44825fa"}, + {file = "regex-2024.5.15-cp38-cp38-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_12_x86_64.manylinux2010_x86_64.whl", hash = "sha256:b2b6f1b3bb6f640c1a92be3bbfbcb18657b125b99ecf141fb3310b5282c7d4ed"}, + {file = "regex-2024.5.15-cp38-cp38-musllinux_1_2_aarch64.whl", hash = "sha256:3ad070b823ca5890cab606c940522d05d3d22395d432f4aaaf9d5b1653e47ced"}, + {file = "regex-2024.5.15-cp38-cp38-musllinux_1_2_i686.whl", hash = "sha256:5b5467acbfc153847d5adb21e21e29847bcb5870e65c94c9206d20eb4e99a384"}, + {file = "regex-2024.5.15-cp38-cp38-musllinux_1_2_ppc64le.whl", hash = "sha256:e6662686aeb633ad65be2a42b4cb00178b3fbf7b91878f9446075c404ada552f"}, + {file = "regex-2024.5.15-cp38-cp38-musllinux_1_2_s390x.whl", hash = "sha256:2b4c884767504c0e2401babe8b5b7aea9148680d2e157fa28f01529d1f7fcf67"}, + {file = "regex-2024.5.15-cp38-cp38-musllinux_1_2_x86_64.whl", hash = "sha256:3cd7874d57f13bf70078f1ff02b8b0aa48d5b9ed25fc48547516c6aba36f5741"}, + {file = "regex-2024.5.15-cp38-cp38-win32.whl", hash = "sha256:e4682f5ba31f475d58884045c1a97a860a007d44938c4c0895f41d64481edbc9"}, + {file = "regex-2024.5.15-cp38-cp38-win_amd64.whl", hash = "sha256:d99ceffa25ac45d150e30bd9ed14ec6039f2aad0ffa6bb87a5936f5782fc1569"}, + {file = "regex-2024.5.15-cp39-cp39-macosx_10_9_universal2.whl", hash = "sha256:13cdaf31bed30a1e1c2453ef6015aa0983e1366fad2667657dbcac7b02f67133"}, + {file = "regex-2024.5.15-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:cac27dcaa821ca271855a32188aa61d12decb6fe45ffe3e722401fe61e323cd1"}, + {file = "regex-2024.5.15-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:7dbe2467273b875ea2de38ded4eba86cbcbc9a1a6d0aa11dcf7bd2e67859c435"}, + {file = "regex-2024.5.15-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:64f18a9a3513a99c4bef0e3efd4c4a5b11228b48aa80743be822b71e132ae4f5"}, + {file = "regex-2024.5.15-cp39-cp39-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:d347a741ea871c2e278fde6c48f85136c96b8659b632fb57a7d1ce1872547600"}, + {file = "regex-2024.5.15-cp39-cp39-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:1878b8301ed011704aea4c806a3cadbd76f84dece1ec09cc9e4dc934cfa5d4da"}, + {file = "regex-2024.5.15-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:4babf07ad476aaf7830d77000874d7611704a7fcf68c9c2ad151f5d94ae4bfc4"}, + {file = "regex-2024.5.15-cp39-cp39-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:35cb514e137cb3488bce23352af3e12fb0dbedd1ee6e60da053c69fb1b29cc6c"}, + {file = "regex-2024.5.15-cp39-cp39-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_12_x86_64.manylinux2010_x86_64.whl", hash = "sha256:cdd09d47c0b2efee9378679f8510ee6955d329424c659ab3c5e3a6edea696294"}, + {file = "regex-2024.5.15-cp39-cp39-musllinux_1_2_aarch64.whl", hash = "sha256:72d7a99cd6b8f958e85fc6ca5b37c4303294954eac1376535b03c2a43eb72629"}, + {file = "regex-2024.5.15-cp39-cp39-musllinux_1_2_i686.whl", hash = "sha256:a094801d379ab20c2135529948cb84d417a2169b9bdceda2a36f5f10977ebc16"}, + {file = "regex-2024.5.15-cp39-cp39-musllinux_1_2_ppc64le.whl", hash = "sha256:c0c18345010870e58238790a6779a1219b4d97bd2e77e1140e8ee5d14df071aa"}, + {file = "regex-2024.5.15-cp39-cp39-musllinux_1_2_s390x.whl", hash = "sha256:16093f563098448ff6b1fa68170e4acbef94e6b6a4e25e10eae8598bb1694b5d"}, + {file = "regex-2024.5.15-cp39-cp39-musllinux_1_2_x86_64.whl", hash = "sha256:e38a7d4e8f633a33b4c7350fbd8bad3b70bf81439ac67ac38916c4a86b465456"}, + {file = "regex-2024.5.15-cp39-cp39-win32.whl", hash = "sha256:71a455a3c584a88f654b64feccc1e25876066c4f5ef26cd6dd711308aa538694"}, + {file = "regex-2024.5.15-cp39-cp39-win_amd64.whl", hash = "sha256:cab12877a9bdafde5500206d1020a584355a97884dfd388af3699e9137bf7388"}, + {file = "regex-2024.5.15.tar.gz", hash = "sha256:d3ee02d9e5f482cc8309134a91eeaacbdd2261ba111b0fef3748eeb4913e6a2c"}, ] [[package]] name = "requests" -version = "2.32.0" +version = "2.32.3" description = "Python HTTP for Humans." optional = false python-versions = ">=3.8" files = [ - {file = "requests-2.32.0-py3-none-any.whl", hash = "sha256:f2c3881dddb70d056c5bd7600a4fae312b2a300e39be6a118d30b90bd27262b5"}, - {file = "requests-2.32.0.tar.gz", hash = "sha256:fa5490319474c82ef1d2c9bc459d3652e3ae4ef4c4ebdd18a21145a47ca4b6b8"}, + {file = "requests-2.32.3-py3-none-any.whl", hash = "sha256:70761cfe03c773ceb22aa2f671b4757976145175cdfca038c02654d061d6dcc6"}, + {file = "requests-2.32.3.tar.gz", hash = "sha256:55365417734eb18255590a9ff9eb97e9e1da868d4ccd6402399eaf68af20a760"}, ] [package.dependencies] @@ -888,19 +888,18 @@ use-chardet-on-py3 = ["chardet (>=3.0.2,<6)"] [[package]] name = "setuptools" -version = "69.5.1" +version = "70.1.0" description = "Easily download, build, install, upgrade, and uninstall Python packages" optional = false python-versions = ">=3.8" files = [ - {file = "setuptools-69.5.1-py3-none-any.whl", hash = "sha256:c636ac361bc47580504644275c9ad802c50415c7522212252c033bd15f301f32"}, - {file = "setuptools-69.5.1.tar.gz", hash = "sha256:6c1fccdac05a97e598fb0ae3bbed5904ccb317337a51139dcd51453611bbb987"}, + {file = "setuptools-70.1.0-py3-none-any.whl", hash = "sha256:d9b8b771455a97c8a9f3ab3448ebe0b29b5e105f1228bba41028be116985a267"}, + {file = "setuptools-70.1.0.tar.gz", hash = "sha256:01a1e793faa5bd89abc851fa15d0a0db26f160890c7102cd8dce643e886b47f5"}, ] [package.extras] -docs = ["furo", "jaraco.packaging (>=9.3)", "jaraco.tidelift (>=1.4)", "pygments-github-lexers (==0.0.5)", "rst.linker (>=1.9)", "sphinx (>=3.5)", "sphinx-favicon", "sphinx-inline-tabs", "sphinx-lint", "sphinx-notfound-page (>=1,<2)", "sphinx-reredirects", "sphinxcontrib-towncrier"] -testing = ["build[virtualenv]", "filelock (>=3.4.0)", "importlib-metadata", "ini2toml[lite] (>=0.9)", "jaraco.develop (>=7.21)", "jaraco.envs (>=2.2)", "jaraco.path (>=3.2.0)", "mypy (==1.9)", "packaging (>=23.2)", "pip (>=19.1)", "pytest (>=6,!=8.1.1)", "pytest-checkdocs (>=2.4)", "pytest-cov", "pytest-enabler (>=2.2)", "pytest-home (>=0.5)", "pytest-mypy", "pytest-perf", "pytest-ruff (>=0.2.1)", "pytest-timeout", "pytest-xdist (>=3)", "tomli", "tomli-w (>=1.0.0)", "virtualenv (>=13.0.0)", "wheel"] -testing-integration = ["build[virtualenv] (>=1.0.3)", "filelock (>=3.4.0)", "jaraco.envs (>=2.2)", "jaraco.path (>=3.2.0)", "packaging (>=23.2)", "pytest", "pytest-enabler", "pytest-xdist", "tomli", "virtualenv (>=13.0.0)", "wheel"] +docs = ["furo", "jaraco.packaging (>=9.3)", "jaraco.tidelift (>=1.4)", "pygments-github-lexers (==0.0.5)", "pyproject-hooks (!=1.1)", "rst.linker (>=1.9)", "sphinx (>=3.5)", "sphinx-favicon", "sphinx-inline-tabs", "sphinx-lint", "sphinx-notfound-page (>=1,<2)", "sphinx-reredirects", "sphinxcontrib-towncrier"] +testing = ["build[virtualenv] (>=1.0.3)", "filelock (>=3.4.0)", "importlib-metadata", "ini2toml[lite] (>=0.14)", "jaraco.develop (>=7.21)", "jaraco.envs (>=2.2)", "jaraco.path (>=3.2.0)", "jaraco.test", "mypy (==1.10.0)", "packaging (>=23.2)", "pip (>=19.1)", "pyproject-hooks (!=1.1)", "pytest (>=6,!=8.1.1)", "pytest-checkdocs (>=2.4)", "pytest-cov", "pytest-enabler (>=2.2)", "pytest-home (>=0.5)", "pytest-mypy", "pytest-perf", "pytest-ruff (>=0.3.2)", "pytest-subprocess", "pytest-timeout", "pytest-xdist (>=3)", "tomli", "tomli-w (>=1.0.0)", "virtualenv (>=13.0.0)", "wheel"] [[package]] name = "six" @@ -943,40 +942,43 @@ zstd = ["zstandard (>=0.18.0)"] [[package]] name = "watchdog" -version = "4.0.0" +version = "4.0.1" description = "Filesystem events monitoring" optional = false python-versions = ">=3.8" files = [ - {file = "watchdog-4.0.0-cp310-cp310-macosx_10_9_universal2.whl", hash = "sha256:39cb34b1f1afbf23e9562501673e7146777efe95da24fab5707b88f7fb11649b"}, - {file = "watchdog-4.0.0-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:c522392acc5e962bcac3b22b9592493ffd06d1fc5d755954e6be9f4990de932b"}, - {file = "watchdog-4.0.0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:6c47bdd680009b11c9ac382163e05ca43baf4127954c5f6d0250e7d772d2b80c"}, - {file = "watchdog-4.0.0-cp311-cp311-macosx_10_9_universal2.whl", hash = "sha256:8350d4055505412a426b6ad8c521bc7d367d1637a762c70fdd93a3a0d595990b"}, - {file = "watchdog-4.0.0-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:c17d98799f32e3f55f181f19dd2021d762eb38fdd381b4a748b9f5a36738e935"}, - {file = "watchdog-4.0.0-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:4986db5e8880b0e6b7cd52ba36255d4793bf5cdc95bd6264806c233173b1ec0b"}, - {file = "watchdog-4.0.0-cp312-cp312-macosx_10_9_universal2.whl", hash = "sha256:11e12fafb13372e18ca1bbf12d50f593e7280646687463dd47730fd4f4d5d257"}, - {file = "watchdog-4.0.0-cp312-cp312-macosx_10_9_x86_64.whl", hash = "sha256:5369136a6474678e02426bd984466343924d1df8e2fd94a9b443cb7e3aa20d19"}, - {file = "watchdog-4.0.0-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:76ad8484379695f3fe46228962017a7e1337e9acadafed67eb20aabb175df98b"}, - {file = "watchdog-4.0.0-cp38-cp38-macosx_10_9_universal2.whl", hash = "sha256:45cc09cc4c3b43fb10b59ef4d07318d9a3ecdbff03abd2e36e77b6dd9f9a5c85"}, - {file = "watchdog-4.0.0-cp38-cp38-macosx_10_9_x86_64.whl", hash = "sha256:eed82cdf79cd7f0232e2fdc1ad05b06a5e102a43e331f7d041e5f0e0a34a51c4"}, - {file = "watchdog-4.0.0-cp38-cp38-macosx_11_0_arm64.whl", hash = "sha256:ba30a896166f0fee83183cec913298151b73164160d965af2e93a20bbd2ab605"}, - {file = "watchdog-4.0.0-cp39-cp39-macosx_10_9_universal2.whl", hash = "sha256:d18d7f18a47de6863cd480734613502904611730f8def45fc52a5d97503e5101"}, - {file = "watchdog-4.0.0-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:2895bf0518361a9728773083908801a376743bcc37dfa252b801af8fd281b1ca"}, - {file = "watchdog-4.0.0-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:87e9df830022488e235dd601478c15ad73a0389628588ba0b028cb74eb72fed8"}, - {file = "watchdog-4.0.0-pp310-pypy310_pp73-macosx_10_9_x86_64.whl", hash = "sha256:6e949a8a94186bced05b6508faa61b7adacc911115664ccb1923b9ad1f1ccf7b"}, - {file = "watchdog-4.0.0-pp38-pypy38_pp73-macosx_10_9_x86_64.whl", hash = "sha256:6a4db54edea37d1058b08947c789a2354ee02972ed5d1e0dca9b0b820f4c7f92"}, - {file = "watchdog-4.0.0-pp39-pypy39_pp73-macosx_10_9_x86_64.whl", hash = "sha256:d31481ccf4694a8416b681544c23bd271f5a123162ab603c7d7d2dd7dd901a07"}, - {file = "watchdog-4.0.0-py3-none-manylinux2014_aarch64.whl", hash = "sha256:8fec441f5adcf81dd240a5fe78e3d83767999771630b5ddfc5867827a34fa3d3"}, - {file = "watchdog-4.0.0-py3-none-manylinux2014_armv7l.whl", hash = "sha256:6a9c71a0b02985b4b0b6d14b875a6c86ddea2fdbebd0c9a720a806a8bbffc69f"}, - {file = "watchdog-4.0.0-py3-none-manylinux2014_i686.whl", hash = "sha256:557ba04c816d23ce98a06e70af6abaa0485f6d94994ec78a42b05d1c03dcbd50"}, - {file = "watchdog-4.0.0-py3-none-manylinux2014_ppc64.whl", hash = "sha256:d0f9bd1fd919134d459d8abf954f63886745f4660ef66480b9d753a7c9d40927"}, - {file = "watchdog-4.0.0-py3-none-manylinux2014_ppc64le.whl", hash = "sha256:f9b2fdca47dc855516b2d66eef3c39f2672cbf7e7a42e7e67ad2cbfcd6ba107d"}, - {file = "watchdog-4.0.0-py3-none-manylinux2014_s390x.whl", hash = "sha256:73c7a935e62033bd5e8f0da33a4dcb763da2361921a69a5a95aaf6c93aa03a87"}, - {file = "watchdog-4.0.0-py3-none-manylinux2014_x86_64.whl", hash = "sha256:6a80d5cae8c265842c7419c560b9961561556c4361b297b4c431903f8c33b269"}, - {file = "watchdog-4.0.0-py3-none-win32.whl", hash = "sha256:8f9a542c979df62098ae9c58b19e03ad3df1c9d8c6895d96c0d51da17b243b1c"}, - {file = "watchdog-4.0.0-py3-none-win_amd64.whl", hash = "sha256:f970663fa4f7e80401a7b0cbeec00fa801bf0287d93d48368fc3e6fa32716245"}, - {file = "watchdog-4.0.0-py3-none-win_ia64.whl", hash = "sha256:9a03e16e55465177d416699331b0f3564138f1807ecc5f2de9d55d8f188d08c7"}, - {file = "watchdog-4.0.0.tar.gz", hash = "sha256:e3e7065cbdabe6183ab82199d7a4f6b3ba0a438c5a512a68559846ccb76a78ec"}, + {file = "watchdog-4.0.1-cp310-cp310-macosx_10_9_universal2.whl", hash = "sha256:da2dfdaa8006eb6a71051795856bedd97e5b03e57da96f98e375682c48850645"}, + {file = "watchdog-4.0.1-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:e93f451f2dfa433d97765ca2634628b789b49ba8b504fdde5837cdcf25fdb53b"}, + {file = "watchdog-4.0.1-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:ef0107bbb6a55f5be727cfc2ef945d5676b97bffb8425650dadbb184be9f9a2b"}, + {file = "watchdog-4.0.1-cp311-cp311-macosx_10_9_universal2.whl", hash = "sha256:17e32f147d8bf9657e0922c0940bcde863b894cd871dbb694beb6704cfbd2fb5"}, + {file = "watchdog-4.0.1-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:03e70d2df2258fb6cb0e95bbdbe06c16e608af94a3ffbd2b90c3f1e83eb10767"}, + {file = "watchdog-4.0.1-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:123587af84260c991dc5f62a6e7ef3d1c57dfddc99faacee508c71d287248459"}, + {file = "watchdog-4.0.1-cp312-cp312-macosx_10_9_universal2.whl", hash = "sha256:093b23e6906a8b97051191a4a0c73a77ecc958121d42346274c6af6520dec175"}, + {file = "watchdog-4.0.1-cp312-cp312-macosx_10_9_x86_64.whl", hash = "sha256:611be3904f9843f0529c35a3ff3fd617449463cb4b73b1633950b3d97fa4bfb7"}, + {file = "watchdog-4.0.1-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:62c613ad689ddcb11707f030e722fa929f322ef7e4f18f5335d2b73c61a85c28"}, + {file = "watchdog-4.0.1-cp38-cp38-macosx_10_9_universal2.whl", hash = "sha256:d4925e4bf7b9bddd1c3de13c9b8a2cdb89a468f640e66fbfabaf735bd85b3e35"}, + {file = "watchdog-4.0.1-cp38-cp38-macosx_10_9_x86_64.whl", hash = "sha256:cad0bbd66cd59fc474b4a4376bc5ac3fc698723510cbb64091c2a793b18654db"}, + {file = "watchdog-4.0.1-cp38-cp38-macosx_11_0_arm64.whl", hash = "sha256:a3c2c317a8fb53e5b3d25790553796105501a235343f5d2bf23bb8649c2c8709"}, + {file = "watchdog-4.0.1-cp39-cp39-macosx_10_9_universal2.whl", hash = "sha256:c9904904b6564d4ee8a1ed820db76185a3c96e05560c776c79a6ce5ab71888ba"}, + {file = "watchdog-4.0.1-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:667f3c579e813fcbad1b784db7a1aaa96524bed53437e119f6a2f5de4db04235"}, + {file = "watchdog-4.0.1-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:d10a681c9a1d5a77e75c48a3b8e1a9f2ae2928eda463e8d33660437705659682"}, + {file = "watchdog-4.0.1-pp310-pypy310_pp73-macosx_10_9_x86_64.whl", hash = "sha256:0144c0ea9997b92615af1d94afc0c217e07ce2c14912c7b1a5731776329fcfc7"}, + {file = "watchdog-4.0.1-pp310-pypy310_pp73-macosx_11_0_arm64.whl", hash = "sha256:998d2be6976a0ee3a81fb8e2777900c28641fb5bfbd0c84717d89bca0addcdc5"}, + {file = "watchdog-4.0.1-pp38-pypy38_pp73-macosx_10_9_x86_64.whl", hash = "sha256:e7921319fe4430b11278d924ef66d4daa469fafb1da679a2e48c935fa27af193"}, + {file = "watchdog-4.0.1-pp38-pypy38_pp73-macosx_11_0_arm64.whl", hash = "sha256:f0de0f284248ab40188f23380b03b59126d1479cd59940f2a34f8852db710625"}, + {file = "watchdog-4.0.1-pp39-pypy39_pp73-macosx_10_9_x86_64.whl", hash = "sha256:bca36be5707e81b9e6ce3208d92d95540d4ca244c006b61511753583c81c70dd"}, + {file = "watchdog-4.0.1-pp39-pypy39_pp73-macosx_11_0_arm64.whl", hash = "sha256:ab998f567ebdf6b1da7dc1e5accfaa7c6992244629c0fdaef062f43249bd8dee"}, + {file = "watchdog-4.0.1-py3-none-manylinux2014_aarch64.whl", hash = "sha256:dddba7ca1c807045323b6af4ff80f5ddc4d654c8bce8317dde1bd96b128ed253"}, + {file = "watchdog-4.0.1-py3-none-manylinux2014_armv7l.whl", hash = "sha256:4513ec234c68b14d4161440e07f995f231be21a09329051e67a2118a7a612d2d"}, + {file = "watchdog-4.0.1-py3-none-manylinux2014_i686.whl", hash = "sha256:4107ac5ab936a63952dea2a46a734a23230aa2f6f9db1291bf171dac3ebd53c6"}, + {file = "watchdog-4.0.1-py3-none-manylinux2014_ppc64.whl", hash = "sha256:6e8c70d2cd745daec2a08734d9f63092b793ad97612470a0ee4cbb8f5f705c57"}, + {file = "watchdog-4.0.1-py3-none-manylinux2014_ppc64le.whl", hash = "sha256:f27279d060e2ab24c0aa98363ff906d2386aa6c4dc2f1a374655d4e02a6c5e5e"}, + {file = "watchdog-4.0.1-py3-none-manylinux2014_s390x.whl", hash = "sha256:f8affdf3c0f0466e69f5b3917cdd042f89c8c63aebdb9f7c078996f607cdb0f5"}, + {file = "watchdog-4.0.1-py3-none-manylinux2014_x86_64.whl", hash = "sha256:ac7041b385f04c047fcc2951dc001671dee1b7e0615cde772e84b01fbf68ee84"}, + {file = "watchdog-4.0.1-py3-none-win32.whl", hash = "sha256:206afc3d964f9a233e6ad34618ec60b9837d0582b500b63687e34011e15bb429"}, + {file = "watchdog-4.0.1-py3-none-win_amd64.whl", hash = "sha256:7577b3c43e5909623149f76b099ac49a1a01ca4e167d1785c76eb52fa585745a"}, + {file = "watchdog-4.0.1-py3-none-win_ia64.whl", hash = "sha256:d7b9f5f3299e8dd230880b6c55504a1f69cf1e4316275d1b215ebdd8187ec88d"}, + {file = "watchdog-4.0.1.tar.gz", hash = "sha256:eebaacf674fa25511e8867028d281e602ee6500045b57f43b08778082f7f8b44"}, ] [package.extras] @@ -984,13 +986,13 @@ watchmedo = ["PyYAML (>=3.10)"] [[package]] name = "wcmatch" -version = "8.5.1" +version = "8.5.2" description = "Wildcard/glob file name matcher." optional = false python-versions = ">=3.8" files = [ - {file = "wcmatch-8.5.1-py3-none-any.whl", hash = "sha256:24c19cedc92bc9c9e27f39db4e1824d72f95bd2cea32b254a47a45b1a1b227ed"}, - {file = "wcmatch-8.5.1.tar.gz", hash = "sha256:c0088c7f6426cf6bf27e530e2b7b734031905f7e490475fd83c7c5008ab581b3"}, + {file = "wcmatch-8.5.2-py3-none-any.whl", hash = "sha256:17d3ad3758f9d0b5b4dedc770b65420d4dac62e680229c287bf24c9db856a478"}, + {file = "wcmatch-8.5.2.tar.gz", hash = "sha256:a70222b86dea82fb382dd87b73278c10756c138bd6f8f714e2183128887b9eb2"}, ] [package.dependencies] From 9602a5b8e2c0db092a36c368e7754c3ee769676c Mon Sep 17 00:00:00 2001 From: Release Bot Date: Mon, 24 Jun 2024 05:22:55 +0000 Subject: [PATCH 110/118] Bump version of scenario_simulator_v2 from version 2.1.8 to version 2.1.9 --- common/math/arithmetic/CHANGELOG.rst | 3 +++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 3 +++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 3 +++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 3 +++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 3 +++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 3 +++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 3 +++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 3 +++ map/kashiwanoha_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 3 +++ mock/cpp_mock_scenarios/package.xml | 2 +- openscenario/openscenario_experimental_catalog/CHANGELOG.rst | 3 +++ openscenario/openscenario_experimental_catalog/package.xml | 2 +- openscenario/openscenario_interpreter/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter/package.xml | 2 +- openscenario/openscenario_interpreter_example/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter_example/package.xml | 2 +- openscenario/openscenario_interpreter_msgs/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter_msgs/package.xml | 2 +- openscenario/openscenario_preprocessor/CHANGELOG.rst | 3 +++ openscenario/openscenario_preprocessor/package.xml | 2 +- openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst | 3 +++ openscenario/openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 3 +++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 3 +++ openscenario/openscenario_validator/package.xml | 2 +- rviz_plugins/openscenario_visualization/CHANGELOG.rst | 3 +++ rviz_plugins/openscenario_visualization/package.xml | 2 +- .../real_time_factor_control_rviz_plugin/CHANGELOG.rst | 3 +++ rviz_plugins/real_time_factor_control_rviz_plugin/package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 3 +++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 3 +++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 3 +++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 3 +++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 3 +++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 3 +++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 3 +++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 3 +++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 3 +++ test_runner/scenario_test_runner/package.xml | 2 +- 56 files changed, 112 insertions(+), 28 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 11e1fa3a014..8b2a1acb162 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package arithmetic * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 0fba5c0bbbe..014eaddb577 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 2.1.8 + 2.1.9 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index e5eeb0d64f1..861956aca48 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package geometry * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index ed5422bd89f..5a92dd40cd1 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 2.1.8 + 2.1.9 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 1780d2722a2..5df4532cc92 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package scenario_simulator_exception * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index ddffee8182d..a8136386167 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 2.1.8 + 2.1.9 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index 8747dd30cc0..4cded42f615 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package junit_exporter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 82ae848e777..60d856ac156 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 2.1.8 + 2.1.9 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index d6edadcb875..95934d7ad3a 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package status_monitor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 8c225fe70b0..a9535b98d91 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 2.1.8 + 2.1.9 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 6744fbee224..a4357170344 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package concealer * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 18241c6c15d..001393c4f2c 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 2.1.8 + 2.1.9 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index b491555134b..270ad085379 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -16,6 +16,9 @@ Changelog for package embree_vendor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 4ac0edb6620..3e94e6e4d14 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 2.1.8 + 2.1.9 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 05bf18e7d9e..c3b390b4653 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package kashiwanoha_map * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index cf2314b7c9b..e7e4574bc4c 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 2.1.8 + 2.1.9 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index 2b307f81cb4..2714fb5de74 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package cpp_mock_scenarios * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 0a20573a470..ef9955bd868 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 2.1.8 + 2.1.9 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 5e47590cb52..9c7cfa3fb31 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_experimental_catalog * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 395c678d5f4..fccf8b4cf8e 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 2.1.8 + 2.1.9 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 1b2d50612c9..3085bc4f052 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -14,6 +14,9 @@ Changelog for package openscenario_interpreter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index e3ae867a80f..b29745c3730 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 2.1.8 + 2.1.9 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 5ecf3d386cb..741c07d3aae 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_interpreter_example * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index ccd1ec9fe07..7d004d78821 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 2.1.8 + 2.1.9 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index 303155de75f..3778bb70424 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_interpreter_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 25525f2821c..9a74c40a52e 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 2.1.8 + 2.1.9 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index d343134493a..b71d1185099 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_preprocessor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 7c88b485345..b52e8e2c0a7 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 2.1.8 + 2.1.9 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index cde933c0d81..fc3e96336e7 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_preprocessor_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index 4cb164009ec..4ec52b8230b 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 2.1.8 + 2.1.9 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 8b5624a5b82..2d1fbf268b8 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -16,6 +16,9 @@ Changelog for package openscenario_utility * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index f4eabc98759..0e25156becb 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 2.1.8 + 2.1.9 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index cc68e668841..0f6f04675ea 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package openscenario_validator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 77f15a0c906..ecf9231c984 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 2.1.8 + 2.1.9 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 5cfc0e50646..9328de804b7 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_visualization * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index ca828bcbda2..d593f34b541 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 2.1.8 + 2.1.9 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 97a6e5df448..7f5795f98bd 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index 70cdf29b301..859bac84c27 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 2.1.8 + 2.1.9 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index da730858713..cfe7fad9ca7 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package scenario_simulator_v2 * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 204ae5d70d7..51e3782b901 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 2.1.8 + 2.1.9 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index da4f5cf2edb..544152df39d 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package behavior_tree_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index df1b803add3..041d46068ef 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 2.1.8 + 2.1.9 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index d66d557ee80..c8de7e0352f 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package do_nothing_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index ad3512650fd..517e15c193b 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 2.1.8 + 2.1.9 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 210c79a9392..97b129cb23e 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package simple_sensor_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge pull request `#1291 `_ from tier4/feature/simple_sensor_simulator_unit_test diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index e3c14edbb7d..c80bfb12559 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 2.1.8 + 2.1.9 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index af5f51fc0f5..b3bc3815a5b 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package simulation_interface * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 0ae0ff65fd9..e64f9dd747a 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 2.1.8 + 2.1.9 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index efc3979d64c..f43f3010403 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package traffic_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 791782a002e..475e7c1b5f4 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 2.1.8 + 2.1.9 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 994f32f3485..1cf4d6b4393 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index e260652502d..2cc6a7929b6 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 2.1.8 + 2.1.9 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index d61aa672c17..019770ee260 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package random_test_runner * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 8048afce27c..a60fdb70d33 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 2.1.8 + 2.1.9 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 07fdfeab3e5..0412152d8e6 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -23,6 +23,9 @@ Changelog for package scenario_test_runner * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.9 (2024-06-24) +------------------ + 2.1.8 (2024-06-20) ------------------ * Merge branch 'master' into feature/simple_sensor_simulator_unit_test diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 6d04eff5cd1..48064a79278 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 2.1.8 + 2.1.9 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 689049c783abd051976bd99349b23583d5c03994 Mon Sep 17 00:00:00 2001 From: Release Bot Date: Mon, 24 Jun 2024 06:48:13 +0000 Subject: [PATCH 111/118] Bump version of scenario_simulator_v2 from version 2.1.9 to version 2.1.10 --- common/math/arithmetic/CHANGELOG.rst | 5 +++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 5 +++++ common/math/geometry/package.xml | 2 +- .../scenario_simulator_exception/CHANGELOG.rst | 5 +++++ .../scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 5 +++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 5 +++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 5 +++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 5 +++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 5 +++++ map/kashiwanoha_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 5 +++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../CHANGELOG.rst | 5 +++++ .../package.xml | 2 +- .../openscenario_interpreter/CHANGELOG.rst | 5 +++++ .../openscenario_interpreter/package.xml | 2 +- .../CHANGELOG.rst | 5 +++++ .../package.xml | 2 +- .../CHANGELOG.rst | 5 +++++ .../openscenario_interpreter_msgs/package.xml | 2 +- .../openscenario_preprocessor/CHANGELOG.rst | 5 +++++ .../openscenario_preprocessor/package.xml | 2 +- .../CHANGELOG.rst | 5 +++++ .../openscenario_preprocessor_msgs/package.xml | 2 +- .../openscenario_utility/CHANGELOG.rst | 5 +++++ openscenario/openscenario_utility/package.xml | 2 +- .../openscenario_validator/CHANGELOG.rst | 5 +++++ .../openscenario_validator/package.xml | 2 +- .../openscenario_visualization/CHANGELOG.rst | 5 +++++ .../openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 5 +++++ .../package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 5 +++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 5 +++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 5 +++++ simulation/do_nothing_plugin/package.xml | 2 +- .../simple_sensor_simulator/CHANGELOG.rst | 5 +++++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 5 +++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 18 ++++++++++++++++++ simulation/traffic_simulator/package.xml | 2 +- .../traffic_simulator_msgs/CHANGELOG.rst | 5 +++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 5 +++++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 5 +++++ test_runner/scenario_test_runner/package.xml | 2 +- 56 files changed, 181 insertions(+), 28 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 8b2a1acb162..cf923e54231 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package arithmetic * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 014eaddb577..ad43c2fe3ea 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 2.1.9 + 2.1.10 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index 861956aca48..bec3d7a6f0e 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package geometry * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 5a92dd40cd1..61c8aac7214 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 2.1.9 + 2.1.10 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 5df4532cc92..bf7fbfb6ca3 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package scenario_simulator_exception * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index a8136386167..d9aa9e53f10 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 2.1.9 + 2.1.10 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index 4cded42f615..ba82a4e0006 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package junit_exporter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 60d856ac156..30b66e10f70 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 2.1.9 + 2.1.10 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index 95934d7ad3a..de1e6e624a9 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package status_monitor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index a9535b98d91..7f6709cd998 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 2.1.9 + 2.1.10 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index a4357170344..17c48aed3a4 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package concealer * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 001393c4f2c..46d2e2a2794 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 2.1.9 + 2.1.10 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 270ad085379..9850e6a0f18 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -16,6 +16,11 @@ Changelog for package embree_vendor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 3e94e6e4d14..d7013abae58 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 2.1.9 + 2.1.10 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index c3b390b4653..bc4bcb4c308 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package kashiwanoha_map * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index e7e4574bc4c..50f4ca558ce 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 2.1.9 + 2.1.10 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index 2714fb5de74..45c06505761 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package cpp_mock_scenarios * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index ef9955bd868..98ac2cca482 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 2.1.9 + 2.1.10 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 9c7cfa3fb31..670e755a837 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_experimental_catalog * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index fccf8b4cf8e..0628a947d8d 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 2.1.9 + 2.1.10 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 3085bc4f052..b7f8e85cbac 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -14,6 +14,11 @@ Changelog for package openscenario_interpreter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index b29745c3730..95fa5726563 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 2.1.9 + 2.1.10 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 741c07d3aae..8931139c180 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_interpreter_example * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 7d004d78821..2faf63cd068 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 2.1.9 + 2.1.10 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index 3778bb70424..c86f31e2e2e 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_interpreter_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 9a74c40a52e..9d2bc7d48d9 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 2.1.9 + 2.1.10 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index b71d1185099..cee033a8bb4 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_preprocessor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index b52e8e2c0a7..c96086ab3bb 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 2.1.9 + 2.1.10 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index fc3e96336e7..28ea16561a3 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_preprocessor_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index 4ec52b8230b..d72052ba7ce 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 2.1.9 + 2.1.10 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 2d1fbf268b8..42ac0cad30c 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -16,6 +16,11 @@ Changelog for package openscenario_utility * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 0e25156becb..9b77f9d5ce8 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 2.1.9 + 2.1.10 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index 0f6f04675ea..ea24f4d2342 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package openscenario_validator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index ecf9231c984..2a15bed413c 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 2.1.9 + 2.1.10 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 9328de804b7..4bbce685b35 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_visualization * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index d593f34b541..072cbe95dba 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 2.1.9 + 2.1.10 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 7f5795f98bd..16ef80497ce 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index 859bac84c27..1453a93551c 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 2.1.9 + 2.1.10 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index cfe7fad9ca7..93edcd36eff 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package scenario_simulator_v2 * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 51e3782b901..a8185dd6aa4 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 2.1.9 + 2.1.10 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 544152df39d..7934c93035b 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package behavior_tree_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 041d46068ef..2052dc1ef78 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 2.1.9 + 2.1.10 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index c8de7e0352f..a5142482e11 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package do_nothing_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 517e15c193b..e4dc067233d 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 2.1.9 + 2.1.10 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 97b129cb23e..e5c5efa4f3e 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package simple_sensor_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index c80bfb12559..08e802fee44 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 2.1.9 + 2.1.10 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index b3bc3815a5b..2231661d498 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package simulation_interface * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index e64f9dd747a..11217b76381 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 2.1.9 + 2.1.10 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index f43f3010403..894528da555 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -13,6 +13,24 @@ Changelog for package traffic_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge pull request `#1286 `_ from tier4/feature/unit_tests/misc_object_entity + Feature/unit tests/misc object entity +* resolve conflict +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* internal review corrections +* use better naming +* merge tests into a singular file +* clean up +* add newline +* rename file definition +* use builder +* remove variables used only once +* make it work; some tests have been deleted +* collect tests, will not compile +* Contributors: Masaya Kataoka, robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 475e7c1b5f4..48f318db533 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 2.1.9 + 2.1.10 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 1cf4d6b4393..1d7f790de48 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 2cc6a7929b6..d83db7567f5 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 2.1.9 + 2.1.10 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 019770ee260..3a2b51da8d3 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package random_test_runner * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index a60fdb70d33..1b685785b60 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 2.1.9 + 2.1.10 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 0412152d8e6..aaaf2f04bc2 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -23,6 +23,11 @@ Changelog for package scenario_test_runner * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.10 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity +* Contributors: robomic + 2.1.9 (2024-06-24) ------------------ diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 48064a79278..10d20650d14 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 2.1.9 + 2.1.10 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 88908163a8f33daa95da2532685cd6fd4440f602 Mon Sep 17 00:00:00 2001 From: Release Bot Date: Mon, 24 Jun 2024 07:19:17 +0000 Subject: [PATCH 112/118] Bump version of scenario_simulator_v2 from version 2.1.10 to version 2.1.11 --- common/math/arithmetic/CHANGELOG.rst | 10 +++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 8 ++++ common/math/geometry/package.xml | 2 +- .../CHANGELOG.rst | 8 ++++ .../scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 8 ++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 8 ++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 8 ++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 8 ++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 8 ++++ map/kashiwanoha_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 8 ++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../CHANGELOG.rst | 8 ++++ .../package.xml | 2 +- .../openscenario_interpreter/CHANGELOG.rst | 8 ++++ .../openscenario_interpreter/package.xml | 2 +- .../CHANGELOG.rst | 8 ++++ .../package.xml | 2 +- .../CHANGELOG.rst | 8 ++++ .../openscenario_interpreter_msgs/package.xml | 2 +- .../openscenario_preprocessor/CHANGELOG.rst | 8 ++++ .../openscenario_preprocessor/package.xml | 2 +- .../CHANGELOG.rst | 8 ++++ .../package.xml | 2 +- .../openscenario_utility/CHANGELOG.rst | 8 ++++ openscenario/openscenario_utility/package.xml | 2 +- .../openscenario_validator/CHANGELOG.rst | 8 ++++ .../openscenario_validator/package.xml | 2 +- .../openscenario_visualization/CHANGELOG.rst | 8 ++++ .../openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 8 ++++ .../package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 8 ++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 8 ++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 8 ++++ simulation/do_nothing_plugin/package.xml | 2 +- .../simple_sensor_simulator/CHANGELOG.rst | 11 +++++ .../simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 12 ++++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 43 +++++++++++++++++++ simulation/traffic_simulator/package.xml | 2 +- .../traffic_simulator_msgs/CHANGELOG.rst | 8 ++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 8 ++++ test_runner/random_test_runner/package.xml | 2 +- .../scenario_test_runner/CHANGELOG.rst | 8 ++++ test_runner/scenario_test_runner/package.xml | 2 +- 56 files changed, 296 insertions(+), 28 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index cf923e54231..d32f7142d9b 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -13,6 +13,16 @@ Changelog for package arithmetic * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* remove arithmetic tests +* floating_point comparison +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index ad43c2fe3ea..710ac165284 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 2.1.10 + 2.1.11 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index bec3d7a6f0e..0d6dc229d4d 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package geometry * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 61c8aac7214..6b869ebec27 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 2.1.10 + 2.1.11 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index bf7fbfb6ca3..f4713f907cb 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package scenario_simulator_exception * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index d9aa9e53f10..8234aa41030 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 2.1.10 + 2.1.11 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index ba82a4e0006..075daebca02 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package junit_exporter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 30b66e10f70..e194a8a35b2 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 2.1.10 + 2.1.11 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index de1e6e624a9..9e500be211f 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package status_monitor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 7f6709cd998..539f2440b02 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 2.1.10 + 2.1.11 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 17c48aed3a4..86d78129c1e 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package concealer * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 46d2e2a2794..ffcea2dd04e 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 2.1.10 + 2.1.11 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 9850e6a0f18..665cefa3f38 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -16,6 +16,14 @@ Changelog for package embree_vendor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index d7013abae58..e842877105b 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 2.1.10 + 2.1.11 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index bc4bcb4c308..7a37d73eead 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package kashiwanoha_map * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 50f4ca558ce..d7578518f65 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 2.1.10 + 2.1.11 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index 45c06505761..fe88f77c9da 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package cpp_mock_scenarios * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 98ac2cca482..45b0d19690a 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 2.1.10 + 2.1.11 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 670e755a837..8975ada787d 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package openscenario_experimental_catalog * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 0628a947d8d..23d833adddd 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 2.1.10 + 2.1.11 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index b7f8e85cbac..4d2074043ac 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -14,6 +14,14 @@ Changelog for package openscenario_interpreter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 95fa5726563..cb140e24ef1 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 2.1.10 + 2.1.11 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 8931139c180..3756ecb8d29 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package openscenario_interpreter_example * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 2faf63cd068..1564cc4ec18 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 2.1.10 + 2.1.11 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index c86f31e2e2e..d2d95e4b6bd 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package openscenario_interpreter_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 9d2bc7d48d9..0f47192ba85 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 2.1.10 + 2.1.11 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index cee033a8bb4..97684b5a8d1 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package openscenario_preprocessor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index c96086ab3bb..7ab867a143a 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 2.1.10 + 2.1.11 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 28ea16561a3..01266aa3592 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package openscenario_preprocessor_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index d72052ba7ce..537f710156f 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 2.1.10 + 2.1.11 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 42ac0cad30c..ad6582f1a5a 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -16,6 +16,14 @@ Changelog for package openscenario_utility * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 9b77f9d5ce8..f0a5287fc05 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 2.1.10 + 2.1.11 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index ea24f4d2342..78f1b1154be 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package openscenario_validator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 2a15bed413c..5e5765021e7 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 2.1.10 + 2.1.11 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 4bbce685b35..2dd79da5875 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package openscenario_visualization * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 072cbe95dba..3ce632c693f 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 2.1.10 + 2.1.11 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 16ef80497ce..340c3b89d06 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index 1453a93551c..549f4b23687 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 2.1.10 + 2.1.11 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 93edcd36eff..944d4ca2241 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package scenario_simulator_v2 * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index a8185dd6aa4..99972f403cb 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 2.1.10 + 2.1.11 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 7934c93035b..fc3fad4120f 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package behavior_tree_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 2052dc1ef78..9e409e7d534 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 2.1.10 + 2.1.11 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index a5142482e11..941029919d1 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package do_nothing_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index e4dc067233d..c2909d4dda2 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 2.1.10 + 2.1.11 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index e5c5efa4f3e..32149516e71 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -13,6 +13,17 @@ Changelog for package simple_sensor_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* remove simple sensor simulator tets +* vertex, toPoints +* simple_sensr_simulation unit tests +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 08e802fee44..42415b09c40 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 2.1.10 + 2.1.11 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 2231661d498..d3e40823508 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -13,6 +13,18 @@ Changelog for package simulation_interface * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* final cleanup +* remove conversions tests +* conversions cleanup +* simulation_interface unit tests +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 11217b76381..e28ca8a2f65 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 2.1.10 + 2.1.11 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index 894528da555..a5ed30cf060 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -13,6 +13,49 @@ Changelog for package traffic_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge pull request `#1287 `_ from tier4/feature/unit_tests/miscellaneous + Feature/unit tests/miscellaneous +* resolve conflict +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* fix typo +* remove unecessary assignment +* add newlines +* add copy of helper_functions, apply review requests +* remove misc tests +* resolve merge confilct +* resolve merge +* remove test list file +* use test fixtures +* rename literal +* use test fixture +* remove variables that are used only once +* remove unnecesary checks +* newlines +* add tes descriptions +* prepare notes for descriptions +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* remove conversions tests +* remove tests that exist in other branches and refactor wht is left +* style +* getDistanceToLeftLaneBound +* getRouteLanelets_empty +* simulation clock tests +* entity_base tests +* entity_base tests; code cleanup +* EntityBase onUpdate. onPostUpdate tests +* entity_base dummy class with a couple of tests +* job accessor tests +* make linter happy +* job and job_list unit tests +* behavior getRequestString +* vertex, toPoints +* traffic light bulb tests +* traffic_light status shape color tests +* traffic_simulator unit tests +* Contributors: Masaya Kataoka, robomic + 2.1.10 (2024-06-24) ------------------- * Merge pull request `#1286 `_ from tier4/feature/unit_tests/misc_object_entity diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 48f318db533..d16543dd403 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 2.1.10 + 2.1.11 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 1d7f790de48..1a554e1ced2 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package openscenario_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index d83db7567f5..4f78ddcf929 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 2.1.10 + 2.1.11 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 3a2b51da8d3..9bd2c9ff839 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package random_test_runner * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 1b685785b60..af396e648a7 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 2.1.10 + 2.1.11 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index aaaf2f04bc2..331aaa1970b 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -23,6 +23,14 @@ Changelog for package scenario_test_runner * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.1.11 (2024-06-24) +------------------- +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* resolve merge confilct +* resolve merge +* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous +* Contributors: robomic + 2.1.10 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 10d20650d14..3244d3b26bb 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 2.1.10 + 2.1.11 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From c50d79fce98242d76671360029b97c166412e76f Mon Sep 17 00:00:00 2001 From: Release Bot Date: Mon, 24 Jun 2024 08:35:34 +0000 Subject: [PATCH 113/118] Bump version of scenario_simulator_v2 from version 2.1.11 to version 2.2.0 --- common/math/arithmetic/CHANGELOG.rst | 8 ++++++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 8 ++++++++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 8 ++++++++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 8 ++++++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 8 ++++++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 8 ++++++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 8 ++++++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 8 ++++++++ map/kashiwanoha_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 8 ++++++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../openscenario_experimental_catalog/CHANGELOG.rst | 8 ++++++++ .../openscenario_experimental_catalog/package.xml | 2 +- openscenario/openscenario_interpreter/CHANGELOG.rst | 8 ++++++++ openscenario/openscenario_interpreter/package.xml | 2 +- .../openscenario_interpreter_example/CHANGELOG.rst | 8 ++++++++ .../openscenario_interpreter_example/package.xml | 2 +- .../openscenario_interpreter_msgs/CHANGELOG.rst | 8 ++++++++ .../openscenario_interpreter_msgs/package.xml | 2 +- openscenario/openscenario_preprocessor/CHANGELOG.rst | 8 ++++++++ openscenario/openscenario_preprocessor/package.xml | 2 +- .../openscenario_preprocessor_msgs/CHANGELOG.rst | 8 ++++++++ .../openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 8 ++++++++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 8 ++++++++ openscenario/openscenario_validator/package.xml | 2 +- rviz_plugins/openscenario_visualization/CHANGELOG.rst | 8 ++++++++ rviz_plugins/openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 8 ++++++++ .../real_time_factor_control_rviz_plugin/package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 8 ++++++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 8 ++++++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 8 ++++++++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 8 ++++++++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 8 ++++++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 11 +++++++++++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 8 ++++++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 8 ++++++++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 8 ++++++++ test_runner/scenario_test_runner/package.xml | 2 +- 56 files changed, 255 insertions(+), 28 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index d32f7142d9b..5af062e0eed 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package arithmetic * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 710ac165284..a19a3444d89 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 2.1.11 + 2.2.0 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index 0d6dc229d4d..1358c24e567 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package geometry * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 6b869ebec27..a71445f382f 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 2.1.11 + 2.2.0 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index f4713f907cb..7cebc1a659d 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package scenario_simulator_exception * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 8234aa41030..ce7af48fb86 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 2.1.11 + 2.2.0 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index 075daebca02..bf2d02f1445 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package junit_exporter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index e194a8a35b2..a3b833caadd 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 2.1.11 + 2.2.0 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index 9e500be211f..019eb70336e 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package status_monitor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 539f2440b02..e27db4c4299 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 2.1.11 + 2.2.0 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 86d78129c1e..00f78d84c8f 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package concealer * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/external/concealer/package.xml b/external/concealer/package.xml index ffcea2dd04e..06370416fb9 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 2.1.11 + 2.2.0 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 665cefa3f38..7794416b522 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -16,6 +16,14 @@ Changelog for package embree_vendor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index e842877105b..097cb5e948d 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 2.1.11 + 2.2.0 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 7a37d73eead..32f973a8a74 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package kashiwanoha_map * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index d7578518f65..76d4a03bfce 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 2.1.11 + 2.2.0 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index fe88f77c9da..dd64ba11d6f 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package cpp_mock_scenarios * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 45b0d19690a..c60d8de32b4 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 2.1.11 + 2.2.0 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 8975ada787d..71125889296 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package openscenario_experimental_catalog * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 23d833adddd..0974ee690d8 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 2.1.11 + 2.2.0 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 4d2074043ac..9ab6475b837 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -14,6 +14,14 @@ Changelog for package openscenario_interpreter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index cb140e24ef1..e04a9a04fae 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 2.1.11 + 2.2.0 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 3756ecb8d29..e108d66bd4d 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package openscenario_interpreter_example * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 1564cc4ec18..ddf527eb221 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 2.1.11 + 2.2.0 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index d2d95e4b6bd..f0f0127c92c 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package openscenario_interpreter_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 0f47192ba85..52ab76ff841 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 2.1.11 + 2.2.0 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index 97684b5a8d1..23120a22ede 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package openscenario_preprocessor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 7ab867a143a..efb615f58a5 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 2.1.11 + 2.2.0 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 01266aa3592..6a5281c3244 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package openscenario_preprocessor_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index 537f710156f..94008a6e532 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 2.1.11 + 2.2.0 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index ad6582f1a5a..41e64560b0c 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -16,6 +16,14 @@ Changelog for package openscenario_utility * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index f0a5287fc05..d7f1bfce065 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 2.1.11 + 2.2.0 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index 78f1b1154be..da59fc059a3 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package openscenario_validator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 5e5765021e7..bdbf94b88e5 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 2.1.11 + 2.2.0 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 2dd79da5875..c8db444d2fb 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package openscenario_visualization * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 3ce632c693f..47b406a795e 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 2.1.11 + 2.2.0 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 340c3b89d06..4f76f5426d1 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index 549f4b23687..de4d9c51451 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 2.1.11 + 2.2.0 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 944d4ca2241..a8605b6c223 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package scenario_simulator_v2 * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 99972f403cb..e85733e15ab 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 2.1.11 + 2.2.0 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index fc3fad4120f..bf06c6e67b4 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package behavior_tree_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 9e409e7d534..fbe496cdc09 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 2.1.11 + 2.2.0 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 941029919d1..836c45e5569 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package do_nothing_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index c2909d4dda2..a2ed5cabb40 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 2.1.11 + 2.2.0 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 32149516e71..259a3477f46 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package simple_sensor_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 42415b09c40..5bd5db802f5 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 2.1.11 + 2.2.0 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index d3e40823508..28c10072192 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package simulation_interface * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index e28ca8a2f65..26992e0f0dc 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 2.1.11 + 2.2.0 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index a5ed30cf060..53bf99c962a 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -13,6 +13,17 @@ Changelog for package traffic_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge pull request `#1292 `_ from tier4/feature/clear_route_api + add API::clearRoute() +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* add API::clearRoute() +* Contributors: Masaya Kataoka, Taiga, Taiga Takano + 2.1.11 (2024-06-24) ------------------- * Merge pull request `#1287 `_ from tier4/feature/unit_tests/miscellaneous diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index d16543dd403..477aa271577 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 2.1.11 + 2.2.0 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 1a554e1ced2..e79f40478b7 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package openscenario_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 4f78ddcf929..2208f900eea 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 2.1.11 + 2.2.0 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 9bd2c9ff839..262b14d50ca 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package random_test_runner * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index af396e648a7..34fcc455d21 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 2.1.11 + 2.2.0 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 331aaa1970b..34543ccefdb 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -23,6 +23,14 @@ Changelog for package scenario_test_runner * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.2.0 (2024-06-24) +------------------ +* Merge branch 'master' into feature/clear_route_api +* Merge remote-tracking branch 'origin/master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Merge branch 'master' into feature/clear_route_api +* Contributors: Masaya Kataoka, Taiga + 2.1.11 (2024-06-24) ------------------- * Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 3244d3b26bb..ff3db499cfa 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 2.1.11 + 2.2.0 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 92803a26ed31fcd54f7f55b01cc878ee061d803e Mon Sep 17 00:00:00 2001 From: Release Bot Date: Thu, 27 Jun 2024 06:59:38 +0000 Subject: [PATCH 114/118] Bump version of scenario_simulator_v2 from version 2.2.0 to version 2.2.1 --- common/math/arithmetic/CHANGELOG.rst | 5 +++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 5 +++++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 5 +++++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 5 +++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 5 +++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 5 +++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 5 +++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 5 +++++ map/kashiwanoha_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 5 +++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../openscenario_experimental_catalog/CHANGELOG.rst | 5 +++++ .../openscenario_experimental_catalog/package.xml | 2 +- openscenario/openscenario_interpreter/CHANGELOG.rst | 5 +++++ openscenario/openscenario_interpreter/package.xml | 2 +- .../openscenario_interpreter_example/CHANGELOG.rst | 5 +++++ openscenario/openscenario_interpreter_example/package.xml | 2 +- openscenario/openscenario_interpreter_msgs/CHANGELOG.rst | 5 +++++ openscenario/openscenario_interpreter_msgs/package.xml | 2 +- openscenario/openscenario_preprocessor/CHANGELOG.rst | 5 +++++ openscenario/openscenario_preprocessor/package.xml | 2 +- openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst | 5 +++++ openscenario/openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 5 +++++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 5 +++++ openscenario/openscenario_validator/package.xml | 2 +- rviz_plugins/openscenario_visualization/CHANGELOG.rst | 5 +++++ rviz_plugins/openscenario_visualization/package.xml | 2 +- .../real_time_factor_control_rviz_plugin/CHANGELOG.rst | 5 +++++ .../real_time_factor_control_rviz_plugin/package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 5 +++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 5 +++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 5 +++++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 5 +++++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 5 +++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 8 ++++++++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 5 +++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 5 +++++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 5 +++++ test_runner/scenario_test_runner/package.xml | 2 +- 56 files changed, 171 insertions(+), 28 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 5af062e0eed..230bb50ff09 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package arithmetic * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index a19a3444d89..55d225c51c2 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 2.2.0 + 2.2.1 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index 1358c24e567..7c13815e45c 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package geometry * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index a71445f382f..de81774558a 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 2.2.0 + 2.2.1 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 7cebc1a659d..d6b2140eb6c 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package scenario_simulator_exception * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index ce7af48fb86..b9f8c199f13 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 2.2.0 + 2.2.1 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index bf2d02f1445..bbcd3957089 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package junit_exporter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index a3b833caadd..079b63e9a9d 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 2.2.0 + 2.2.1 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index 019eb70336e..49b2f764968 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package status_monitor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index e27db4c4299..601bc78e02d 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 2.2.0 + 2.2.1 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 00f78d84c8f..ee2f54b70fa 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package concealer * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 06370416fb9..2abb0cb80dd 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 2.2.0 + 2.2.1 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 7794416b522..6a46b675635 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -16,6 +16,11 @@ Changelog for package embree_vendor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 097cb5e948d..bfe7ca2b88f 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 2.2.0 + 2.2.1 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 32f973a8a74..5e67e3a6e37 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package kashiwanoha_map * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 76d4a03bfce..7d8eee6c0fc 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 2.2.0 + 2.2.1 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index dd64ba11d6f..279a699868c 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package cpp_mock_scenarios * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index c60d8de32b4..610872d2e88 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 2.2.0 + 2.2.1 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 71125889296..3356fc0fde8 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_experimental_catalog * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 0974ee690d8..349afd1aa29 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 2.2.0 + 2.2.1 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 9ab6475b837..b03a1df08e4 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -14,6 +14,11 @@ Changelog for package openscenario_interpreter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index e04a9a04fae..5185f74f749 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 2.2.0 + 2.2.1 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index e108d66bd4d..475ea6d4519 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_interpreter_example * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index ddf527eb221..a07590261da 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 2.2.0 + 2.2.1 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index f0f0127c92c..b907a88941c 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_interpreter_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 52ab76ff841..41d476c00a9 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 2.2.0 + 2.2.1 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index 23120a22ede..c5e4630cb1a 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_preprocessor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index efb615f58a5..c86a9121a48 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 2.2.0 + 2.2.1 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 6a5281c3244..ef4fa3927e2 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_preprocessor_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index 94008a6e532..2333c750686 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 2.2.0 + 2.2.1 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 41e64560b0c..f5575d5e8d6 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -16,6 +16,11 @@ Changelog for package openscenario_utility * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index d7f1bfce065..71484043b65 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 2.2.0 + 2.2.1 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index da59fc059a3..6cdcb30a770 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package openscenario_validator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index bdbf94b88e5..d93f690194b 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 2.2.0 + 2.2.1 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index c8db444d2fb..0a6daf0831b 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_visualization * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 47b406a795e..090ca4b8224 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 2.2.0 + 2.2.1 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 4f76f5426d1..06632b114bf 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index de4d9c51451..8533351d1ab 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 2.2.0 + 2.2.1 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index a8605b6c223..443983929c6 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package scenario_simulator_v2 * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index e85733e15ab..703cec66520 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 2.2.0 + 2.2.1 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index bf06c6e67b4..faeb100ac05 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package behavior_tree_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index fbe496cdc09..a62f5e52e68 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 2.2.0 + 2.2.1 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 836c45e5569..4cc0cec13e7 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package do_nothing_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index a2ed5cabb40..59a66b050ce 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 2.2.0 + 2.2.1 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 259a3477f46..1759a42630b 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package simple_sensor_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 5bd5db802f5..e6d370e35e1 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 2.2.0 + 2.2.1 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 28c10072192..fca52566a36 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package simulation_interface * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 26992e0f0dc..1a0aad86e59 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 2.2.0 + 2.2.1 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index 53bf99c962a..ff0ea863095 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package traffic_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge pull request `#1293 `_ from tier4/fix/issue1276-re + Optimize entity frame computation. +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Optimize entity frame computation. +* Contributors: Masaya Kataoka, Taiga Takano + 2.2.0 (2024-06-24) ------------------ * Merge pull request `#1292 `_ from tier4/feature/clear_route_api diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 477aa271577..62cdd9edf22 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 2.2.0 + 2.2.1 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index e79f40478b7..e1f2447bb20 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package openscenario_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 2208f900eea..715c1c326fc 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 2.2.0 + 2.2.1 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 262b14d50ca..c8ec4143e51 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -13,6 +13,11 @@ Changelog for package random_test_runner * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 34fcc455d21..88daa749203 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 2.2.0 + 2.2.1 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 34543ccefdb..815b1563140 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -23,6 +23,11 @@ Changelog for package scenario_test_runner * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.2.1 (2024-06-27) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/issue1276-re +* Contributors: Masaya Kataoka + 2.2.0 (2024-06-24) ------------------ * Merge branch 'master' into feature/clear_route_api diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index ff3db499cfa..ad4ecbdfbfd 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 2.2.0 + 2.2.1 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 8b2b42ad096a35699d16e1e2ad5a177167f7aa26 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Fri, 28 Jun 2024 13:50:24 +0900 Subject: [PATCH 115/118] Delete merged branch using github-script instead of delete-merged-branch action --- .github/workflows/Release.yaml | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/.github/workflows/Release.yaml b/.github/workflows/Release.yaml index 49ccac2bb0d..65940236bb1 100644 --- a/.github/workflows/Release.yaml +++ b/.github/workflows/Release.yaml @@ -130,8 +130,13 @@ jobs: name: ${{ steps.new_version.outputs.new_version }} body: ${{ steps.get_release_description.outputs.stdout }} - - name: Delete branch + - name: Delete branch if merged if: github.event.pull_request.merged == true - uses: SvanBoxel/delete-merged-branch@main - env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + uses: actions/github-script@v5 + with: + script: | + github.rest.git.deleteRef({ + owner: context.repo.owner, + repo: context.repo.repo, + ref: `heads/${context.payload.pull_request.head.ref}`, + }) From e8fe111fb14239eddc3c8c32f804dd3338fdaa3e Mon Sep 17 00:00:00 2001 From: Release Bot Date: Fri, 28 Jun 2024 08:51:14 +0000 Subject: [PATCH 116/118] Bump version of scenario_simulator_v2 from version 2.2.1 to version 2.2.2 --- common/math/arithmetic/CHANGELOG.rst | 3 +++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 3 +++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 3 +++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 3 +++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 3 +++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 3 +++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 3 +++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 3 +++ map/kashiwanoha_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 3 +++ mock/cpp_mock_scenarios/package.xml | 2 +- openscenario/openscenario_experimental_catalog/CHANGELOG.rst | 3 +++ openscenario/openscenario_experimental_catalog/package.xml | 2 +- openscenario/openscenario_interpreter/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter/package.xml | 2 +- openscenario/openscenario_interpreter_example/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter_example/package.xml | 2 +- openscenario/openscenario_interpreter_msgs/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter_msgs/package.xml | 2 +- openscenario/openscenario_preprocessor/CHANGELOG.rst | 3 +++ openscenario/openscenario_preprocessor/package.xml | 2 +- openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst | 3 +++ openscenario/openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 3 +++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 3 +++ openscenario/openscenario_validator/package.xml | 2 +- rviz_plugins/openscenario_visualization/CHANGELOG.rst | 3 +++ rviz_plugins/openscenario_visualization/package.xml | 2 +- .../real_time_factor_control_rviz_plugin/CHANGELOG.rst | 3 +++ rviz_plugins/real_time_factor_control_rviz_plugin/package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 3 +++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 3 +++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 3 +++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 3 +++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 3 +++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 3 +++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 3 +++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 3 +++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 3 +++ test_runner/scenario_test_runner/package.xml | 2 +- 56 files changed, 112 insertions(+), 28 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 230bb50ff09..5f6d6bad47f 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package arithmetic * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 55d225c51c2..54607ba6aa9 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 2.2.1 + 2.2.2 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index 7c13815e45c..80294ca6eef 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package geometry * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index de81774558a..46fb3e6a19c 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 2.2.1 + 2.2.2 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index d6b2140eb6c..ac179947355 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package scenario_simulator_exception * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index b9f8c199f13..e1c3f3a118d 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 2.2.1 + 2.2.2 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index bbcd3957089..4e58856f21e 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package junit_exporter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 079b63e9a9d..21374b90041 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 2.2.1 + 2.2.2 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index 49b2f764968..0f2e2c997dc 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package status_monitor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 601bc78e02d..b6ce52be046 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 2.2.1 + 2.2.2 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index ee2f54b70fa..0e32dd7943b 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package concealer * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 2abb0cb80dd..cfd6d98aad9 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 2.2.1 + 2.2.2 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 6a46b675635..af5c618917b 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -16,6 +16,9 @@ Changelog for package embree_vendor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index bfe7ca2b88f..92b04874265 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 2.2.1 + 2.2.2 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 5e67e3a6e37..8d45e3c9b32 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package kashiwanoha_map * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 7d8eee6c0fc..9d4f3b74ae9 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 2.2.1 + 2.2.2 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index 279a699868c..15066476e67 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package cpp_mock_scenarios * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 610872d2e88..907641c5a7d 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 2.2.1 + 2.2.2 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 3356fc0fde8..0fb5f0cf2c6 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_experimental_catalog * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 349afd1aa29..c8cdf1db421 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 2.2.1 + 2.2.2 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index b03a1df08e4..60974492fcc 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -14,6 +14,9 @@ Changelog for package openscenario_interpreter * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 5185f74f749..736ebe9f644 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 2.2.1 + 2.2.2 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 475ea6d4519..a4d1da3b22a 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_interpreter_example * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index a07590261da..33d0bad1382 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 2.2.1 + 2.2.2 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index b907a88941c..1d3321783da 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_interpreter_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 41d476c00a9..1763b81f992 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 2.2.1 + 2.2.2 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index c5e4630cb1a..42ae7cbb048 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_preprocessor * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index c86a9121a48..7d7b11326b6 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 2.2.1 + 2.2.2 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index ef4fa3927e2..ebcfc366542 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_preprocessor_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index 2333c750686..fee84e73d6a 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 2.2.1 + 2.2.2 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index f5575d5e8d6..c908bfc1151 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -16,6 +16,9 @@ Changelog for package openscenario_utility * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 71484043b65..ea1f8237e92 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 2.2.1 + 2.2.2 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index 6cdcb30a770..836d132e539 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package openscenario_validator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index d93f690194b..b3629608897 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 2.2.1 + 2.2.2 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 0a6daf0831b..bce43ce5d78 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_visualization * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 090ca4b8224..93acdd3c03f 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 2.2.1 + 2.2.2 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 06632b114bf..31649c3c965 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index 8533351d1ab..38ed6f5ad01 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 2.2.1 + 2.2.2 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 443983929c6..03f7be0ff10 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package scenario_simulator_v2 * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 703cec66520..f548e67cd5a 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 2.2.1 + 2.2.2 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index faeb100ac05..985e4e0234f 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package behavior_tree_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index a62f5e52e68..138a3e24b6e 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 2.2.1 + 2.2.2 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 4cc0cec13e7..83b2dc95374 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package do_nothing_plugin * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 59a66b050ce..380138560dd 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 2.2.1 + 2.2.2 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 1759a42630b..73c524ff7d4 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package simple_sensor_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index e6d370e35e1..cba88c93183 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 2.2.1 + 2.2.2 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index fca52566a36..9d4f77d4635 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package simulation_interface * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 1a0aad86e59..1080cf140d0 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 2.2.1 + 2.2.2 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index ff0ea863095..29b6d963cec 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package traffic_simulator * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge pull request `#1293 `_ from tier4/fix/issue1276-re diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 62cdd9edf22..3c2ff1308c3 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 2.2.1 + 2.2.2 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index e1f2447bb20..d00725765b1 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package openscenario_msgs * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 715c1c326fc..df39eb4a4ae 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 2.2.1 + 2.2.2 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index c8ec4143e51..47b666910ec 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -13,6 +13,9 @@ Changelog for package random_test_runner * Merge branch 'master' into refactor/drop_workflow * Contributors: Kotaro Yoshimoto +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 88daa749203..57746f594a2 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 2.2.1 + 2.2.2 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 815b1563140..b21cad5b858 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -23,6 +23,9 @@ Changelog for package scenario_test_runner * refactor: delete workflow.Workflow and rename workflow.py to scenario.py * Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki +2.2.2 (2024-06-28) +------------------ + 2.2.1 (2024-06-27) ------------------ * Merge remote-tracking branch 'origin/master' into fix/issue1276-re diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index ad4ecbdfbfd..55e3498354c 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 2.2.1 + 2.2.2 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 938683501bf87ac8114e59fdbf1ee9bf9fed0444 Mon Sep 17 00:00:00 2001 From: robomic Date: Fri, 28 Jun 2024 12:46:38 +0200 Subject: [PATCH 117/118] review changes --- .../test_occupancy_grid_builder.cpp | 81 +++++---- .../test/src/test_scenario_simulator.cpp | 155 +----------------- .../test/src/test_scenario_simulator.hpp | 133 +++++++++++++++ 3 files changed, 173 insertions(+), 196 deletions(-) create mode 100644 simulation/simple_sensor_simulator/test/src/test_scenario_simulator.hpp diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp index f74400a3c6f..d065c1668b9 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp @@ -20,17 +20,6 @@ #include #include -auto printGrid(const simple_sensor_simulator::OccupancyGridBuilder & builder) -> void -{ - const auto & grid = builder.get(); - for (size_t i = static_cast(0); i < builder.height; ++i) { - for (size_t j = static_cast(0); j < builder.width; ++j) { - std::cout << static_cast(grid.at(i * builder.width + j)) << ", "; - } - std::cout << std::endl; - } -} - auto makePose(const double x, const double y) -> geometry_msgs::msg::Pose { return geometry_msgs::build() @@ -65,13 +54,6 @@ TEST(OccupancyGridBuilder, add_overLimit) */ TEST(OccupancyGridBuilder, add_primitiveInside) { - auto builder = simple_sensor_simulator::OccupancyGridBuilder( - 1.0, static_cast(10), static_cast(20), static_cast(2), - static_cast(1)); - builder.add(makeBox(5.0f, 4.0, 0.0)); - builder.add(makeBox(2.0f, -5.0, 0.0)); - builder.build(); - const auto & result_obtained = builder.get(); const auto result_expected = std::vector{ // clang-format off 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, @@ -86,7 +68,16 @@ TEST(OccupancyGridBuilder, add_primitiveInside) 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, // clang-format on }; - EXPECT_EQ(result_expected, result_obtained); + + auto builder = simple_sensor_simulator::OccupancyGridBuilder( + 1.0, static_cast(10), static_cast(20), static_cast(2), + static_cast(1)); + builder.add(makeBox(5.0f, 4.0, 0.0)); + builder.add(makeBox(2.0f, -5.0, 0.0)); + + builder.build(); + + EXPECT_EQ(result_expected, builder.get()); } /** @@ -95,13 +86,6 @@ TEST(OccupancyGridBuilder, add_primitiveInside) */ TEST(OccupancyGridBuilder, add_primitiveProtruding) { - auto builder = simple_sensor_simulator::OccupancyGridBuilder( - 1.0, static_cast(10), static_cast(20), static_cast(2), - static_cast(1)); - builder.add(makeBox(5.0f, 9.0, -4.0)); - builder.add(makeBox(2.0f, -10.0, 3.0)); - builder.build(); - const auto & result_obtained = builder.get(); const auto result_expected = std::vector{ // clang-format off 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 2, 2, 2, @@ -116,7 +100,16 @@ TEST(OccupancyGridBuilder, add_primitiveProtruding) 2, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // clang-format on }; - EXPECT_EQ(result_expected, result_obtained); + + auto builder = simple_sensor_simulator::OccupancyGridBuilder( + 1.0, static_cast(10), static_cast(20), static_cast(2), + static_cast(1)); + builder.add(makeBox(5.0f, 9.0, -4.0)); + builder.add(makeBox(2.0f, -10.0, 3.0)); + + builder.build(); + + EXPECT_EQ(result_expected, builder.get()); } /** @@ -125,12 +118,6 @@ TEST(OccupancyGridBuilder, add_primitiveProtruding) */ TEST(OccupancyGridBuilder, add_primitiveInCenter) { - auto builder = simple_sensor_simulator::OccupancyGridBuilder( - 1.0, static_cast(10), static_cast(20), static_cast(2), - static_cast(1)); - builder.add(makeBox(5.0f, 0.0, 0.0)); - builder.build(); - const auto & result_obtained = builder.get(); const auto result_expected = std::vector{ // clang-format off 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, @@ -157,7 +144,15 @@ TEST(OccupancyGridBuilder, add_primitiveInCenter) 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, */ }; - EXPECT_EQ(result_expected, result_obtained); + + auto builder = simple_sensor_simulator::OccupancyGridBuilder( + 1.0, static_cast(10), static_cast(20), static_cast(2), + static_cast(1)); + builder.add(makeBox(5.0f, 0.0, 0.0)); + + builder.build(); + + EXPECT_EQ(result_expected, builder.get()); } /** @@ -166,13 +161,6 @@ TEST(OccupancyGridBuilder, add_primitiveInCenter) */ TEST(OccupancyGridBuilder, reset) { - auto builder = simple_sensor_simulator::OccupancyGridBuilder( - 1.0, static_cast(10), static_cast(20), static_cast(2), - static_cast(1)); - builder.add(makeBox(5.0f, 0.0, 0.0)); - builder.reset(makePose(0.0, 0.0)); - builder.build(); - const auto & result_obtained = builder.get(); const auto result_expected = std::vector{ // clang-format off 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, @@ -187,7 +175,16 @@ TEST(OccupancyGridBuilder, reset) 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // clang-format on }; - EXPECT_EQ(result_expected, result_obtained); + + auto builder = simple_sensor_simulator::OccupancyGridBuilder( + 1.0, static_cast(10), static_cast(20), static_cast(2), + static_cast(1)); + builder.add(makeBox(5.0f, 0.0, 0.0)); + builder.reset(makePose(0.0, 0.0)); + + builder.build(); + + EXPECT_EQ(result_expected, builder.get()); } int main(int argc, char ** argv) diff --git a/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp index 154a8c88778..51a45c7979a 100644 --- a/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp +++ b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.cpp @@ -12,122 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include -#include -#include -#include -#include -#include -#include - #include "sensor_simulation/expect_eq_macros.hpp" - -auto makeInitializeRequest() -> simulation_api_schema::InitializeRequest -{ - auto request = simulation_api_schema::InitializeRequest(); - request.set_lanelet2_map_path( - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"); - return request; -} - -auto makeUpdateFrameRequest() -> simulation_api_schema::UpdateFrameRequest -{ - auto request = simulation_api_schema::UpdateFrameRequest(); - request.set_current_scenario_time(1.0); - request.set_current_simulation_time(1.0); - return request; -} - -auto makeDespawnEntityRequest(const std::string name = "name") - -> simulation_api_schema::DespawnEntityRequest -{ - auto request = simulation_api_schema::DespawnEntityRequest(); - request.set_name(name); - return request; -} - -auto makeSpawnVehicleEntityRequest(const std::string name = "name", const bool is_ego = false) - -> simulation_api_schema::SpawnVehicleEntityRequest -{ - auto request = simulation_api_schema::SpawnVehicleEntityRequest(); - const auto params = traffic_simulator_msgs::msg::VehicleParameters{}; - simulation_interface::toProto(params, *request.mutable_parameters()); - request.mutable_parameters()->set_name(name); - request.set_is_ego(is_ego); - return request; -} - -auto makeSpawnPedestrianEntityRequest(const std::string name = "name") - -> simulation_api_schema::SpawnPedestrianEntityRequest -{ - auto request = simulation_api_schema::SpawnPedestrianEntityRequest(); - const auto params = traffic_simulator_msgs::msg::PedestrianParameters{}; - simulation_interface::toProto(params, *request.mutable_parameters()); - request.mutable_parameters()->set_name(name); - return request; -} - -auto makeSpawnMiscObjectEntityRequest(const std::string name = "name") - -> simulation_api_schema::SpawnMiscObjectEntityRequest -{ - auto request = simulation_api_schema::SpawnMiscObjectEntityRequest(); - const auto params = traffic_simulator_msgs::msg::MiscObjectParameters{}; - simulation_interface::toProto(params, *request.mutable_parameters()); - request.mutable_parameters()->set_name(name); - return request; -} - -auto makeAttachDetectionSensorRequest() -> simulation_api_schema::AttachDetectionSensorRequest -{ - auto request = simulation_api_schema::AttachDetectionSensorRequest(); - auto configuration = traffic_simulator::helper::constructDetectionSensorConfiguration( - "entity_name", "awf/universe", 1.0); - *request.mutable_configuration() = configuration; - return request; -} - -auto makeAttachLidarSensorRequest() -> simulation_api_schema::AttachLidarSensorRequest -{ - auto request = simulation_api_schema::AttachLidarSensorRequest(); - auto configuration = traffic_simulator::helper::constructLidarConfiguration( - traffic_simulator::helper::LidarType::VLP16, "entity_name", "awf/universe", 1.0); - *request.mutable_configuration() = configuration; - return request; -} - -auto makeAttachOccupancyGridSensorRequest() - -> simulation_api_schema::AttachOccupancyGridSensorRequest -{ - auto request = simulation_api_schema::AttachOccupancyGridSensorRequest(); - auto configuration = simulation_api_schema::OccupancyGridSensorConfiguration(); - - configuration.set_entity("entity_name"); - configuration.set_architecture_type("awf/universe"); - configuration.set_update_duration(1.0); - configuration.set_range(300.0); - configuration.set_resolution(1.0); - *request.mutable_configuration() = configuration; - return request; -} - -class ScenarioSimulatorTest : public testing::Test -{ -protected: - ScenarioSimulatorTest() - : server([] { - rclcpp::init(0, nullptr); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - }), - client(simulation_interface::TransportProtocol::TCP, "localhost", 5555U) - { - } - std::thread server; - zeromq::MultiClient client; -}; +#include "test_scenario_simulator.hpp" /** * @note Test initialization correctness with a sample request with the default port (5555). @@ -180,9 +66,6 @@ TEST_F(ScenarioSimulatorTest, updateFrame_correct) { EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); EXPECT_TRUE(client.call(makeUpdateFrameRequest()).result().success()); - - rclcpp::shutdown(); - server.join(); } /** @@ -192,9 +75,6 @@ TEST_F(ScenarioSimulatorTest, updateFrame_noInitialize) { // UB is entered during this test. ScenarioSimulator constructor is missing "initialized_(false)," EXPECT_FALSE(client.call(makeUpdateFrameRequest()).result().success()); - - rclcpp::shutdown(); - server.join(); } /** @@ -205,9 +85,6 @@ TEST_F(ScenarioSimulatorTest, spawnVehicleEntity_firstEgo) { EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); EXPECT_TRUE(client.call(makeSpawnVehicleEntityRequest("ego", true)).result().success()); - - rclcpp::shutdown(); - server.join(); } /** @@ -217,9 +94,6 @@ TEST_F(ScenarioSimulatorTest, spawnVehicleEntity_npc) { EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); EXPECT_TRUE(client.call(makeSpawnVehicleEntityRequest("npc", false)).result().success()); - - rclcpp::shutdown(); - server.join(); } /** @@ -229,9 +103,6 @@ TEST_F(ScenarioSimulatorTest, spawnPedestrianEntity) { EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); EXPECT_TRUE(client.call(makeSpawnPedestrianEntityRequest("bob")).result().success()); - - rclcpp::shutdown(); - server.join(); } /** @@ -241,9 +112,6 @@ TEST_F(ScenarioSimulatorTest, spawnMiscObjectEntity) { EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); EXPECT_TRUE(client.call(makeSpawnMiscObjectEntityRequest("blob")).result().success()); - - rclcpp::shutdown(); - server.join(); } /** @@ -254,9 +122,6 @@ TEST_F(ScenarioSimulatorTest, despawnEntity_vehicle) EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); EXPECT_TRUE(client.call(makeSpawnVehicleEntityRequest("npc", false)).result().success()); EXPECT_TRUE(client.call(makeDespawnEntityRequest("npc")).result().success()); - - rclcpp::shutdown(); - server.join(); } /** @@ -267,9 +132,6 @@ TEST_F(ScenarioSimulatorTest, despawnEntity_pedestrian) EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); EXPECT_TRUE(client.call(makeSpawnPedestrianEntityRequest("bob")).result().success()); EXPECT_TRUE(client.call(makeDespawnEntityRequest("bob")).result().success()); - - rclcpp::shutdown(); - server.join(); } /** @@ -280,9 +142,6 @@ TEST_F(ScenarioSimulatorTest, despawnEntity_miscObject) EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); EXPECT_TRUE(client.call(makeSpawnMiscObjectEntityRequest("blob")).result().success()); EXPECT_TRUE(client.call(makeDespawnEntityRequest("blob")).result().success()); - - rclcpp::shutdown(); - server.join(); } /** @@ -292,9 +151,6 @@ TEST_F(ScenarioSimulatorTest, despawnEntity_invalidName) { EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); EXPECT_FALSE(client.call(makeDespawnEntityRequest("invalid")).result().success()); - - rclcpp::shutdown(); - server.join(); } /** @@ -304,9 +160,6 @@ TEST_F(ScenarioSimulatorTest, attachDetectionSensor) { EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); EXPECT_TRUE(client.call(makeAttachDetectionSensorRequest()).result().success()); - - rclcpp::shutdown(); - server.join(); } /** @@ -316,9 +169,6 @@ TEST_F(ScenarioSimulatorTest, attachLidarSensor) { EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); EXPECT_TRUE(client.call(makeAttachLidarSensorRequest()).result().success()); - - rclcpp::shutdown(); - server.join(); } /** @@ -328,9 +178,6 @@ TEST_F(ScenarioSimulatorTest, attachOccupancyGridSensor) { EXPECT_TRUE(client.call(makeInitializeRequest()).result().success()); EXPECT_TRUE(client.call(makeAttachOccupancyGridSensorRequest()).result().success()); - - rclcpp::shutdown(); - server.join(); } int main(int argc, char ** argv) diff --git a/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.hpp b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.hpp new file mode 100644 index 00000000000..72e4fb38738 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.hpp @@ -0,0 +1,133 @@ +// 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 + +#include +#include +#include +#include +#include +#include +#include + +auto makeInitializeRequest() -> simulation_api_schema::InitializeRequest +{ + auto request = simulation_api_schema::InitializeRequest(); + request.set_lanelet2_map_path( + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"); + return request; +} + +auto makeUpdateFrameRequest() -> simulation_api_schema::UpdateFrameRequest +{ + auto request = simulation_api_schema::UpdateFrameRequest(); + request.set_current_scenario_time(1.0); + request.set_current_simulation_time(1.0); + return request; +} + +auto makeDespawnEntityRequest(const std::string name = "name") + -> simulation_api_schema::DespawnEntityRequest +{ + auto request = simulation_api_schema::DespawnEntityRequest(); + request.set_name(name); + return request; +} + +auto makeSpawnVehicleEntityRequest(const std::string name = "name", const bool is_ego = false) + -> simulation_api_schema::SpawnVehicleEntityRequest +{ + auto request = simulation_api_schema::SpawnVehicleEntityRequest(); + const auto params = traffic_simulator_msgs::msg::VehicleParameters{}; + simulation_interface::toProto(params, *request.mutable_parameters()); + request.mutable_parameters()->set_name(name); + request.set_is_ego(is_ego); + return request; +} + +auto makeSpawnPedestrianEntityRequest(const std::string name = "name") + -> simulation_api_schema::SpawnPedestrianEntityRequest +{ + auto request = simulation_api_schema::SpawnPedestrianEntityRequest(); + const auto params = traffic_simulator_msgs::msg::PedestrianParameters{}; + simulation_interface::toProto(params, *request.mutable_parameters()); + request.mutable_parameters()->set_name(name); + return request; +} + +auto makeSpawnMiscObjectEntityRequest(const std::string name = "name") + -> simulation_api_schema::SpawnMiscObjectEntityRequest +{ + auto request = simulation_api_schema::SpawnMiscObjectEntityRequest(); + const auto params = traffic_simulator_msgs::msg::MiscObjectParameters{}; + simulation_interface::toProto(params, *request.mutable_parameters()); + request.mutable_parameters()->set_name(name); + return request; +} + +auto makeAttachDetectionSensorRequest() -> simulation_api_schema::AttachDetectionSensorRequest +{ + auto request = simulation_api_schema::AttachDetectionSensorRequest(); + auto configuration = traffic_simulator::helper::constructDetectionSensorConfiguration( + "entity_name", "awf/universe", 1.0); + *request.mutable_configuration() = configuration; + return request; +} + +auto makeAttachLidarSensorRequest() -> simulation_api_schema::AttachLidarSensorRequest +{ + auto request = simulation_api_schema::AttachLidarSensorRequest(); + auto configuration = traffic_simulator::helper::constructLidarConfiguration( + traffic_simulator::helper::LidarType::VLP16, "entity_name", "awf/universe", 1.0); + *request.mutable_configuration() = configuration; + return request; +} + +auto makeAttachOccupancyGridSensorRequest() + -> simulation_api_schema::AttachOccupancyGridSensorRequest +{ + auto request = simulation_api_schema::AttachOccupancyGridSensorRequest(); + auto configuration = simulation_api_schema::OccupancyGridSensorConfiguration(); + + configuration.set_entity("entity_name"); + configuration.set_architecture_type("awf/universe"); + configuration.set_update_duration(1.0); + configuration.set_range(300.0); + configuration.set_resolution(1.0); + *request.mutable_configuration() = configuration; + return request; +} + +class ScenarioSimulatorTest : public testing::Test +{ +protected: + ScenarioSimulatorTest() + : server([] { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + }), + client(simulation_interface::TransportProtocol::TCP, "localhost", 5555U) + { + } + std::thread server; + zeromq::MultiClient client; + ~ScenarioSimulatorTest() + { + rclcpp::shutdown(); + server.join(); + } +}; \ No newline at end of file From 61f421aa49aac7f346553d236a681a4f0369f22b Mon Sep 17 00:00:00 2001 From: robomic Date: Fri, 28 Jun 2024 13:50:23 +0200 Subject: [PATCH 118/118] simplify file and package dependencies --- .../occupancy_grid/CMakeLists.txt | 2 +- .../test_occupancy_grid_builder.cpp | 32 ++++++------------- .../test/src/test_scenario_simulator.hpp | 26 ++++++--------- .../test/src/utils/helper_functions.hpp | 26 +++++++++++++-- 4 files changed, 45 insertions(+), 41 deletions(-) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt index a8dad91304f..082937dc6f7 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/CMakeLists.txt @@ -2,4 +2,4 @@ ament_add_gtest(test_grid_traversal test_grid_traversal.cpp) target_link_libraries(test_grid_traversal simple_sensor_simulator_component) ament_add_gtest(test_occupancy_grid_builder test_occupancy_grid_builder.cpp) -target_link_libraries(test_occupancy_grid_builder simple_sensor_simulator_component) +target_link_libraries(test_occupancy_grid_builder simple_sensor_simulator_component ${Protobuf_LIBRARIES}) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp index d065c1668b9..3fcbcd511a2 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/occupancy_grid/test_occupancy_grid_builder.cpp @@ -20,19 +20,7 @@ #include #include -auto makePose(const double x, const double y) -> geometry_msgs::msg::Pose -{ - return geometry_msgs::build() - .position(geometry_msgs::build().x(x).y(y).z(0.0)) - .orientation( - geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0)); -} - -auto makeBox(const float size, const double x, const double y) - -> simple_sensor_simulator::primitives::Box -{ - return simple_sensor_simulator::primitives::Box(size, size, size, makePose(x, y)); -} +#include "../../utils/helper_functions.hpp" /** * @note Test function behavior when called after the limit has been reached. @@ -43,9 +31,9 @@ TEST(OccupancyGridBuilder, add_overLimit) auto builder = simple_sensor_simulator::OccupancyGridBuilder( 0.1, static_cast(20), static_cast(40)); for (int i = 0; i < static_cast(std::numeric_limits::max()); ++i) { - builder.add(makeBox(5.0f, 0.0, 0.0)); + builder.add(utils::makeBox(5.0f, 0.0, 0.0)); } - EXPECT_THROW(builder.add(makeBox(5.0f, 0.0, 0.0)), std::runtime_error); + EXPECT_THROW(builder.add(utils::makeBox(5.0f, 0.0, 0.0)), std::runtime_error); } /** @@ -72,8 +60,8 @@ TEST(OccupancyGridBuilder, add_primitiveInside) auto builder = simple_sensor_simulator::OccupancyGridBuilder( 1.0, static_cast(10), static_cast(20), static_cast(2), static_cast(1)); - builder.add(makeBox(5.0f, 4.0, 0.0)); - builder.add(makeBox(2.0f, -5.0, 0.0)); + builder.add(utils::makeBox(5.0f, 4.0, 0.0)); + builder.add(utils::makeBox(2.0f, -5.0, 0.0)); builder.build(); @@ -104,8 +92,8 @@ TEST(OccupancyGridBuilder, add_primitiveProtruding) auto builder = simple_sensor_simulator::OccupancyGridBuilder( 1.0, static_cast(10), static_cast(20), static_cast(2), static_cast(1)); - builder.add(makeBox(5.0f, 9.0, -4.0)); - builder.add(makeBox(2.0f, -10.0, 3.0)); + builder.add(utils::makeBox(5.0f, 9.0, -4.0)); + builder.add(utils::makeBox(2.0f, -10.0, 3.0)); builder.build(); @@ -148,7 +136,7 @@ TEST(OccupancyGridBuilder, add_primitiveInCenter) auto builder = simple_sensor_simulator::OccupancyGridBuilder( 1.0, static_cast(10), static_cast(20), static_cast(2), static_cast(1)); - builder.add(makeBox(5.0f, 0.0, 0.0)); + builder.add(utils::makeBox(5.0f, 0.0, 0.0)); builder.build(); @@ -179,8 +167,8 @@ TEST(OccupancyGridBuilder, reset) auto builder = simple_sensor_simulator::OccupancyGridBuilder( 1.0, static_cast(10), static_cast(20), static_cast(2), static_cast(1)); - builder.add(makeBox(5.0f, 0.0, 0.0)); - builder.reset(makePose(0.0, 0.0)); + builder.add(utils::makeBox(5.0f, 0.0, 0.0)); + builder.reset(utils::makePose(0.0, 0.0)); builder.build(); diff --git a/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.hpp b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.hpp index 72e4fb38738..650342dd3ed 100644 --- a/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.hpp +++ b/simulation/simple_sensor_simulator/test/src/test_scenario_simulator.hpp @@ -20,13 +20,15 @@ #include #include #include -#include + +#include "utils/helper_functions.hpp" auto makeInitializeRequest() -> simulation_api_schema::InitializeRequest { auto request = simulation_api_schema::InitializeRequest(); request.set_lanelet2_map_path( - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"); + ament_index_cpp::get_package_share_directory("simple_sensor_simulator") + + "/map/lanelet2_map.osm"); return request; } @@ -80,18 +82,16 @@ auto makeSpawnMiscObjectEntityRequest(const std::string name = "name") auto makeAttachDetectionSensorRequest() -> simulation_api_schema::AttachDetectionSensorRequest { auto request = simulation_api_schema::AttachDetectionSensorRequest(); - auto configuration = traffic_simulator::helper::constructDetectionSensorConfiguration( - "entity_name", "awf/universe", 1.0); - *request.mutable_configuration() = configuration; + *request.mutable_configuration() = + utils::constructDetectionSensorConfiguration("entity_name", "awf/universe", 1.0); return request; } auto makeAttachLidarSensorRequest() -> simulation_api_schema::AttachLidarSensorRequest { auto request = simulation_api_schema::AttachLidarSensorRequest(); - auto configuration = traffic_simulator::helper::constructLidarConfiguration( - traffic_simulator::helper::LidarType::VLP16, "entity_name", "awf/universe", 1.0); - *request.mutable_configuration() = configuration; + *request.mutable_configuration() = + utils::constructLidarConfiguration("entity_name", "awf/universe", 0.0, 1.0 / 180.0 * M_PI); return request; } @@ -99,14 +99,8 @@ auto makeAttachOccupancyGridSensorRequest() -> simulation_api_schema::AttachOccupancyGridSensorRequest { auto request = simulation_api_schema::AttachOccupancyGridSensorRequest(); - auto configuration = simulation_api_schema::OccupancyGridSensorConfiguration(); - - configuration.set_entity("entity_name"); - configuration.set_architecture_type("awf/universe"); - configuration.set_update_duration(1.0); - configuration.set_range(300.0); - configuration.set_resolution(1.0); - *request.mutable_configuration() = configuration; + *request.mutable_configuration() = + utils::constructOccupancyGridSensorConfiguration("entity_name", "awf/universe", 1.0); return request; } 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 881249afd5a..842e5774024 100644 --- a/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp +++ b/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -43,8 +44,8 @@ inline auto makePoint(const double px, const double py, const double pz) } inline auto makePose( - const double px, const double py, const double pz, const double ox, const double oy, - const double oz, const double ow) -> geometry_msgs::msg::Pose + const double px, const double py, const double pz = 0.0, const double ox = 0.0, + const double oy = 0.0, const double oz = 0.0, const double ow = 1.0) -> geometry_msgs::msg::Pose { return geometry_msgs::build() .position(geometry_msgs::build().x(px).y(py).z(pz)) @@ -57,6 +58,12 @@ inline auto makeDimensions(const double x, const double y, const double z) return geometry_msgs::build().x(x).y(y).z(z); } +inline auto makeBox(const float size, const double x, const double y) + -> simple_sensor_simulator::primitives::Box +{ + return simple_sensor_simulator::primitives::Box(size, size, size, utils::makePose(x, y)); +} + 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, @@ -111,6 +118,21 @@ inline auto constructLidarConfiguration( return configuration; } +inline auto constructOccupancyGridSensorConfiguration( + const std::string & entity, const std::string & architecture_type, + const double horizontal_resolution) + -> const simulation_api_schema::OccupancyGridSensorConfiguration +{ + auto configuration = simulation_api_schema::OccupancyGridSensorConfiguration(); + + configuration.set_entity(entity); + configuration.set_architecture_type(architecture_type); + configuration.set_update_duration(1.0); + configuration.set_range(300.0); + configuration.set_resolution(horizontal_resolution); + return configuration; +} + inline auto createEntityStatus( const std::string & name, const EntityType::Enum type, const std::optional & subtype, const geometry_msgs::msg::Pose & pose,