diff --git a/.github/workflows/BuildAndRun.yaml b/.github/workflows/BuildAndRun.yaml index d261ca86f8b..f2fd62c574e 100644 --- a/.github/workflows/BuildAndRun.yaml +++ b/.github/workflows/BuildAndRun.yaml @@ -93,14 +93,12 @@ jobs: run: | source install/setup.bash source install/local_setup.bash - ros2 launch scenario_test_runner scenario_test_runner.launch.py workflow:='$(find-pkg-share scenario_test_runner)/config/workflow_example.yaml' global_frame_rate:=20 - ros2 run scenario_test_runner result_checker.py /tmp/scenario_test_runner/result.junit.xml + ./src/scenario_simulator_v2/.github/workflows/workflow.sh ./src/scenario_simulator_v2/test_runner/scenario_test_runner/config/workflow.txt global_frame_rate:=20 shell: bash - - name: Basic test - run: | - source install/setup.bash - source install/local_setup.bash - ros2 launch scenario_test_runner scenario_test_runner.launch.py workflow:='$(find-pkg-share scenario_simulator_v2_scenarios)/workflow/basic.yaml' - ros2 run scenario_test_runner result_checker.py /tmp/scenario_test_runner/result.junit.xml - shell: bash +# - name: Basic test +# run: | +# source install/setup.bash +# source install/local_setup.bash +# ./src/scenario_simulator_v2/.github/workflows/workflow.sh ./src/scenario_simulator_v2_scenarios/workflow/basic.txt +# shell: bash diff --git a/.github/workflows/workflow.sh b/.github/workflows/workflow.sh new file mode 100755 index 00000000000..736b7561e97 --- /dev/null +++ b/.github/workflows/workflow.sh @@ -0,0 +1,27 @@ +#!/bin/sh + +file_path="$1" + +# Arguments other than file path are passed to the command's arguments +shift 1 + +if [ ! -f "$file_path" ] +then + echo "No such file: $file_path" + exit 1 +fi + +exit_status=0 + +while IFS= read -r line +do + ros2 launch scenario_test_runner scenario_test_runner.launch.py scenario:="$line" "$@" + ros2 run scenario_test_runner result_checker.py /tmp/scenario_test_runner/result.junit.xml + command_exit_status=$? + if [ $command_exit_status -ne 0 ]; then + echo "Error: caught non-zero exit status（code: $command_exit_status)" + exit_status=1 + fi +done < "$file_path" + +exit $exit_status diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index b086d1c751c..ea8293a79cf 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package arithmetic ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index d24a2611e3b..52c78ae6a05 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 1.13.0 + 1.14.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 4c2fa75f35e..4aec92bc30d 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package geometry ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- +* Merge pull request `#1229 `_ from tier4/feature/follow_trajectory_action_in_do_nothing_plugin + add follow trajectory action in do_nothing_plugin +* add follow trajectory action in do_nothing_plugin +* Contributors: Masaya Kataoka, Tatsuya Yamasaki + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/common/math/geometry/include/geometry/linear_algebra.hpp b/common/math/geometry/include/geometry/linear_algebra.hpp index c52ec730b53..a520ff1a6c6 100644 --- a/common/math/geometry/include/geometry/linear_algebra.hpp +++ b/common/math/geometry/include/geometry/linear_algebra.hpp @@ -38,6 +38,8 @@ double getInternalAngle( geometry_msgs::msg::Vector3 operator/(const geometry_msgs::msg::Vector3 & vec, double value); geometry_msgs::msg::Vector3 operator*(const geometry_msgs::msg::Vector3 & vec, double value); geometry_msgs::msg::Vector3 operator*(double value, const geometry_msgs::msg::Vector3 & vec); +geometry_msgs::msg::Point operator*(const geometry_msgs::msg::Point & point, const double value); +geometry_msgs::msg::Point operator*(const double value, const geometry_msgs::msg::Point & point); geometry_msgs::msg::Point operator+( const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Vector3 & v1); geometry_msgs::msg::Vector3 operator+( diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 8c14fe22590..40504a47bba 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 1.13.0 + 1.14.1 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/math/geometry/src/linear_algebra.cpp b/common/math/geometry/src/linear_algebra.cpp index 0db41afa163..e80a992ef03 100644 --- a/common/math/geometry/src/linear_algebra.cpp +++ b/common/math/geometry/src/linear_algebra.cpp @@ -90,6 +90,19 @@ geometry_msgs::msg::Vector3 operator*(double value, const geometry_msgs::msg::Ve return vec * value; } +geometry_msgs::msg::Point operator*(const geometry_msgs::msg::Point & point, const double value) +{ + return geometry_msgs::build() + .x(point.x * value) + .y(point.z * value) + .z(point.z * value); +} + +geometry_msgs::msg::Point operator*(const double value, const geometry_msgs::msg::Point & point) +{ + return point * value; +} + geometry_msgs::msg::Point operator+( const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Vector3 & v1) { diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 88d04b0d7ee..0ba7d7b54a9 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package scenario_simulator_exception ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index f52795d05ff..d8911cca39b 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 1.13.0 + 1.14.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 bc1016a896c..592afb417df 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package junit_exporter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index e53ac47a22b..412609e0f05 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 1.13.0 + 1.14.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 28bb8d76422..08c7866d993 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package status_monitor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index b5440e386de..b1fbc80ecf3 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 1.13.0 + 1.14.1 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 176c82b06e4..4f319aa7d9d 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package concealer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 418cc21cba2..6f5e5fb7441 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 1.13.0 + 1.14.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 a43b3b59ab4..2c43f42b25f 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -5,6 +5,12 @@ Changelog for package embree_vendor 0.0.9 (2021-07-13) ------------------ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 1872b962790..a70c81a5abb 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 1.13.0 + 1.14.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 b6bd21d55b5..3ee1b2d89ff 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package kashiwanoha_map ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 26178193b9b..b8d31589c01 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 1.13.0 + 1.14.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 a3f8685b12b..f5861a347ca 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package cpp_mock_scenarios ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- +* Merge pull request `#1229 `_ from tier4/feature/follow_trajectory_action_in_do_nothing_plugin + add follow trajectory action in do_nothing_plugin +* add follow trajectory action in do_nothing_plugin +* Contributors: Masaya Kataoka, Tatsuya Yamasaki + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/mock/cpp_mock_scenarios/CMakeLists.txt b/mock/cpp_mock_scenarios/CMakeLists.txt index 9d6e37a592a..3088392fb53 100644 --- a/mock/cpp_mock_scenarios/CMakeLists.txt +++ b/mock/cpp_mock_scenarios/CMakeLists.txt @@ -45,6 +45,7 @@ if(BUILD_CPP_MOCK_SCENARIOS) add_subdirectory(src/crosswalk) add_subdirectory(src/follow_front_entity) add_subdirectory(src/follow_lane) + add_subdirectory(src/follow_trajectory) add_subdirectory(src/lane_change) add_subdirectory(src/measurement) add_subdirectory(src/merge) diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index a6ce860b0d9..d7fe27c2816 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 1.13.0 + 1.14.1 C++ mock scenarios masaya Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/src/follow_trajectory/CMakeLists.txt b/mock/cpp_mock_scenarios/src/follow_trajectory/CMakeLists.txt new file mode 100644 index 00000000000..f4bc1752f55 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/follow_trajectory/CMakeLists.txt @@ -0,0 +1,14 @@ +ament_auto_add_executable(follow_polyline_trajectory_with_do_nothing_plugin +follow_polyline_trajectory_with_do_nothing_plugin.cpp +) +target_link_libraries(follow_polyline_trajectory_with_do_nothing_plugin cpp_scenario_node) + +install(TARGETS + follow_polyline_trajectory_with_do_nothing_plugin + DESTINATION lib/cpp_mock_scenarios +) + +if(BUILD_TESTING) + include(../../cmake/add_cpp_mock_scenario_test.cmake) + add_cpp_mock_scenario_test(${PROJECT_NAME} "follow_polyline_trajectory_with_do_nothing_plugin" "15.0") +endif() 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 new file mode 100644 index 00000000000..6bf26f4bfc6 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp @@ -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. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cpp_mock_scenarios +{ +class FollowPolylineTrajectoryWithDoNothingPluginScenario +: public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit FollowPolylineTrajectoryWithDoNothingPluginScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "lanechange_left", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", + "lanelet2_map.osm", __FILE__, false, option), + spawn_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))), + trajectory_start_pose( + geometry_msgs::build() + .position(geometry_msgs::build().x(10).y(0).z(0)) + .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1))), + trajectory_waypoint_pose( + geometry_msgs::build() + .position(geometry_msgs::build().x(15).y(0).z(0)) + .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1))), + trajectory_goal_pose( + geometry_msgs::build() + .position(geometry_msgs::build().x(20).y(0).z(0)) + .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1))) + { + start(); + } + +private: + bool requested = false; + const geometry_msgs::msg::Pose spawn_pose; + const geometry_msgs::msg::Pose trajectory_start_pose; + const geometry_msgs::msg::Pose trajectory_waypoint_pose; + const geometry_msgs::msg::Pose trajectory_goal_pose; + + void onUpdate() override + { + // LCOV_EXCL_START + if (api_.getCurrentTime() >= 10.0) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + // LCOV_EXCL_STOP + if (equals(api_.getCurrentTime(), 0.0, 0.01) && !api_.reachPosition("ego", spawn_pose, 0.1)) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + if ( + equals(api_.getCurrentTime(), 1.0, 0.01) && + !api_.reachPosition("ego", trajectory_start_pose, 0.1)) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + if ( + equals(api_.getCurrentTime(), 1.5, 0.01) && + !api_.reachPosition("ego", trajectory_waypoint_pose, 0.1)) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + if ( + equals(api_.getCurrentTime(), 2.0, 0.01) && + !api_.reachPosition("ego", trajectory_goal_pose, 0.1)) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + if (equals(api_.getCurrentTime(), 2.5, 0.01)) { + if (api_.reachPosition("ego", trajectory_goal_pose, 0.1)) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } else { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + } + void onInitialize() override + { + api_.spawn( + "ego", spawn_pose, getVehicleParameters(), + traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing()); + api_.setLinearVelocity("ego", 10); + api_.requestSpeedChange("ego", 10, true); + api_.requestFollowTrajectory( + "ego", + std::make_shared( + traffic_simulator_msgs::build() + .initial_distance_offset(0.0) + .dynamic_constraints_ignorable(true) + .base_time(0.0) + .closed(false) + .shape(traffic_simulator_msgs::build().vertices( + [this]() { + std::vector vertices; + vertices.emplace_back( + traffic_simulator_msgs::build() + .time(1.0) + .position(trajectory_start_pose)); + vertices.emplace_back( + traffic_simulator_msgs::build() + .time(1.5) + .position(trajectory_waypoint_pose)); + vertices.emplace_back( + traffic_simulator_msgs::build() + .time(2.0) + .position(trajectory_goal_pose)); + return vertices; + }())))); + } +}; +} // namespace cpp_mock_scenarios + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = + std::make_shared( + options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 7ab7887dd3d..5e2a41c0905 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package openscenario_experimental_catalog ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index a55bfd90299..526439bd486 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 1.13.0 + 1.14.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 478c11c50d1..ea882bca195 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package openscenario_interpreter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge pull request `#1216 `_ from tier4/feature/routing-algorithm diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 40f4e4f6d52..301de7ee4e7 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 1.13.0 + 1.14.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 7ce87ad787a..ab12316e289 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package openscenario_interpreter_example ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 23441cfa8bd..f0a25ced9a0 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 1.13.0 + 1.14.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 b2b1c066e7b..9b3547b3dcd 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package openscenario_interpreter_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index d60681e5133..558de8f53dc 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 1.13.0 + 1.14.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 2a00d7f1757..f61d2e058ce 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package openscenario_preprocessor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index b976b71f769..14dafcb4ab5 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 1.13.0 + 1.14.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 9405bc72b5e..80549c1d97f 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package openscenario_preprocessor_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index 9861cef9659..ac06bd3aa77 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 1.13.0 + 1.14.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 6bc8f7c4c92..5277c66864f 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package openscenario_utility ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/openscenario/openscenario_utility/openscenario_utility/scenario_validation.py b/openscenario/openscenario_utility/openscenario_utility/scenario_validation.py index 7fecdc6395c..6a987509751 100755 --- a/openscenario/openscenario_utility/openscenario_utility/scenario_validation.py +++ b/openscenario/openscenario_utility/openscenario_utility/scenario_validation.py @@ -20,7 +20,6 @@ from pkg_resources import resource_string from sys import exit, modules from openscenario_utility.conversion import convert -from scenario_test_runner.workflow import Workflow import argparse import xmlschema @@ -105,30 +104,6 @@ def validate_file( overall_result = False print(str(path) + " : " + str(overall_result)) - -def validate_workflow(): - - schema = xmlschema.XMLSchema( - resource_string(__name__, "resources/OpenSCENARIO-1.2.xsd").decode("utf-8") - ) - - parser = argparse.ArgumentParser( - description="Validate if the Workflow Files pass the conditions checked by validators" - ) - parser.add_argument("--workflow_paths", type=Path, nargs="+") - parser.add_argument("--validators", nargs="+") - args = parser.parse_args() - - scenario_validators = list() - for validator in args.validators: - scenario_validators.append(getattr(modules[__name__], validator)()) - - for path in args.workflow_paths: - workflow = Workflow(path, 30.0) - for scenario in workflow.scenarios: - validate_file(scenario.path, schema, scenario_validators) - - def validate_scenario(): schema = xmlschema.XMLSchema( diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 06bf71b47a7..42b03160cf6 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 1.13.0 + 1.14.1 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/poetry.lock b/poetry.lock index fedbe82fea3..dce42c64d3a 100644 --- a/poetry.lock +++ b/poetry.lock @@ -1,4 +1,4 @@ -# This file is automatically @generated by Poetry 1.8.1 and should not be changed by hand. +# This file is automatically @generated by Poetry 1.8.2 and should not be changed by hand. [[package]] name = "babel" @@ -239,13 +239,13 @@ pyparsing = {version = ">=2.4.2,<3.0.0 || >3.0.0,<3.0.1 || >3.0.1,<3.0.2 || >3.0 [[package]] name = "idna" -version = "3.4" +version = "3.7" description = "Internationalized Domain Names in Applications (IDNA)" optional = false python-versions = ">=3.5" files = [ - {file = "idna-3.4-py3-none-any.whl", hash = "sha256:90b77e79eaa3eba6de819a0c442c0b4ceefc341a7a2ab77d7562bf49f425c5c2"}, - {file = "idna-3.4.tar.gz", hash = "sha256:814f528e8dead7d329833b91c5faa87d60bf71824cd12a7530b5526063d02cb4"}, + {file = "idna-3.7-py3-none-any.whl", hash = "sha256:82fee1fc78add43492d3a1898bfa6d8a904cc97d8427f683ed8e798d07761aa0"}, + {file = "idna-3.7.tar.gz", hash = "sha256:028ff3aadf0609c1fd278d8ea3089299412a7a8b9bd005dd08b9f8285bcb5cfc"}, ] [[package]] diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 2e328d9952e..0f17b400f1d 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package openscenario_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 0ca96c76647..c064b5cad0e 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 1.13.0 + 1.14.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 31dbb27fa4d..72c59d8b2e7 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package real_time_factor_control_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm 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 cb8dabca536..3a8022a08e3 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 - 1.13.0 + 1.14.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 6968395a49f..f37de2dfeb3 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package scenario_simulator_v2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 53d9c4e0f59..01bb587efbe 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 1.13.0 + 1.14.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 a3ad2a4e23a..da589b5f5f2 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package behavior_tree_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 3880a452e00..3ddc36fb7b0 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 1.13.0 + 1.14.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 fa037878230..14aef275d69 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package do_nothing_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- +* Merge pull request `#1229 `_ from tier4/feature/follow_trajectory_action_in_do_nothing_plugin + add follow trajectory action in do_nothing_plugin +* add follow trajectory action in do_nothing_plugin +* Contributors: Masaya Kataoka, Tatsuya Yamasaki + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp b/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp index 4c6b2a95baa..b4f1284bddc 100644 --- a/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp +++ b/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp @@ -48,15 +48,12 @@ public: \ DEFINE_GETTER_SETTER(DebugMarker, std::vector) DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, double) DEFINE_GETTER_SETTER(EntityTypeList, EntityTypeDict) - DEFINE_GETTER_SETTER(PolylineTrajectory, std::shared_ptr) DEFINE_GETTER_SETTER(GoalPoses, std::vector) - DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr) DEFINE_GETTER_SETTER(LaneChangeParameters, traffic_simulator::lane_change::Parameter) DEFINE_GETTER_SETTER(Obstacle, std::optional) DEFINE_GETTER_SETTER(OtherEntityStatus, EntityStatusDict) DEFINE_GETTER_SETTER(PedestrianParameters, traffic_simulator_msgs::msg::PedestrianParameters) DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr) - DEFINE_GETTER_SETTER(Request, traffic_simulator::behavior::Request) DEFINE_GETTER_SETTER(RouteLanelets, lanelet::Ids) DEFINE_GETTER_SETTER(TargetSpeed, std::optional) DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) @@ -74,11 +71,14 @@ public: \ private: \ TYPE FIELD_NAME; // clang-format off - DEFINE_GETTER_SETTER(CurrentTime, double, current_time_) - DEFINE_GETTER_SETTER(StepTime, double, step_time_) - DEFINE_GETTER_SETTER(EntityStatus, std::shared_ptr, entity_status_) - DEFINE_GETTER_SETTER(BehaviorParameter, traffic_simulator_msgs::msg::BehaviorParameter, behavior_parameter_) - DEFINE_GETTER_SETTER(UpdatedStatus, std::shared_ptr, updated_status_) + DEFINE_GETTER_SETTER(BehaviorParameter, traffic_simulator_msgs::msg::BehaviorParameter, behavior_parameter_) + DEFINE_GETTER_SETTER(CurrentTime, double, current_time_) + DEFINE_GETTER_SETTER(EntityStatus, std::shared_ptr, entity_status_) + DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr, hdmap_utils_) + DEFINE_GETTER_SETTER(PolylineTrajectory, std::shared_ptr, polyline_trajectory) + DEFINE_GETTER_SETTER(Request, traffic_simulator::behavior::Request, request) + DEFINE_GETTER_SETTER(StepTime, double, step_time_) + DEFINE_GETTER_SETTER(UpdatedStatus, std::shared_ptr, updated_status_) // clang-format on #undef DEFINE_GETTER_SETTER }; diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 34ef67f04e9..ee0c96b12aa 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 1.13.0 + 1.14.1 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/do_nothing_plugin/src/plugin.cpp b/simulation/do_nothing_plugin/src/plugin.cpp index fd044c728b6..76a4baec2fe 100644 --- a/simulation/do_nothing_plugin/src/plugin.cpp +++ b/simulation/do_nothing_plugin/src/plugin.cpp @@ -13,15 +13,156 @@ // limitations under the License. #include +#include +#include namespace entity_behavior { +namespace do_nothing_behavior +{ +namespace follow_trajectory +{ +bool checkPolylineTrajectory( + const std::shared_ptr & trajectory) +{ + if (trajectory) { + if (trajectory->closed) { + THROW_SIMULATION_ERROR("Currently, closed trajectory does not supported."); + } + if (!trajectory->dynamic_constraints_ignorable) { + THROW_SIMULATION_ERROR( + "Currently, dynamic_constraints_ignorable = true (in OpenSCENARIO, followingMode = " + "follow) does not support in DoNothingBehavior."); + } + if (std::abs(trajectory->initial_distance_offset) > std::numeric_limits::epsilon()) { + THROW_SIMULATION_ERROR( + "Currently, initial_distance_offset should be 0 when following trajectory in " + "DoNothingBehavior.", + "You specified : ", trajectory->initial_distance_offset); + } + if (trajectory->shape.vertices.size() <= 1) { + THROW_SIMULATION_ERROR( + "FollowPolylineTrajectory is requested, but vertex points are less than 1 point.", + "At least 2 vertex points are required.", "Please check description of the scenario."); + } + } else { + THROW_SIMULATION_ERROR( + "Traffic simulator send requests of FollowTrajectory, but the trajectory is empty.", + "This message is not originally intended to be displayed, if you see it, please " + "contact the developer of traffic_simulator."); + } + return true; +} + +auto getLastVertexTimestamp( + const std::shared_ptr & trajectory) + -> std::optional +{ + checkPolylineTrajectory(trajectory); + return trajectory->base_time + trajectory->shape.vertices.back().time; +} + +auto interpolateEntityStatusFromPolylineTrajectory( + const std::shared_ptr & trajectory, + const std::shared_ptr & entity_status, + double current_time, double step_time) -> std::optional +{ + if (!trajectory) { + return std::nullopt; + } + + const auto interpolate_entity_status = + [&]( + const double interpolation_ratio, const traffic_simulator_msgs::msg::Vertex & v0, + const traffic_simulator_msgs::msg::Vertex & v1) -> traffic_simulator_msgs::msg::EntityStatus { + auto interpolated_entity_status = + static_cast(*entity_status); + interpolated_entity_status.lanelet_pose_valid = false; + interpolated_entity_status.lanelet_pose = traffic_simulator_msgs::msg::LaneletPose(); + interpolated_entity_status.pose = + geometry_msgs::build() + .position( + v0.position.position * (1 - interpolation_ratio) + + v1.position.position * interpolation_ratio) + .orientation(quaternion_operation::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); + const auto linear_acceleration = + (entity_status->getTwist().linear.x - linear_velocity) / (v1.time - v0.time); + const auto linear_jerk = + (entity_status->getAccel().linear.x - linear_acceleration) / (v1.time - v0.time); + + interpolated_entity_status.action_status.twist = + geometry_msgs::build() + .linear(geometry_msgs::build().x(linear_velocity).y(0).z(0)) + .angular(geometry_msgs::build().x(0).y(0).z(0)); + interpolated_entity_status.action_status.accel = + geometry_msgs::build() + .linear( + geometry_msgs::build().x(linear_acceleration).y(0).z(0)) + .angular(geometry_msgs::build().x(0).y(0).z(0)); + interpolated_entity_status.action_status.linear_jerk = linear_jerk; + return interpolated_entity_status; + }; + + if ((current_time + step_time) <= trajectory->shape.vertices.begin()->time) { + return std::nullopt; + } + if (trajectory->shape.vertices.back().time <= (current_time + step_time)) { + return interpolate_entity_status( + 1, *std::prev(trajectory->shape.vertices.end(), 2), + *std::prev(trajectory->shape.vertices.end(), 1)); + } + if (const auto vertex_iter = std::adjacent_find( + trajectory->shape.vertices.begin(), trajectory->shape.vertices.end(), + [&](const auto & vertex_a, const auto & vertex_b) { + return (trajectory->base_time + vertex_a.time) <= (current_time + step_time) and + (current_time + step_time) <= (trajectory->base_time + vertex_b.time); + }); + vertex_iter != trajectory->shape.vertices.end()) { + return interpolate_entity_status( + (current_time + step_time - trajectory->base_time - vertex_iter->time) / + (std::next(vertex_iter)->time - vertex_iter->time), + *vertex_iter, *std::next(vertex_iter)); + } else { + return std::nullopt; + } +} +} // namespace follow_trajectory +} // namespace do_nothing_behavior + void DoNothingBehavior::configure(const rclcpp::Logger &) {} -void DoNothingBehavior::update(double current_time, double) +void DoNothingBehavior::update(double current_time, double step_time) { + setCurrentTime(current_time); + setStepTime(step_time); + + const auto interpolate_entity_status_on_polyline_trajectory = [&]() { + do_nothing_behavior::follow_trajectory::checkPolylineTrajectory(getPolylineTrajectory()); + if ( + const auto interpolated_status = + do_nothing_behavior::follow_trajectory::interpolateEntityStatusFromPolylineTrajectory( + getPolylineTrajectory(), getEntityStatus(), getCurrentTime(), getStepTime())) { + return std::make_shared( + traffic_simulator::CanonicalizedEntityStatus(interpolated_status.value(), getHdMapUtils())); + } else { + return entity_status_; + } + }; + entity_status_->setTime(current_time); - setUpdatedStatus(entity_status_); + if (getRequest() == traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY) { + setUpdatedStatus(interpolate_entity_status_on_polyline_trajectory()); + if ( + getCurrentTime() + getStepTime() >= + do_nothing_behavior::follow_trajectory::getLastVertexTimestamp(getPolylineTrajectory())) { + setRequest(traffic_simulator::behavior::Request::NONE); + } + } else { + setUpdatedStatus(entity_status_); + } } const std::string & DoNothingBehavior::getCurrentAction() const diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 97a4798aac5..b5f2833b869 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package simple_sensor_simulator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 9e21645cf4a..29425440717 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 1.13.0 + 1.14.1 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 34d398d83df..945d95be16e 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package simulation_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index be62ce761ee..8f66353480c 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 1.13.0 + 1.14.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 bb7355da9b1..d42d1e82854 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package traffic_simulator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge pull request `#1216 `_ from tier4/feature/routing-algorithm diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 52d8d3f512a..b8ef8e01255 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 1.13.0 + 1.14.1 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 1689192fbb5..90e41e2f2ff 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package openscenario_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index e2b23d3a01d..0856f98ae9d 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 1.13.0 + 1.14.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 1f68b048379..95511258255 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package random_test_runner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge remote-tracking branch 'origin/feature/routing-algorithm' into feature/routing-algorithm diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index cc6688a8527..dcddb2d9c2b 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 1.13.0 + 1.14.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 2725fe8a7c3..4f7b3f6fbb2 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package scenario_test_runner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.14.1 (2024-04-12) +------------------- + +1.14.0 (2024-04-12) +------------------- + 1.13.0 (2024-04-11) ------------------- * Merge pull request `#1216 `_ from tier4/feature/routing-algorithm diff --git a/test_runner/scenario_test_runner/config/workflow.txt b/test_runner/scenario_test_runner/config/workflow.txt new file mode 100644 index 00000000000..786a9c1d113 --- /dev/null +++ b/test_runner/scenario_test_runner/config/workflow.txt @@ -0,0 +1,18 @@ +$(find-pkg-share scenario_test_runner)/scenario/set_behavior_parameters_in_object_controller.yaml +$(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml +$(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml +$(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.DistanceCondition.yaml +$(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.DistanceCondition.Shortest.yaml +$(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeDistanceCondition.yaml +$(find-pkg-share scenario_test_runner)/scenario/ControllerAction.AssignControllerAction.yaml +$(find-pkg-share scenario_test_runner)/scenario/LongitudinalAction.SpeedAction.yaml +$(find-pkg-share scenario_test_runner)/scenario/Property.isBlind.yaml +$(find-pkg-share scenario_test_runner)/scenario/RoutingAction.FollowTrajectoryAction-star.yaml +$(find-pkg-share scenario_test_runner)/scenario/RoutingAction.FollowTrajectoryAction-straight.yaml +$(find-pkg-share scenario_test_runner)/scenario/RoutingAction.FollowTrajectoryAction-straight-pedestrian.yaml +$(find-pkg-share scenario_test_runner)/scenario/RoutingAction.FollowTrajectoryAction-straight-bicycle.yaml +$(find-pkg-share scenario_test_runner)/scenario/RoutingAction.FollowTrajectoryAction-zero-distance.yaml +$(find-pkg-share scenario_test_runner)/scenario/all-in-one.yaml +$(find-pkg-share scenario_test_runner)/scenario/minimal.yaml +$(find-pkg-share scenario_test_runner)/scenario/prefixed-name-reference.yaml +$(find-pkg-share scenario_test_runner)/scenario/stand-still.yaml diff --git a/test_runner/scenario_test_runner/config/workflow_example.yaml b/test_runner/scenario_test_runner/config/workflow_example.yaml deleted file mode 100644 index fb881c8e7e5..00000000000 --- a/test_runner/scenario_test_runner/config/workflow_example.yaml +++ /dev/null @@ -1,22 +0,0 @@ -Scenario: - # - { path: $(find-pkg-share scenario_test_runner)/scenario/collision.yaml } - # - { path: $(find-pkg-share scenario_test_runner)/scenario/distance-condition.yaml } - # - { path: $(find-pkg-share scenario_test_runner)/scenario/success.yaml} - - path: $(find-pkg-share scenario_test_runner)/scenario/set_behavior_parameters_in_object_controller.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.DistanceCondition.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.DistanceCondition.Shortest.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeDistanceCondition.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/ControllerAction.AssignControllerAction.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/LongitudinalAction.SpeedAction.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/Property.isBlind.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/RoutingAction.FollowTrajectoryAction-star.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/RoutingAction.FollowTrajectoryAction-straight.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/RoutingAction.FollowTrajectoryAction-straight-pedestrian.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/RoutingAction.FollowTrajectoryAction-straight-bicycle.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/RoutingAction.FollowTrajectoryAction-zero-distance.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/all-in-one.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/minimal.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/prefixed-name-reference.yaml - - path: $(find-pkg-share scenario_test_runner)/scenario/stand-still.yaml diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index b77ee4fc93f..22e460b7f66 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -81,7 +81,6 @@ def launch_setup(context, *args, **kwargs): sigterm_timeout = LaunchConfiguration("sigterm_timeout", default=8) use_sim_time = LaunchConfiguration("use_sim_time", default=False) vehicle_model = LaunchConfiguration("vehicle_model", default="") - workflow = LaunchConfiguration("workflow", default=Path("/dev/null")) # fmt: on print(f"architecture_type := {architecture_type.perform(context)}") @@ -104,7 +103,6 @@ def launch_setup(context, *args, **kwargs): print(f"sigterm_timeout := {sigterm_timeout.perform(context)}") print(f"use_sim_time := {use_sim_time.perform(context)}") print(f"vehicle_model := {vehicle_model.perform(context)}") - print(f"workflow := {workflow.perform(context)}") def make_parameters(): parameters = [ @@ -157,7 +155,6 @@ def description(): DeclareLaunchArgument("sigterm_timeout", default_value=sigterm_timeout ), DeclareLaunchArgument("use_sim_time", default_value=use_sim_time ), DeclareLaunchArgument("vehicle_model", default_value=vehicle_model ), - DeclareLaunchArgument("workflow", default_value=workflow ), # fmt: on Node( package="scenario_test_runner", @@ -173,7 +170,6 @@ def description(): "--global-timeout", global_timeout, "--output-directory", output_directory, "--scenario", scenario, - "--workflow", workflow, # fmt: on ], ), diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 592aaa02d74..2047055c462 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 1.13.0 + 1.14.1 scenario test runner package Tatsuya Yamasaki Apache License 2.0 diff --git a/test_runner/scenario_test_runner/scenario_test_runner/scenario.py b/test_runner/scenario_test_runner/scenario_test_runner/scenario.py new file mode 100755 index 00000000000..d30b6373a7c --- /dev/null +++ b/test_runner/scenario_test_runner/scenario_test_runner/scenario.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2020 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. + +from ament_index_python.packages import get_package_share_directory +from pathlib import Path +from re import sub + + +def substitute_ros_package(pathname: Path): + def find_pkg_share(match): + return get_package_share_directory(match.group(1)) + + return Path( + sub("\\$\\(find-pkg-share\\s+([^\\)]+)\\)", find_pkg_share, str(pathname)) + ) + +class Scenario: + """ + Manages a scenario. + + Attributes + ---------- + path: Path + The path to a scenario. + + """ + + def __init__(self, path: Path, frame_rate: float): + + self.path = substitute_ros_package(path).resolve() + + self.frame_rate = frame_rate + +if __name__ == "__main__": + """Entrypoint.""" + pass diff --git a/test_runner/scenario_test_runner/scenario_test_runner/scenario_test_runner.py b/test_runner/scenario_test_runner/scenario_test_runner/scenario_test_runner.py index 3f076ea3387..bba9932cda1 100755 --- a/test_runner/scenario_test_runner/scenario_test_runner/scenario_test_runner.py +++ b/test_runner/scenario_test_runner/scenario_test_runner/scenario_test_runner.py @@ -32,9 +32,8 @@ from shutil import rmtree from sys import exit from typing import List -from workflow import Scenario -from workflow import Workflow -from workflow import substitute_ros_package +from scenario import Scenario +from scenario import substitute_ros_package def convert_scenarios_to_xosc(scenarios: List[Scenario], output_directory: Path): @@ -110,8 +109,6 @@ def __init__( os.remove(target) self.output_directory.mkdir(parents=True, exist_ok=True) - self.current_workflow = None - self.check_preprocessor_client = self.create_client(CheckDerivativeRemained, '/simulation/openscenario_preprocessor/check') while not self.check_preprocessor_client.wait_for_service(timeout_sec=1.0): @@ -129,32 +126,6 @@ def __init__( self.print_debug('connection established with preprocessor') - def run_workflow(self, path: Path): - """ - Run workflow. - - Arguments - --------- - path : Path - Path to the workflow file. - - Returns - ------- - None - - """ - self.current_workflow = Workflow( - path, - self.global_frame_rate, - # TODO self.global_real_time_factor, - ) - - converted_scenarios = convert_scenarios_to_xosc( - self.current_workflow.scenarios, self.output_directory - ) - - self.run_scenarios(converted_scenarios) - def spin(self): """Run scenario.""" time.sleep(self.SLEEP_RATE) @@ -313,13 +284,6 @@ def main(args=None): parser.add_argument("-s", "--scenario", default="/dev/null", type=Path) - parser.add_argument( - "-w", - "--workflow", - type=Path, - default="$(find-pkg-share scenario_test_runner)/config/workflow_example.yaml", - ) - parser.add_argument("--ros-args", nargs="*") # XXX DIRTY HACK parser.add_argument("-r", nargs="*") # XXX DIRTY HACK @@ -339,12 +303,8 @@ def main(args=None): args.global_frame_rate, )] ) - - elif args.workflow != Path("/dev/null"): - test_runner.run_workflow(substitute_ros_package(args.workflow).resolve()) - else: - print("Neither the scenario nor the workflow is specified. Specify either one.") + print("No scenario is specified. Specify one.") if __name__ == "__main__": diff --git a/test_runner/scenario_test_runner/scenario_test_runner/workflow.py b/test_runner/scenario_test_runner/scenario_test_runner/workflow.py deleted file mode 100755 index 57822ce437d..00000000000 --- a/test_runner/scenario_test_runner/scenario_test_runner/workflow.py +++ /dev/null @@ -1,140 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2020 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. - -import yamale - -from ament_index_python.packages import get_package_share_directory -from enum import IntEnum -from pathlib import Path -from re import sub -from sys import exit, stderr -from yaml import safe_load - - -def substitute_ros_package(pathname: Path): - def find_pkg_share(match): - return get_package_share_directory(match.group(1)) - - return Path( - sub("\\$\\(find-pkg-share\\s+([^\\)]+)\\)", find_pkg_share, str(pathname)) - ) - -class Scenario: - """ - Manages a scenario given as an element of workflow.yaml. - - Attributes - ---------- - path: Path - The path to a scenario. - - """ - - def __init__(self, path: Path, frame_rate: float): - - self.path = substitute_ros_package(path).resolve() - - self.frame_rate = frame_rate - - -class Workflow: - """ - Manages a set of scenario test items given as workflow.yaml. - - Attributes - ---------- - path: Path - The path to the given workflow file. - - scenarios : List[str] - - """ - - def __init__(self, path: Path, global_frame_rate: float): - - self.path = path - - self.schema = yamale.make_schema( - Path(__file__).parent / "resources/workflow_schema.yaml" - ) - - self.global_frame_rate = global_frame_rate - - self.scenarios = self.read(self.path) - - def validate(self, path: Path): - """ - Validate workflow file. - - Arguments - --------- - path : Path - Path to the workflow file. - - Returns - ------- - None - - """ - try: - yamale.validate(self.schema, yamale.make_data(path)) - - except yamale.yamale_error.YamaleError: - exit(1) - - def read(self, workflow_path: Path): - """ - Safely load workflow files. - - Arguments - --------- - workflow_path : Path - Path to the workflow file. - - Returns - ------- - scenarios : List[str] - - """ - self.validate(workflow_path) - - if workflow_path.exists(): - with workflow_path.open("r") as file: - - scenarios = [] - - for each in safe_load(file)["Scenario"]: - - scenarios.append( - Scenario( - each["path"], - each["frame-rate"] - if "frame-rate" in each - else self.global_frame_rate, - ) - ) - - return scenarios - - else: - print("No such file or directory: " + str(workflow_path), file=stderr) - exit(1) - - -if __name__ == "__main__": - """Entrypoint.""" - pass