diff --git a/.github/workflows/custom_spell.json b/.github/workflows/custom_spell.json index cecc85a3aab..b9638744476 100644 --- a/.github/workflows/custom_spell.json +++ b/.github/workflows/custom_spell.json @@ -41,6 +41,7 @@ "TESTRANDOMIZER", "travelling", "Tschirnhaus", + "walltime", "xerces", "xercesc", "yamacir-kit" diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 8d95f2507bf..7c95d4238bb 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,115 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 80822986ba2..225fcd9dfe5 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 3.5.5 + 4.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 8d6f6602bf9..ed87df592b4 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,170 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge pull request `#1341 `_ from tier4/RJD-1278/geometry-update + Rjd 1278/geometry update +* Merge branch 'master' into RJD-1278/geometry-update +* updates after merge +* update testcase +* remove empty line +* isIntersect2D_collinear +* refactor toPolygon2D tests +* add comments +* bounding_box clean up +* clean up vector3 +* rename tests in HermiteCurve +* rename tests in CatmullRomSpline +* quaternion operators +* tune down numbers +* sort tests, rm old duplicate +* getIntersection2D function +* getIntersection2DSValue and isIntersect2D functions +* getPose, getPoint refactor +* add a proper structure to the test files +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* update testcase +* remove empty line +* isIntersect2D_collinear +* Merge branch 'master' into RJD-1278/geometry-update +* refactor toPolygon2D tests +* add comments +* bounding_box clean up +* clean up vector3 +* rename tests in HermiteCurve +* rename tests in CatmullRomSpline +* quaternion operators +* tune down numbers +* sort tests, rm old duplicate +* getIntersection2D function +* getIntersection2DSValue and isIntersect2D functions +* getPose, getPoint refactor +* add a proper structure to the test files +* Contributors: Masaya Kataoka, Michał Ciasnocha, robomic + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge pull request `#1353 `_ from tier4/RJD-1278/fix-line-segment + Rjd 1278/fix line segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* make const members public +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* remove{} +* expand on the note, add else to if-stmts +* note +* add else to if statements +* rename getSlope, add consts +* remove unnecessary lambda +* simplify denormalize logic +* use has_value +* rename getIntersection2DSValue, minor logical fixes regarding 2D vs 3D +* LineSegment 2D vs 3D indistinction fixes +* return const& and remove implicit conversions +* vector fields for LineSegment class, cleanup +* use isInBounds function +* combine 2 PR, apply review requests +* Merge branch 'RJD-1278/fix-1344-getIntersection2DSValue' of github.com:tier4/scenario_simulator_v2 into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* isIntesect2D initial solution +* 1344 fix initial solution +* Contributors: Masaya Kataoka, Michał Ciasnocha, robomic + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/common/math/geometry/include/geometry/polygon/line_segment.hpp b/common/math/geometry/include/geometry/polygon/line_segment.hpp index ec17419f31d..8ba0f98bc0c 100644 --- a/common/math/geometry/include/geometry/polygon/line_segment.hpp +++ b/common/math/geometry/include/geometry/polygon/line_segment.hpp @@ -29,6 +29,13 @@ namespace geometry class LineSegment { public: + const geometry_msgs::msg::Point start_point; + const geometry_msgs::msg::Point end_point; + const geometry_msgs::msg::Vector3 vector; + const geometry_msgs::msg::Vector3 vector_2d; + const double length; + const double length_2d; + LineSegment( const geometry_msgs::msg::Point & start_point, const geometry_msgs::msg::Point & end_point); LineSegment( @@ -36,36 +43,34 @@ class LineSegment double length); ~LineSegment(); LineSegment & operator=(const LineSegment &); - const geometry_msgs::msg::Point start_point; - const geometry_msgs::msg::Point end_point; + + auto isIntersect2D(const geometry_msgs::msg::Point & point) const -> bool; + auto isIntersect2D(const LineSegment & line) const -> bool; + auto isInBounds2D(const geometry_msgs::msg::Point & point) const -> bool; + auto getPoint(const double s, const bool denormalize_s = false) const -> geometry_msgs::msg::Point; auto getPose(const double s, const bool denormalize_s = false, const bool fill_pitch = true) const -> geometry_msgs::msg::Pose; - auto isIntersect2D(const geometry_msgs::msg::Point & point) const -> bool; - auto isIntersect2D(const LineSegment & l0) const -> bool; - auto getIntersection2DSValue( + auto get2DIntersectionSValue( const geometry_msgs::msg::Point & point, const bool denormalize_s = false) const -> std::optional; - auto getIntersection2DSValue(const LineSegment & line, const bool denormalize_s = false) const + auto get2DIntersectionSValue(const LineSegment & line, const bool denormalize_s = false) const -> std::optional; auto getIntersection2D(const LineSegment & line) const -> std::optional; auto getSValue( const geometry_msgs::msg::Pose & pose, double threshold_distance, bool denormalize_s) const -> std::optional; - auto getVector() const -> geometry_msgs::msg::Vector3; auto getNormalVector() const -> geometry_msgs::msg::Vector3; - auto get2DVector() const -> geometry_msgs::msg::Vector3; - auto getLength() const -> double; - auto get2DLength() const -> double; - auto getSlope() const -> double; + auto get2DVectorSlope() const -> double; auto getSquaredDistanceIn2D( const geometry_msgs::msg::Point & point, const double s, const bool denormalize_s = false) const -> double; auto getSquaredDistanceVector( const geometry_msgs::msg::Point & point, const double s, const bool denormalize_s = false) const -> geometry_msgs::msg::Vector3; + auto relativePointPosition2D(const geometry_msgs::msg::Point & point) const -> int; private: auto denormalize(const std::optional & s, const bool throw_error_on_out_of_range = true) diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index ab400ec03a7..2d29d6f85f1 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 3.5.5 + 4.2.2 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/math/geometry/src/intersection/intersection.cpp b/common/math/geometry/src/intersection/intersection.cpp index 07a32df0b9a..85f0b691809 100644 --- a/common/math/geometry/src/intersection/intersection.cpp +++ b/common/math/geometry/src/intersection/intersection.cpp @@ -13,7 +13,9 @@ // limitations under the License. #include +#include #include +#include namespace math { @@ -21,22 +23,23 @@ namespace geometry { bool isIntersect2D(const LineSegment & line0, const LineSegment & line1) { - double s, t; - s = (line0.start_point.x - line0.end_point.x) * (line1.start_point.y - line0.start_point.y) - - (line0.start_point.y - line0.end_point.y) * (line1.start_point.x - line0.start_point.x); - t = (line0.start_point.x - line0.end_point.x) * (line1.end_point.y - line0.start_point.y) - - (line0.start_point.y - line0.end_point.y) * (line1.end_point.x - line0.start_point.x); - if (s * t > 0) { - return false; - } - s = (line1.start_point.x - line1.end_point.x) * (line0.start_point.y - line1.start_point.y) - - (line1.start_point.y - line1.end_point.y) * (line0.start_point.x - line1.start_point.x); - t = (line1.start_point.x - line1.end_point.x) * (line0.end_point.y - line1.start_point.y) - - (line1.start_point.y - line1.end_point.y) * (line0.end_point.x - line1.start_point.x); - if (s * t > 0) { - return false; + const auto &p0 = line0.start_point, &q0 = line0.end_point; + const auto &p1 = line1.start_point, &q1 = line1.end_point; + + const int relative_position_p0 = line1.relativePointPosition2D(p0); + const int relative_position_q0 = line1.relativePointPosition2D(q0); + const int relative_position_p1 = line0.relativePointPosition2D(p1); + const int relative_position_q1 = line0.relativePointPosition2D(q1); + + if ( + relative_position_p1 == 0 && relative_position_q1 == 0 && relative_position_p0 == 0 && + relative_position_q0 == 0) { + return line0.isInBounds2D(p1) || line0.isInBounds2D(q1) || line1.isInBounds2D(p0) || + line1.isInBounds2D(q0); + } else { + return relative_position_p1 != relative_position_q1 && + relative_position_p0 != relative_position_q0; } - return true; } bool isIntersect2D(const std::vector & lines) @@ -54,21 +57,35 @@ bool isIntersect2D(const std::vector & lines) std::optional getIntersection2D( const LineSegment & line0, const LineSegment & line1) { - if (!isIntersect2D(line0, line1)) { + if (not line0.isIntersect2D(line1)) { return std::nullopt; + } else { + // 'line0' represented as a0x + b0y = c0 + const double a0 = line0.vector_2d.y; + const double b0 = -line0.vector_2d.x; + const double c0 = a0 * line0.start_point.x + b0 * line0.start_point.y; + + // 'line1' represented as a1x + b1y = c1 + const double a1 = line1.vector_2d.y; + const double b1 = -line1.vector_2d.x; + const double c1 = a1 * line1.start_point.x + b1 * line1.start_point.y; + + const double determinant = a0 * b1 - a1 * b0; + + if (std::abs(determinant) <= std::numeric_limits::epsilon()) { + // The lines do intersect, but they are collinear and overlap. + THROW_SIMULATION_ERROR( + "Line segments are collinear. So determinant is zero.", + "If this message was displayed, something completely unexpected happens.", + "This message is not originally intended to be displayed, if you see it, please " + "contact the developer of traffic_simulator."); + } else { + return geometry_msgs::build() + .x((b1 * c0 - b0 * c1) / determinant) + .y((a0 * c1 - a1 * c0) / determinant) + .z(0.0); + } } - const auto det = - (line0.start_point.x - line0.end_point.x) * (line1.end_point.y - line1.start_point.y) - - (line1.end_point.x - line1.start_point.x) * (line0.start_point.y - line0.end_point.y); - const auto t = - ((line1.end_point.y - line1.start_point.y) * (line1.end_point.x - line0.end_point.x) + - (line1.start_point.x - line1.end_point.x) * (line1.end_point.y - line0.end_point.y)) / - det; - geometry_msgs::msg::Point point; - point.x = t * line0.start_point.x + (1.0 - t) * line0.end_point.x; - point.y = t * line0.start_point.y + (1.0 - t) * line0.end_point.y; - point.z = t * line0.start_point.z + (1.0 - t) * line0.end_point.z; - return point; } std::vector getIntersection2D(const std::vector & lines) diff --git a/common/math/geometry/src/polygon/line_segment.cpp b/common/math/geometry/src/polygon/line_segment.cpp index 5238a24b307..b3b71a13b13 100644 --- a/common/math/geometry/src/polygon/line_segment.cpp +++ b/common/math/geometry/src/polygon/line_segment.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -28,27 +29,37 @@ namespace geometry { LineSegment::LineSegment( const geometry_msgs::msg::Point & start_point, const geometry_msgs::msg::Point & end_point) -: start_point(start_point), end_point(end_point) +: start_point(start_point), + end_point(end_point), + vector(geometry_msgs::build() + .x(end_point.x - start_point.x) + .y(end_point.y - start_point.y) + .z(end_point.z - start_point.z)), + vector_2d(geometry_msgs::build() + .x(end_point.x - start_point.x) + .y(end_point.y - start_point.y) + .z(0.0)), + length(hypot(end_point, start_point)), + length_2d(std::hypot(end_point.x - start_point.x, end_point.y - start_point.y)) { } LineSegment::LineSegment( const geometry_msgs::msg::Point & start_point, const geometry_msgs::msg::Vector3 & vec, - double length) -: start_point(start_point), end_point([&]() -> geometry_msgs::msg::Point { - geometry_msgs::msg::Point ret; - double vec_size = std::hypot(vec.x, vec.y); - if (vec_size == 0.0) { + const double length) +: LineSegment::LineSegment(start_point, [&]() -> geometry_msgs::msg::Point { + if (const double vec_size = std::hypot(vec.x, vec.y); vec_size == 0.0) { THROW_SIMULATION_ERROR( "Invalid vector is specified, while constructing LineSegment. ", "The vector should have a non zero length to initialize the line segment correctly. ", "This message is not originally intended to be displayed, if you see it, please " "contact the developer of traffic_simulator."); + } else { + return geometry_msgs::build() + .x(start_point.x + vec.x / vec_size * length) + .y(start_point.y + vec.y / vec_size * length) + .z(start_point.z + vec.z / vec_size * length); } - ret.x = start_point.x + vec.x / vec_size * length; - ret.y = start_point.y + vec.y / vec_size * length; - ret.z = start_point.z + vec.z / vec_size * length; - return ret; }()) { } @@ -64,25 +75,24 @@ LineSegment::~LineSegment() {} auto LineSegment::getPoint(const double s, const bool denormalize_s) const -> geometry_msgs::msg::Point { - const double s_normalized = denormalize_s ? s / getLength() : s; - if (0 <= s_normalized && s_normalized <= 1) { + const double s_normalized = denormalize_s ? s / length : s; + if (0.0 <= s_normalized && s_normalized <= 1.0) { return geometry_msgs::build() - .x(start_point.x + (end_point.x - start_point.x) * s_normalized) - .y(start_point.y + (end_point.y - start_point.y) * s_normalized) - .z(start_point.z + (end_point.z - start_point.z) * s_normalized); - } - if (denormalize_s) { + .x(start_point.x + vector.x * s_normalized) + .y(start_point.y + vector.y * s_normalized) + .z(start_point.z + vector.z * s_normalized); + } else if (denormalize_s) { THROW_SIMULATION_ERROR( "Invalid S value is specified, while getting point on a line segment.", - "The range of s_normalized value should be in range [0,", getLength(), "].", - "But, your values are = ", s, " and length = ", getLength(), + "The range of s_normalized value should be in range [0,", length, "].", + "But, your values are = ", s, " and length = ", length, " This message is not originally intended to be displayed, if you see it, please " "contact the developer of traffic_simulator."); } else { THROW_SIMULATION_ERROR( "Invalid S value is specified, while getting point on a line segment.", "The range of s_normalized value should be in range [0,1].", "But, your values are = ", s, - " and length = ", getLength(), + " and length = ", length, " This message is not originally intended to be displayed, if you see it, please " "contact the developer of traffic_simulator."); } @@ -101,65 +111,55 @@ auto LineSegment::getPose(const double s, const bool denormalize_s, const bool f { return geometry_msgs::build() .position(getPoint(s, denormalize_s)) - .orientation([this, fill_pitch]() -> geometry_msgs::msg::Quaternion { - const auto tangent_vec = getVector(); - return math::geometry::convertEulerAngleToQuaternion( - geometry_msgs::build() - .x(0.0) - .y( - fill_pitch ? std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)) : 0.0) - .z(std::atan2(tangent_vec.y, tangent_vec.x))); - }()); + .orientation(math::geometry::convertEulerAngleToQuaternion( + geometry_msgs::build() + .x(0.0) + .y(fill_pitch ? std::atan2(-vector.z, std::hypot(vector.x, vector.y)) : 0.0) + .z(std::atan2(vector.y, vector.x)))); } /** * @brief Checking the intersection with 1 line segment and 1 point in 2D (x,y) coordinate. Ignore z axis. * @param point point you want to check intersection. * @return true Intersection detected. - * @return false Intersection does not detected. + * @return false Intersection not detected. */ auto LineSegment::isIntersect2D(const geometry_msgs::msg::Point & point) const -> bool { - return getIntersection2DSValue(point, true) ? true : false; + if (isInBounds2D(point)) { + const double cross_product = + (point.y - start_point.y) * vector_2d.x - (point.x - start_point.x) * vector_2d.y; + constexpr double tolerance = std::numeric_limits::epsilon(); + return std::abs(cross_product) <= length_2d * tolerance; + } else { + return false; + } } auto LineSegment::getSValue( const geometry_msgs::msg::Pose & pose, double threshold_distance, bool denormalize_s) const -> std::optional { - return getIntersection2DSValue( + return get2DIntersectionSValue( LineSegment( math::geometry::transformPoint( - pose, geometry_msgs::build().x(0).y(threshold_distance).z(0)), + pose, + geometry_msgs::build().x(0.0).y(threshold_distance).z(0.0)), math::geometry::transformPoint( - pose, geometry_msgs::build().x(0).y(-threshold_distance).z(0))), + pose, + geometry_msgs::build().x(0.0).y(-threshold_distance).z(0.0))), denormalize_s); } /** * @brief Checking the intersection with 2 line segments in 2D (x,y) coordinate. Ignore z axis. - * @param l0 line segments you want to check intersection. + * @param line line segments you want to check intersection. * @return true Intersection detected. - * @return false Intersection does not detected. + * @return false Intersection not detected. */ -auto LineSegment::isIntersect2D(const LineSegment & l0) const -> bool +auto LineSegment::isIntersect2D(const LineSegment & line) const -> bool { - double s, t; - s = (l0.start_point.x - l0.end_point.x) * (start_point.y - l0.start_point.y) - - (l0.start_point.y - l0.end_point.y) * (start_point.x - l0.start_point.x); - t = (l0.start_point.x - l0.end_point.x) * (end_point.y - l0.start_point.y) - - (l0.start_point.y - l0.end_point.y) * (end_point.x - l0.start_point.x); - if (s * t > 0) { - return false; - } - s = (start_point.x - end_point.x) * (l0.start_point.y - start_point.y) - - (start_point.y - end_point.y) * (l0.start_point.x - start_point.x); - t = (start_point.x - end_point.x) * (l0.end_point.y - start_point.y) - - (start_point.y - end_point.y) * (l0.end_point.x - start_point.x); - if (s * t > 0) { - return false; - } - return true; + return math::geometry::isIntersect2D(*this, line); } /** @@ -167,35 +167,22 @@ auto LineSegment::isIntersect2D(const LineSegment & l0) const -> bool * @param point point of you want to find intersection. * @return std::optional */ -auto LineSegment::getIntersection2DSValue( +auto LineSegment::get2DIntersectionSValue( const geometry_msgs::msg::Point & point, const bool denormalize_s) const -> std::optional { - const auto get_s_normalized = [this](const auto & point) -> std::optional { - const auto get_s_from_x = [this](const auto & point) { - const auto s = (point.x - start_point.x) / (end_point.x - start_point.x); - return 0 <= s && s <= 1 ? s : std::optional(); - }; - const auto get_s_from_y = [this](const auto & point) { - const auto s = (point.y - start_point.y) / (end_point.y - start_point.y); - return 0 <= s && s <= 1 ? s : std::optional(); - }; - constexpr double epsilon = std::numeric_limits::epsilon(); - if (std::abs(end_point.x - start_point.x) <= epsilon) { - if (std::abs(end_point.y - start_point.y) <= epsilon) { - /// @note If start_point and end_point is a same point, checking the point is same as end_point or not. - return (std::abs(end_point.x - point.x) <= epsilon && - std::abs(end_point.y - point.y) <= epsilon) - ? std::optional(0) - : std::optional(); - } - /// @note If the line segment is parallel to y axis, calculate s value from y axis value. - return std::abs(point.x - start_point.x) <= epsilon ? get_s_from_y(point) - : std::optional(); - } - /// @note If the line segment is not parallel to x and y axis, calculate s value from x axis value. - return get_s_from_x(point); - }; - return denormalize_s ? denormalize(get_s_normalized(point)) : get_s_normalized(point); + /// @note This function checks for an SValue along the line. + /// The term "2D" in the function name specifically refers to the intersection point, not the SValue. + /// Therefore, the intersection is determined by disregarding the z-coordinate, hence the term "2D." + /// After finding the intersection, we calculate its position using a proportion. + /// Finally, we multiply this proportion by the actual 3D length to obtain the total SValue. + if (isIntersect2D(point)) { + const double proportion_2d = + std::hypot(point.x - start_point.x, point.y - start_point.y) / length_2d; + return denormalize_s ? std::make_optional(proportion_2d * length) + : std::make_optional(proportion_2d); + } else { + return std::nullopt; + } } /** @@ -204,33 +191,14 @@ auto LineSegment::getIntersection2DSValue( * @param denormalize_s If true, s value should be normalized in range [0,1]. If false, s value is not normalized. * @return std::optional */ -auto LineSegment::getIntersection2DSValue(const LineSegment & line, const bool denormalize_s) const +auto LineSegment::get2DIntersectionSValue(const LineSegment & line, const bool denormalize_s) const -> std::optional { - /// @note Hard coded parameter, this parameter describes the tolerance of the range of s value (-s_tolerance ~ 1.0 + s_tolerance) - constexpr double s_tolerance = 1e-10; - const auto get_s_normalized = [this](const auto & line) -> std::optional { - if (!isIntersect2D(line)) { - return std::optional(); - } - const double det = (start_point.x - end_point.x) * (line.end_point.y - line.start_point.y) - - (line.end_point.x - line.start_point.x) * (start_point.y - end_point.y); - const double s = - 1 - ((line.end_point.y - line.start_point.y) * (line.end_point.x - end_point.x) + - (line.start_point.x - line.end_point.x) * (line.end_point.y - end_point.y)) / - det; - if (std::isnan(s)) { - THROW_SIMULATION_ERROR( - "One line segment is on top of the other. So determinant is zero.", - "If this message was displayed, something completely unexpected happens.", - "This message is not originally intended to be displayed, if you see it, please " - "contact the developer of traffic_simulator."); - } - return (-s_tolerance <= s && s <= 1 + s_tolerance) - ? std::optional(std::clamp(s, 0.0, 1.0)) - : std::optional(); - }; - return denormalize_s ? denormalize(get_s_normalized(line)) : get_s_normalized(line); + if (const auto point = getIntersection2D(line); point.has_value()) { + return get2DIntersectionSValue(point.value(), denormalize_s); + } else { + return std::nullopt; + } } /** @@ -241,23 +209,7 @@ auto LineSegment::getIntersection2DSValue(const LineSegment & line, const bool d auto LineSegment::getIntersection2D(const LineSegment & line) const -> std::optional { - const auto s = getIntersection2DSValue(line, false); - return s ? geometry_msgs::build() - .x(s.value() * start_point.x + (1.0 - s.value()) * end_point.x) - .y(s.value() * start_point.y + (1.0 - s.value()) * end_point.y) - .z(s.value() * start_point.z + (1.0 - s.value()) * end_point.z) - : std::optional(); -} - -auto LineSegment::getVector() const -> geometry_msgs::msg::Vector3 -{ - using math::geometry::operator-; - const auto result_pt = end_point - start_point; - auto result = geometry_msgs::msg::Vector3(); - result.x = result_pt.x; - result.y = result_pt.y; - result.z = result_pt.z; - return result; + return math::geometry::getIntersection2D(*this, line); } /** @@ -266,32 +218,20 @@ auto LineSegment::getVector() const -> geometry_msgs::msg::Vector3 */ auto LineSegment::getNormalVector() const -> geometry_msgs::msg::Vector3 { - geometry_msgs::msg::Vector3 tangent_vec = getVector(); - double theta = M_PI / 2.0; + const double theta = M_PI / 2.0; return geometry_msgs::build() - .x(tangent_vec.x * std::cos(theta) - tangent_vec.y * std::sin(theta)) - .y(tangent_vec.x * std::sin(theta) + tangent_vec.y * std::cos(theta)) - .z(0); + .x(vector.x * std::cos(theta) - vector.y * std::sin(theta)) + .y(vector.x * std::sin(theta) + vector.y * std::cos(theta)) + .z(0.0); } -auto LineSegment::get2DVector() const -> geometry_msgs::msg::Vector3 +auto LineSegment::get2DVectorSlope() const -> double { - return geometry_msgs::build() - .x(end_point.x - start_point.x) - .y(end_point.y - start_point.y) - .z(0); -} - -auto LineSegment::get2DLength() const -> double -{ - return std::hypot(end_point.x - start_point.x, end_point.y - start_point.y); -} - -auto LineSegment::getLength() const -> double { return hypot(end_point, start_point); } - -auto LineSegment::getSlope() const -> double -{ - return (end_point.y - start_point.y) / (end_point.x - start_point.x); + if (vector_2d.x <= std::numeric_limits::epsilon()) { + THROW_SIMULATION_ERROR("Slope of a vertical line is undefined"); + } else { + return vector_2d.y / vector_2d.x; + } } /** @@ -337,10 +277,11 @@ auto LineSegment::denormalize( const std::optional & s, const bool throw_error_on_out_of_range) const -> std::optional { - if (!throw_error_on_out_of_range && s && !(0 <= s.value() && s.value() <= 1)) { - return std::optional(); + if (!s.has_value() || (!throw_error_on_out_of_range && !(0.0 <= s.value() && s.value() <= 1.0))) { + return std::nullopt; + } else { + return std::make_optional(denormalize(s.value())); } - return s ? denormalize(s.value()) : std::optional(); } /** @@ -350,13 +291,14 @@ auto LineSegment::denormalize( */ auto LineSegment::denormalize(const double s) const -> double { - if (0 <= s && s <= 1) { - return s * getLength(); + if (0.0 <= s && s <= 1.0) { + return s * length; + } else { + THROW_SIMULATION_ERROR( + "Invalid normalized s value, s = ", s, ", S value should be in range [0,1].", + "This message is not originally intended to be displayed, if you see it, please " + "contact the developer of traffic_simulator."); } - THROW_SIMULATION_ERROR( - "Invalid normalized s value, s = ", s, ", S value should be in range [0,1].", - "This message is not originally intended to be displayed, if you see it, please " - "contact the developer of traffic_simulator."); } LineSegment & LineSegment::operator=(const LineSegment &) { return *this; } @@ -386,5 +328,43 @@ auto getLineSegments( } } +/** + * @brief Checks if the given point lies within the bounding box of the line segment. + * @param point Points you want to test. + * @return + * - `true` if the points is within the bounds. + * - `false` otherwise. + */ +auto LineSegment::isInBounds2D(const geometry_msgs::msg::Point & point) const -> bool +{ + return ( + point.x >= std::min(start_point.x, end_point.x) && + point.x <= std::max(start_point.x, end_point.x) && + point.y >= std::min(start_point.y, end_point.y) && + point.y <= std::max(start_point.y, end_point.y)); +} + +/** + * @brief Determines the relative position of a point with respect to the line segment. + * + * This method computes the relative position of a given point with respect to the line segment defined by + * `start_point` and `end_point` using a cross product. The result indicates on which side of the line + * segment the point lies. + * + * @param point The point to be evaluated. + * @return + * - `1` if the point is to the left of the line segment (when moving from `start_point` to `end_point`). + * - `-1` if the point is to the right of the line segment. + * - `0` if the point is collinear with the line segment (i.e., lies exactly on the line defined by + * `start_point` and `end_point`). + */ +auto LineSegment::relativePointPosition2D(const geometry_msgs::msg::Point & point) const -> int +{ + constexpr double tolerance = std::numeric_limits::epsilon(); + const double determinant = + vector_2d.y * (point.x - end_point.x) - vector_2d.x * (point.y - end_point.y); + return static_cast(determinant > tolerance) - static_cast(determinant < -tolerance); +} + } // namespace geometry } // namespace math diff --git a/common/math/geometry/src/spline/catmull_rom_spline.cpp b/common/math/geometry/src/spline/catmull_rom_spline.cpp index 523b6f28099..01dafbf7594 100644 --- a/common/math/geometry/src/spline/catmull_rom_spline.cpp +++ b/common/math/geometry/src/spline/catmull_rom_spline.cpp @@ -139,7 +139,7 @@ CatmullRomSpline::CatmullRomSpline(const std::vector break; /// @note In this case, spline is interpreted as line segment. case 2: - total_length_ = line_segments_[0].getLength(); + total_length_ = line_segments_[0].length; break; /// @note In this case, spline is interpreted as curve. default: @@ -316,7 +316,7 @@ auto CatmullRomSpline::getCollisionPointsIn2D( "This message is not originally intended to be displayed, if you see it, please " "contact the developer of traffic_simulator."); } - if (const auto s = line_segments_[0].getIntersection2DSValue(line, true)) { + if (const auto s = line_segments_[0].get2DIntersectionSValue(line, true)) { s_value_candidates.insert(s.value()); } } @@ -628,7 +628,7 @@ auto CatmullRomSpline::getTangentVector(const double s) const -> geometry_msgs:: "contact the developer of traffic_simulator."); } if (0 <= s && s <= getLength()) { - return line_segments_[0].getVector(); + return line_segments_[0].vector; } THROW_SIMULATION_ERROR( "Invalid S value is specified, while getting tangent vector.", diff --git a/common/math/geometry/test/CMakeLists.txt b/common/math/geometry/test/CMakeLists.txt index dfd0a09460b..eed4ab3431c 100644 --- a/common/math/geometry/test/CMakeLists.txt +++ b/common/math/geometry/test/CMakeLists.txt @@ -1,26 +1,15 @@ -add_subdirectory(vector3) +add_subdirectory(src/intersection) +add_subdirectory(src/polygon) +add_subdirectory(src/quaternion) +add_subdirectory(src/solver) +add_subdirectory(src/spline) +add_subdirectory(src/vector3) -ament_add_gtest(test_bounding_box test_bounding_box.cpp) -ament_add_gtest(test_catmull_rom_spline test_catmull_rom_spline.cpp) -ament_add_gtest(test_catmull_rom_subspline test_catmull_rom_subspline.cpp) -ament_add_gtest(test_collision test_collision.cpp) -ament_add_gtest(test_distance test_distance.cpp) -ament_add_gtest(test_hermite_curve test_hermite_curve.cpp) -ament_add_gtest(test_polygon test_polygon.cpp) -ament_add_gtest(test_polynomial_solver test_polynomial_solver.cpp) -ament_add_gtest(test_transform test_transform.cpp) -ament_add_gtest(test_intersection test_intersection.cpp) -ament_add_gtest(test_line_segment test_line_segment.cpp) -ament_add_gtest(test_quaternion test_quaternion.cpp) -target_link_libraries(test_bounding_box geometry) -target_link_libraries(test_catmull_rom_spline geometry) -target_link_libraries(test_catmull_rom_subspline geometry) -target_link_libraries(test_collision geometry) +ament_add_gtest(test_distance src/test_distance.cpp) target_link_libraries(test_distance geometry) -target_link_libraries(test_hermite_curve geometry) -target_link_libraries(test_polygon geometry) -target_link_libraries(test_polynomial_solver geometry) + +ament_add_gtest(test_bounding_box src/test_bounding_box.cpp) +target_link_libraries(test_bounding_box geometry) + +ament_add_gtest(test_transform src/test_transform.cpp) target_link_libraries(test_transform geometry) -target_link_libraries(test_intersection geometry) -target_link_libraries(test_line_segment geometry) -target_link_libraries(test_quaternion geometry) diff --git a/common/math/geometry/test/expect_eq_macros.hpp b/common/math/geometry/test/src/expect_eq_macros.hpp similarity index 100% rename from common/math/geometry/test/expect_eq_macros.hpp rename to common/math/geometry/test/src/expect_eq_macros.hpp diff --git a/common/math/geometry/test/src/intersection/CMakeLists.txt b/common/math/geometry/test/src/intersection/CMakeLists.txt new file mode 100644 index 00000000000..ea171973a2c --- /dev/null +++ b/common/math/geometry/test/src/intersection/CMakeLists.txt @@ -0,0 +1,5 @@ +ament_add_gtest(test_collision test_collision.cpp) +target_link_libraries(test_collision geometry) + +ament_add_gtest(test_intersection test_intersection.cpp) +target_link_libraries(test_intersection geometry) diff --git a/common/math/geometry/test/test_collision.cpp b/common/math/geometry/test/src/intersection/test_collision.cpp similarity index 99% rename from common/math/geometry/test/test_collision.cpp rename to common/math/geometry/test/src/intersection/test_collision.cpp index cf60a70eba2..a32622d4f0c 100644 --- a/common/math/geometry/test/test_collision.cpp +++ b/common/math/geometry/test/src/intersection/test_collision.cpp @@ -17,7 +17,7 @@ #include #include -#include "test_utils.hpp" +#include "../test_utils.hpp" TEST(Collision, DifferentHeight) { diff --git a/common/math/geometry/test/test_intersection.cpp b/common/math/geometry/test/src/intersection/test_intersection.cpp similarity index 91% rename from common/math/geometry/test/test_intersection.cpp rename to common/math/geometry/test/src/intersection/test_intersection.cpp index 4ee95a08cd9..f21879f6555 100644 --- a/common/math/geometry/test/test_intersection.cpp +++ b/common/math/geometry/test/src/intersection/test_intersection.cpp @@ -16,9 +16,10 @@ #include #include +#include -#include "expect_eq_macros.hpp" -#include "test_utils.hpp" +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" TEST(Intersection, isIntersect2DDisjoint) { @@ -108,9 +109,7 @@ TEST(Intersection, getIntersection2DIntersectVector) TEST(Intersection, getIntersection2DIdentical) { math::geometry::LineSegment line(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); - auto ans = math::geometry::getIntersection2D(line, line); - EXPECT_TRUE(ans); - EXPECT_POINT_NAN(ans.value()); + EXPECT_THROW(math::geometry::getIntersection2D(line, line), common::SimulationError); } TEST(Intersection, getIntersection2DIdenticalVector) @@ -121,14 +120,7 @@ TEST(Intersection, getIntersection2DIdenticalVector) lines.push_back(line); lines.push_back(line); - auto ans = math::geometry::getIntersection2D(lines); - EXPECT_EQ(ans.size(), size_t(6)); - EXPECT_POINT_NAN(ans[0]); - EXPECT_POINT_NAN(ans[1]); - EXPECT_POINT_NAN(ans[2]); - EXPECT_POINT_NAN(ans[3]); - EXPECT_POINT_NAN(ans[4]); - EXPECT_POINT_NAN(ans[5]); + EXPECT_THROW(math::geometry::getIntersection2D(lines), common::SimulationError); } TEST(Intersection, getIntersection2DEmptyVector) diff --git a/common/math/geometry/test/src/polygon/CMakeLists.txt b/common/math/geometry/test/src/polygon/CMakeLists.txt new file mode 100644 index 00000000000..8a2eae13418 --- /dev/null +++ b/common/math/geometry/test/src/polygon/CMakeLists.txt @@ -0,0 +1,5 @@ +ament_add_gtest(test_line_segment test_line_segment.cpp) +target_link_libraries(test_line_segment geometry) + +ament_add_gtest(test_polygon test_polygon.cpp) +target_link_libraries(test_polygon geometry) diff --git a/common/math/geometry/test/src/polygon/test_line_segment.cpp b/common/math/geometry/test/src/polygon/test_line_segment.cpp new file mode 100644 index 00000000000..354e9d71584 --- /dev/null +++ b/common/math/geometry/test/src/polygon/test_line_segment.cpp @@ -0,0 +1,785 @@ +// 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 "../expect_eq_macros.hpp" +#include "../test_utils.hpp" + +TEST(LineSegment, initializeDifferentPoints) +{ + geometry_msgs::msg::Point point0 = makePoint(0.0, 0.0); + geometry_msgs::msg::Point point1 = makePoint(1.0, 1.0); + EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point0, point1)); + const math::geometry::LineSegment line_segment(point0, point1); + EXPECT_POINT_EQ(line_segment.start_point, point0); + EXPECT_POINT_EQ(line_segment.end_point, point1); +} + +TEST(LineSegment, initializeIdenticalPoints) +{ + geometry_msgs::msg::Point point = makePoint(0.0, 0.0); + EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point, point)); + const math::geometry::LineSegment line_segment(point, point); + EXPECT_POINT_EQ(line_segment.start_point, point); + EXPECT_POINT_EQ(line_segment.end_point, point); +} + +TEST(LineSegment, initializeVector) +{ + geometry_msgs::msg::Point point = makePoint(0.0, 0.0); + geometry_msgs::msg::Vector3 vec = makeVector(1.0, 1.0); + EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point, vec, 1.0)); + const math::geometry::LineSegment line_segment(point, vec, 1.0); + EXPECT_POINT_EQ(line_segment.start_point, point); + EXPECT_POINT_EQ(line_segment.end_point, makePoint(std::sqrt(2.0) / 2.0, std::sqrt(2.0) / 2.0)); +} + +TEST(LineSegment, initializeVectorZero) +{ + geometry_msgs::msg::Point point = makePoint(0.0, 0.0); + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 0.0); + EXPECT_THROW( + const math::geometry::LineSegment line_segment(point, vec, 1.0), common::SimulationError); +} + +TEST(LineSegment, initializeVectorZeroLength) +{ + geometry_msgs::msg::Point point = makePoint(0.0, 0.0); + geometry_msgs::msg::Vector3 vec = makeVector(1.0, 1.0); + EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point, vec, 0.0)); + const math::geometry::LineSegment line_segment(point, vec, 0.0); + EXPECT_POINT_EQ(line_segment.start_point, point); + EXPECT_POINT_EQ(line_segment.end_point, point); +} + +/** + * @note Test error throwing when s is out of bounds. + */ +TEST(LineSegment, getPoint_outOfBounds_denormalized) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 1.0), makePoint(3.0, 2.0, -1.0)); + ASSERT_DOUBLE_EQ(line.length, 6.0); + + EXPECT_THROW(line.getPoint(7.0, true), common::SimulationError); + EXPECT_THROW(line.getPoint(-1.0, true), common::SimulationError); +} + +/** + * @note Test error throwing when normalized s is out of bounds. + */ +TEST(LineSegment, getPoint_outOfBounds_normalized) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 1.0), makePoint(3.0, 2.0, -1.0)); + ASSERT_DOUBLE_EQ(line.length, 6.0); + + EXPECT_THROW(line.getPoint(1.1, false), common::SimulationError); + EXPECT_THROW(line.getPoint(-0.1, false), common::SimulationError); +} + +/** + * @note Test calculation correctness when s is out of bounds. + */ +TEST(LineSegment, getPoint_inside_denormalized) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 1.0), makePoint(3.0, 2.0, -1.0)); + ASSERT_DOUBLE_EQ(line.length, 6.0); + + EXPECT_POINT_EQ(line.getPoint(0.0, true), makePoint(-1.0, -2.0, 1.0)); + EXPECT_POINT_EQ(line.getPoint(3.0, true), makePoint(1.0, 0.0, 0.0)); + EXPECT_POINT_EQ(line.getPoint(6.0, true), makePoint(3.0, 2.0, -1.0)); +} + +/** + * @note Test calculation correctness when normalized s is out of bounds. + */ +TEST(LineSegment, getPoint_inside_normalized) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 1.0), makePoint(3.0, 2.0, -1.0)); + ASSERT_DOUBLE_EQ(line.length, 6.0); + + EXPECT_POINT_EQ(line.getPoint(0.0, false), makePoint(-1.0, -2.0, 1.0)); + EXPECT_POINT_EQ(line.getPoint(0.5, false), makePoint(1.0, 0.0, 0.0)); + EXPECT_POINT_EQ(line.getPoint(1.0, false), makePoint(3.0, 2.0, -1.0)); +} + +/** + * @note Test calculation correctness with denormalized s. + */ +TEST(LineSegment, getPose_denormalized) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(3.0, 2.0, 4.0)); + const double length = 4.0 * std::sqrt(3.0); + ASSERT_DOUBLE_EQ(line.length, length); + + EXPECT_POSE_EQ( + line.getPose(0.0 * length, true, false), + makePose( + -1.0, -2.0, 0.0, + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4)))); + + EXPECT_POSE_EQ( + line.getPose(0.5 * length, true, false), + makePose( + 1.0, 0.0, 2.0, math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4)))); + + EXPECT_POSE_EQ( + line.getPose(1.0 * length, true, false), + makePose( + 3.0, 2.0, 4.0, math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4)))); +} + +/** + * @note Test calculation correctness with normalized s. + */ +TEST(LineSegment, getPose_normalized) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(3.0, 2.0, 4.0)); + const double length = 4.0 * std::sqrt(3.0); + ASSERT_DOUBLE_EQ(line.length, length); + + EXPECT_POSE_EQ( + line.getPose(0.0, false, false), + makePose( + -1.0, -2.0, 0.0, + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4)))); + + EXPECT_POSE_EQ( + line.getPose(0.5, false, false), + makePose( + 1.0, 0.0, 2.0, math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4)))); + + EXPECT_POSE_EQ( + line.getPose(1.0, false, false), + makePose( + 3.0, 2.0, 4.0, math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4)))); +} + +/** + * @note Test pitch calculation correctness with fill_pitch = true. + */ +TEST(LineSegment, getPose_pitch) +{ + const auto line = math::geometry::LineSegment( + makePoint(-1.0, -2.0, 0.0 * std::sqrt(2.0)), makePoint(3.0, 2.0, 4.0 * std::sqrt(2.0))); + const double length = 8.0; + ASSERT_DOUBLE_EQ(line.length, length); + + EXPECT_POSE_EQ( + line.getPose(0.0 * length, true, true), + makePose( + -1.0, -2.0, 0.0 * std::sqrt(2.0), + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, -M_PI_4, M_PI_4)))); + + EXPECT_POSE_EQ( + line.getPose(0.5 * length, true, true), + makePose( + 1.0, 0.0, 2.0 * std::sqrt(2.0), + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, -M_PI_4, M_PI_4)))); + + EXPECT_POSE_EQ( + line.getPose(1.0 * length, true, true), + makePose( + 3.0, 2.0, 4.0 * std::sqrt(2.0), + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, -M_PI_4, M_PI_4)))); +} + +TEST(LineSegment, isIntersect2DDisjoint) +{ + const math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + const math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); + EXPECT_FALSE(line0.isIntersect2D(line1)); +} + +TEST(LineSegment, isIntersect2DIntersect) +{ + const math::geometry::LineSegment line0(makePoint(1.0, 1.0), makePoint(2.0, 1.0)); + const math::geometry::LineSegment line1(makePoint(1.0, 0.0), makeVector(1.0, 1.0), 3.0); + EXPECT_TRUE(line0.isIntersect2D(line1)); +} + +TEST(LineSegment, isIntersect2DIdentical) +{ + const math::geometry::LineSegment line(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + EXPECT_TRUE(line.isIntersect2D(line)); +} + +/** + * @note Test function behavior with two disjoint, collinear lines. + */ +TEST(LineSegment, isIntersect2D_collinear) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, 1.0, 0.0), makePoint(1.0, 3.0, 0.0)); + + EXPECT_FALSE(line.isIntersect2D( + math::geometry::LineSegment(makePoint(3.0, 5.0, 0.0), makePoint(5.0, 7.0, 0.0)))); + EXPECT_FALSE(line.isIntersect2D( + math::geometry::LineSegment(makePoint(-3.0, -1.0, 0.0), makePoint(-2.0, 0.0, 0.0)))); +} + +/** + * @note Test function behavior with a point on the line. + */ +TEST(LineSegment, isIntersect2D_pointInside) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(3.0, 2.0, 4.0)); + + EXPECT_TRUE(line.isIntersect2D(makePoint(0.0, -1.0, 1.0))); + EXPECT_TRUE(line.isIntersect2D(makePoint(0.0, -1.0, 0.0))); +} + +/** + * @note Test function behavior with a point outside of the line. + */ +TEST(LineSegment, isIntersect2D_pointOutside) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(3.0, 2.0, 4.0)); + + EXPECT_FALSE(line.isIntersect2D(makePoint(-1.0, -1.0, 1.0))); + EXPECT_FALSE(line.isIntersect2D(makePoint(0.0, 89.0, 97.0))); + + EXPECT_FALSE(line.isIntersect2D(makePoint(-3.0, 0.0, 0.0))); + EXPECT_FALSE(line.isIntersect2D(makePoint(4.0, 0.0, 0.0))); +} + +/** + * @note Test function behavior with a point that is collinear and external to the line. + */ +TEST(LineSegment, isIntersect2D_pointCollinear) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(3.0, 2.0, 4.0)); + + EXPECT_FALSE(line.isIntersect2D(makePoint(-2.0, -3.0, -1.0))); + EXPECT_FALSE(line.isIntersect2D(makePoint(-2.0, -3.0, 0.0))); + + EXPECT_FALSE(line.isIntersect2D(makePoint(4.0, 3.0, 5.0))); + EXPECT_FALSE(line.isIntersect2D(makePoint(4.0, 3.0, 0.0))); +} + +/** + * @note Test function behavior with a point on an end of the line. + */ +TEST(LineSegment, isIntersect2D_pointOnEnd) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(3.0, 2.0, 4.0)); + + EXPECT_TRUE(line.isIntersect2D(makePoint(-1.0, -2.0, 0.0))); + EXPECT_TRUE(line.isIntersect2D(makePoint(-1.0, -2.0, 7.0))); + + EXPECT_TRUE(line.isIntersect2D(makePoint(3.0, 2.0, 4.0))); + EXPECT_TRUE(line.isIntersect2D(makePoint(3.0, 2.0, -5.0))); +} + +/** + * @note Test result correctness with a line intersecting with a vertical line. + */ +TEST(LineSegment, get2DIntersectionSValue_line_vertical) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(-1.0, 2.0, 0.0)); + { + const auto s_value = line.get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(-2.0, -2.0, 0.0), makePoint(1.0, 1.0, 0.0)), true); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 1.0); + } + { + const auto s_value = line.get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(-2.0, -2.0, 0.0), makePoint(1.0, 1.0, 0.0)), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 0.25); + } +} + +/** + * @note Test result correctness with a line intersecting with a horizontal line. + */ +TEST(LineSegment, get2DIntersectionSValue_line_horizontal) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, 2.0, 0.0), makePoint(1.0, 2.0, 0.0)); + { + const auto s_value = line.get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(-1.0, 0.0, 0.0), makePoint(1.0, 4.0, 0.0)), true); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 1.0); + } + { + const auto s_value = line.get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(-1.0, 0.0, 0.0), makePoint(1.0, 4.0, 0.0)), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 0.5); + } +} + +/** + * @note Test result correctness with lines intersecting at the start and end of a line. + */ +TEST(LineSegment, get2DIntersectionSValue_line_bounds) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, 1.0, 0.0), makePoint(1.0, 3.0, 0.0)); + { + const auto s_value = line.get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(-2.0, 2.0, 0.0), makePoint(0.0, 0.0, 0.0)), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 0.0); + } + { + const auto s_value = line.get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(1.0, 3.0, 0.0), makePoint(2.0, 2.0, 0.0)), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 1.0); + } +} + +/** + * @note Test result correctness with a line outside of the line. + */ +TEST(LineSegment, get2DIntersectionSValue_line_outside) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, 1.0, 0.0), makePoint(1.0, 3.0, 0.0)); + + EXPECT_FALSE( + line + .get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(-2.0, 1.0, 0.0), makePoint(0.0, 0.0, 0.0)), false) + .has_value()); + + EXPECT_FALSE( + line + .get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(1.0, 4.0, 0.0), makePoint(2.0, 2.0, 0.0)), false) + .has_value()); +} + +/** + * @note Test result correctness when lines are collinear. + */ +TEST(LineSegment, get2DIntersectionSValue_line_collinear) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, 1.0, 0.0), makePoint(1.0, 3.0, 0.0)); + + EXPECT_THROW(line.get2DIntersectionSValue(line, false), common::SimulationError); + + EXPECT_FALSE( + line + .get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(3.0, 5.0, 0.0), makePoint(5.0, 7.0, 0.0)), false) + .has_value()); +} + +/** + * @note Test result correctness with a point inside a vertical line. + */ +TEST(LineSegment, get2DIntersectionSValue_point_vertical) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(-1.0, 2.0, 0.0)); + + { + const auto s_value = line.get2DIntersectionSValue(makePoint(-1.0, 0.0, 0.0), true); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 2.0); + } + { + const auto s_value = line.get2DIntersectionSValue(makePoint(-1.0, 0.0, 0.0), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 0.5); + } + { + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-101.0, 0.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(103.0, 0.0, 0.0), false).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-1.0, -107.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-1.0, 109.0, 0.0), false).has_value()); + } +} + +/** + * @note Test result correctness with a point inside a horizontal line. + */ +TEST(LineSegment, get2DIntersectionSValue_point_horizontal) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, 2.0, 0.0), makePoint(1.0, 2.0, 0.0)); + + { + const auto s_value = line.get2DIntersectionSValue(makePoint(0.0, 2.0, 0.0), true); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 1.0); + } + { + const auto s_value = line.get2DIntersectionSValue(makePoint(0.0, 2.0, 0.0), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 0.5); + } + { + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-113.0, 2.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(127.0, 2.0, 0.0), false).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(0.0, -131.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(0.0, 137.0, 0.0), false).has_value()); + } +} + +/** + * @note Test result correctness with points at the start and end of a line. + */ +TEST(LineSegment, get2DIntersectionSValue_point_bounds) +{ + const auto line = + math::geometry::LineSegment(makePoint(-2.0, -2.0, 0.0), makePoint(1.0, 4.0, 0.0)); + + { + const auto s_value = line.get2DIntersectionSValue(makePoint(-2.0, -2.0, 0.0), true); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 0.0); + } + { + const auto s_value = line.get2DIntersectionSValue(makePoint(-2.0, -2.0, 0.0), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 0.0); + } + { + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-139.0, -2.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(149.0, -2.0, 0.0), false).has_value()); + } + + { + const auto s_value = line.get2DIntersectionSValue(makePoint(1.0, 4.0, 0.0), true); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 3.0 * std::sqrt(5)); + } + { + const auto s_value = line.get2DIntersectionSValue(makePoint(1.0, 4.0, 0.0), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 1.0); + } + { + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-151.0, 4.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(157.0, 4.0, 0.0), false).has_value()); + } +} + +/** + * @note Test result correctness with a point outside of the line. + */ +TEST(LineSegment, get2DIntersectionSValue_point_outside) +{ + const auto line = + math::geometry::LineSegment(makePoint(-2.0, -2.0, 0.0), makePoint(1.0, 4.0, 0.0)); + + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-3.0, 1.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-1.0, 1.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(0.0, 1.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(2.0, 1.0, 0.0), true).has_value()); + + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-0.5, -5.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-0.5, -1.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-0.5, 3.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-0.5, 7.0, 0.0), true).has_value()); + + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(7.0, 7.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(7.0, -7.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-7.0, 7.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-7.0, -7.0, 0.0), true).has_value()); +} + +TEST(LineSegment, getIntersection2DDisjoint) +{ + const math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + const math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); + EXPECT_FALSE(line0.getIntersection2D(line1)); +} + +/** + * @note Test function behavior with two intersecting lines. + */ +TEST(LineSegment, getIntersection2DIntersect) +{ + const auto line0 = math::geometry::LineSegment(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + const auto line1 = math::geometry::LineSegment(makePoint(1.0, 0.0), makePoint(0.0, 1.0)); + + const auto p0 = line0.getIntersection2D(line1); + const auto p1 = line1.getIntersection2D(line0); + + ASSERT_TRUE(p0.has_value()); + ASSERT_TRUE(p1.has_value()); + EXPECT_POINT_EQ(p0.value(), makePoint(0.5, 0.5)); + EXPECT_POINT_EQ(p1.value(), makePoint(0.5, 0.5)); +} + +/** + * @note Test function behavior with two identical lines. + */ +TEST(LineSegment, getIntersection2DIdentical) +{ + const auto line = math::geometry::LineSegment(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + + EXPECT_THROW(line.getIntersection2D(line), common::SimulationError); +} + +TEST(LineSegment, getSValue) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); + const auto s = line.getSValue(makePose(2.0, 2.0, 2.0), 1.0, false); + EXPECT_TRUE(s); + if (s) { + EXPECT_DOUBLE_EQ(s.value(), 2.0 / 3.0); + } +} + +TEST(LineSegment, getSValue_denormalize) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); + const auto s = line.getSValue(makePose(2.0, 2.0, 2.0), 1.0, true); + EXPECT_TRUE(s); + if (s) { + EXPECT_DOUBLE_EQ(s.value(), std::hypot(2.0, 2.0, 2.0)); + } +} + +TEST(LineSegment, getSValue_outOfRange) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); + const auto s = line.getSValue(makePose(4.0, 4.0, 4.0), 1.0, false); + EXPECT_FALSE(s); +} + +TEST(LineSegment, getSValue_outOfRangeDenormalize) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); + const auto s = line.getSValue(makePose(4.0, 4.0, 4.0), 1.0, true); + EXPECT_FALSE(s); +} + +TEST(LineSegment, getSValue_parallel) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 0.0)); + const auto s = line.getSValue( + makePose( + 1.0, 0.0, 0.0, + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4 * 3.0))), + 1000.0, false); + EXPECT_FALSE(s); +} + +TEST(LineSegment, getSValue_parallelDenormalize) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 0.0)); + const auto s = line.getSValue( + makePose( + 1.0, 0.0, 0.0, + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4 * 3.0))), + 1000.0, true); + EXPECT_FALSE(s); +} + +TEST(LineSegment, getVector) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); + EXPECT_VECTOR3_EQ(line.vector, makeVector(1.0, 1.0, 1.0)); +} + +TEST(LineSegment, getVectorZeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_VECTOR3_EQ(line.vector, makeVector(0.0, 0.0, 0.0)); +} + +TEST(LineSegment, getNormalVector) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); + EXPECT_VECTOR3_EQ(line.getNormalVector(), makeVector(-1.0, 1.0, 0.0)); +} + +TEST(LineSegment, getNormalVector_zeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_VECTOR3_EQ(line.getNormalVector(), makeVector(0.0, 0.0, 0.0)); +} + +TEST(LineSegment, get2DVector) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); + EXPECT_VECTOR3_EQ(line.vector_2d, makeVector(1.0, 1.0, 0.0)); +} + +TEST(LineSegment, get2DVectorZeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_VECTOR3_EQ(line.vector_2d, makeVector(0.0, 0.0, 0.0)); +} + +TEST(LineSegment, getLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); + EXPECT_DOUBLE_EQ(line.length, std::sqrt(3.0)); +} + +TEST(LineSegment, getLengthZeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_DOUBLE_EQ(line.length, 0.0); +} + +TEST(LineSegment, get2DLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); + EXPECT_DOUBLE_EQ(line.length_2d, std::sqrt(2.0)); +} + +TEST(LineSegment, get2DLengthZeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_DOUBLE_EQ(line.length_2d, 0.0); +} + +TEST(LineSegment, get2DVectorSlope) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 3.0, 4.0)); + EXPECT_DOUBLE_EQ(line.get2DVectorSlope(), 0.5); +} + +TEST(LineSegment, get2DVectorSlopeZeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_THROW(line.get2DVectorSlope(), common::SimulationError); +} + +TEST(LineSegment, getSquaredDistanceIn2D) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); + EXPECT_DOUBLE_EQ(line.getSquaredDistanceIn2D(makePoint(0.0, 1.0, 2.0), 0.5, false), 8.0); +} + +TEST(LineSegment, getSquaredDistanceIn2D_denormalize) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); + EXPECT_DOUBLE_EQ( + line.getSquaredDistanceIn2D(makePoint(0.0, 1.0, 2.0), std::sqrt(3.0), true), 8.0); +} + +TEST(LineSegment, getSquaredDistanceVector) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); + EXPECT_VECTOR3_EQ( + line.getSquaredDistanceVector(makePoint(0.0, 1.0, 2.0), 0.5, false), + makeVector(-2.0, -2.0, -2.0)); +} + +TEST(LineSegment, getSquaredDistanceVector_denormalize) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); + EXPECT_VECTOR3_EQ( + line.getSquaredDistanceVector(makePoint(0.0, 1.0, 2.0), std::sqrt(3.0), true), + makeVector(-2.0, -2.0, -2.0)); +} + +TEST(LineSegment, getLineSegments) +{ + const std::vector points{ + makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0), makePoint(3.0, 4.0, 5.0), + makePoint(4.0, 5.0, 6.0)}; + const std::vector lines = + math::geometry::getLineSegments(points, false); + EXPECT_EQ(lines.size(), size_t(3)); + EXPECT_POINT_EQ(lines[0].start_point, points[0]); + EXPECT_POINT_EQ(lines[0].end_point, points[1]); + EXPECT_POINT_EQ(lines[1].start_point, points[1]); + EXPECT_POINT_EQ(lines[1].end_point, points[2]); + EXPECT_POINT_EQ(lines[2].start_point, points[2]); + EXPECT_POINT_EQ(lines[2].end_point, points[3]); +} + +TEST(LineSegment, getLineSegments_closeStartEnd) +{ + const std::vector points{ + makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0), makePoint(3.0, 4.0, 5.0), + makePoint(4.0, 5.0, 6.0)}; + const std::vector lines = + math::geometry::getLineSegments(points, true); + EXPECT_EQ(lines.size(), size_t(4)); + EXPECT_POINT_EQ(lines[0].start_point, points[0]); + EXPECT_POINT_EQ(lines[0].end_point, points[1]); + EXPECT_POINT_EQ(lines[1].start_point, points[1]); + EXPECT_POINT_EQ(lines[1].end_point, points[2]); + EXPECT_POINT_EQ(lines[2].start_point, points[2]); + EXPECT_POINT_EQ(lines[2].end_point, points[3]); + EXPECT_POINT_EQ(lines[3].start_point, points[3]); + EXPECT_POINT_EQ(lines[3].end_point, points[0]); +} + +TEST(LineSegment, getLineSegmentsVectorEmpty) +{ + const std::vector points; + const std::vector lines = math::geometry::getLineSegments(points); + EXPECT_EQ(lines.size(), size_t(0)); +} + +TEST(LineSegment, getLineSegmentsVectorIdentical) +{ + geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); + const std::vector points{point, point, point, point}; + const std::vector lines = + math::geometry::getLineSegments(points, false); + EXPECT_EQ(lines.size(), size_t(3)); + EXPECT_POINT_EQ(lines[0].start_point, point); + EXPECT_POINT_EQ(lines[0].end_point, point); + EXPECT_POINT_EQ(lines[1].start_point, point); + EXPECT_POINT_EQ(lines[1].end_point, point); + EXPECT_POINT_EQ(lines[2].start_point, point); + EXPECT_POINT_EQ(lines[2].end_point, point); +} + +TEST(LineSegment, getLineSegmentsVectorIdentical_closeStartEnd) +{ + geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); + const std::vector points{point, point, point, point}; + const std::vector lines = + math::geometry::getLineSegments(points, true); + EXPECT_EQ(lines.size(), size_t(4)); + EXPECT_POINT_EQ(lines[0].start_point, point); + EXPECT_POINT_EQ(lines[0].end_point, point); + EXPECT_POINT_EQ(lines[1].start_point, point); + EXPECT_POINT_EQ(lines[1].end_point, point); + EXPECT_POINT_EQ(lines[2].start_point, point); + EXPECT_POINT_EQ(lines[2].end_point, point); + EXPECT_POINT_EQ(lines[3].start_point, point); + EXPECT_POINT_EQ(lines[3].end_point, point); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/math/geometry/test/test_polygon.cpp b/common/math/geometry/test/src/polygon/test_polygon.cpp similarity index 98% rename from common/math/geometry/test/test_polygon.cpp rename to common/math/geometry/test/src/polygon/test_polygon.cpp index 8ec0ea97ad9..e7278fa6566 100644 --- a/common/math/geometry/test/test_polygon.cpp +++ b/common/math/geometry/test/src/polygon/test_polygon.cpp @@ -17,8 +17,8 @@ #include #include -#include "expect_eq_macros.hpp" -#include "test_utils.hpp" +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" TEST(Polygon, filterByAxis) { diff --git a/common/math/geometry/test/src/quaternion/CMakeLists.txt b/common/math/geometry/test/src/quaternion/CMakeLists.txt new file mode 100644 index 00000000000..2ea2aaf3d91 --- /dev/null +++ b/common/math/geometry/test/src/quaternion/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_quaternion test_quaternion.cpp) +target_link_libraries(test_quaternion geometry) diff --git a/common/math/geometry/test/test_quaternion.cpp b/common/math/geometry/test/src/quaternion/test_quaternion.cpp similarity index 59% rename from common/math/geometry/test/test_quaternion.cpp rename to common/math/geometry/test/src/quaternion/test_quaternion.cpp index 7dfd1285664..0b31df60805 100644 --- a/common/math/geometry/test/test_quaternion.cpp +++ b/common/math/geometry/test/src/quaternion/test_quaternion.cpp @@ -18,47 +18,59 @@ #include #include -#include "expect_eq_macros.hpp" -#include "test_utils.hpp" +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" constexpr double EPS = 1e-6; -TEST(Quaternion, testCase1) +/** + * @note Test result correctness with a basic example. + */ +TEST(Quaternion, operator_addition) { using math::geometry::operator+; - auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1); - auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); - auto ans = q1 + q2; + const auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1); + const auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); + const auto ans = q1 + q2; EXPECT_QUATERNION_EQ(ans, math::geometry::makeQuaternion(0, 2, 0, 2)) } -TEST(Quaternion, testCase2) +/** + * @note Test result correctness with a basic example. + */ +TEST(Quaternion, operator_subtraction) { using math::geometry::operator-; - auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1); - auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); - auto ans = q1 - q2; + const auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1); + const auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); + const auto ans = q1 - q2; EXPECT_QUATERNION_EQ(ans, math::geometry::makeQuaternion(0, 0, 0, 0)) } -TEST(Quaternion, testCase3) +/** + * @note Test result correctness with a basic example. + */ +TEST(Quaternion, operator_multiplication) { using math::geometry::operator*; - auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1); - auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); - auto ans = q1 * q2; + const auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1); + const auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); + const auto ans = q1 * q2; EXPECT_QUATERNION_EQ(ans, math::geometry::makeQuaternion(0, 2, 0, 0)) } -TEST(Quaternion, testCase4) +/** + * @note Test result correctness with a basic example. + */ +TEST(Quaternion, operator_additionAssignment) { using math::geometry::operator+=; auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1); - auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); + const auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); q1 += q2; EXPECT_QUATERNION_EQ(q1, math::geometry::makeQuaternion(0, 2, 0, 2)) } diff --git a/common/math/geometry/test/src/solver/CMakeLists.txt b/common/math/geometry/test/src/solver/CMakeLists.txt new file mode 100644 index 00000000000..0716910f184 --- /dev/null +++ b/common/math/geometry/test/src/solver/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_polynomial_solver test_polynomial_solver.cpp) +target_link_libraries(test_polynomial_solver geometry) diff --git a/common/math/geometry/test/test_polynomial_solver.cpp b/common/math/geometry/test/src/solver/test_polynomial_solver.cpp similarity index 100% rename from common/math/geometry/test/test_polynomial_solver.cpp rename to common/math/geometry/test/src/solver/test_polynomial_solver.cpp diff --git a/common/math/geometry/test/src/spline/CMakeLists.txt b/common/math/geometry/test/src/spline/CMakeLists.txt new file mode 100644 index 00000000000..d6a2793f80d --- /dev/null +++ b/common/math/geometry/test/src/spline/CMakeLists.txt @@ -0,0 +1,8 @@ +ament_add_gtest(test_catmull_rom_spline test_catmull_rom_spline.cpp) +target_link_libraries(test_catmull_rom_spline geometry) + +ament_add_gtest(test_catmull_rom_subspline test_catmull_rom_subspline.cpp) +target_link_libraries(test_catmull_rom_subspline geometry) + +ament_add_gtest(test_hermite_curve test_hermite_curve.cpp) +target_link_libraries(test_hermite_curve geometry) diff --git a/common/math/geometry/test/test_catmull_rom_spline.cpp b/common/math/geometry/test/src/spline/test_catmull_rom_spline.cpp similarity index 99% rename from common/math/geometry/test/test_catmull_rom_spline.cpp rename to common/math/geometry/test/src/spline/test_catmull_rom_spline.cpp index b29aba00130..68ad3855eca 100644 --- a/common/math/geometry/test/test_catmull_rom_spline.cpp +++ b/common/math/geometry/test/src/spline/test_catmull_rom_spline.cpp @@ -18,8 +18,8 @@ #include #include -#include "expect_eq_macros.hpp" -#include "test_utils.hpp" +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" constexpr double EPS = 1e-6; @@ -70,7 +70,7 @@ void addOffset(geometry_msgs::msg::Point & point, const double offset, const dou /// @brief Testing the `CatmullRomSpline::getCollisionPointIn2D` function works valid. /// In this test case, number of the control points of the catmull-rom spline (variable name `spline`) is 2, so the shape of the value `spline` is line segment. -TEST(CatmullRomSpline, GetCollisionWith2ControlPoints) +TEST(CatmullRomSpline, getCollisionPointIn2D_2ControlPoints) { /// @note The `spline` has control points p0 and p1. Control point p0 is point (x,y,z) = (0,0,0) and control point p1 is point (x,y,z) = (1,0,0) in the cartesian coordinate system. // [Snippet_construct_spline] @@ -149,7 +149,7 @@ TEST(CatmullRomSpline, GetCollisionWith2ControlPoints) /// @brief Testing the `CatmullRomSpline::getCollisionPointIn2D` function works valid /// In this test case, number of the control points of the catmull-rom spline (variable name `spline`) is 1, so the shape of the value `spline` is single point. -TEST(CatmullRomSpline, GetCollisionWith1ControlPoint) +TEST(CatmullRomSpline, getCollisionPointIn2D_1ControlPoint) { /// @note The variable `spline` has control point with point (x,y,z) = (0,1,0) in the cartesian coordinate system. So, `spline` is same as point (x,y,z) = (0,1,0). auto spline = math::geometry::CatmullRomSpline( diff --git a/common/math/geometry/test/test_catmull_rom_subspline.cpp b/common/math/geometry/test/src/spline/test_catmull_rom_subspline.cpp similarity index 98% rename from common/math/geometry/test/test_catmull_rom_subspline.cpp rename to common/math/geometry/test/src/spline/test_catmull_rom_subspline.cpp index 567d1bf089e..10b2b56124c 100644 --- a/common/math/geometry/test/test_catmull_rom_subspline.cpp +++ b/common/math/geometry/test/src/spline/test_catmull_rom_subspline.cpp @@ -17,8 +17,8 @@ #include #include -#include "expect_eq_macros.hpp" -#include "test_utils.hpp" +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" constexpr double EPS = 1e-6; diff --git a/common/math/geometry/test/test_hermite_curve.cpp b/common/math/geometry/test/src/spline/test_hermite_curve.cpp similarity index 94% rename from common/math/geometry/test/test_hermite_curve.cpp rename to common/math/geometry/test/src/spline/test_hermite_curve.cpp index 9efbccef633..96eb39fd2ec 100644 --- a/common/math/geometry/test/test_hermite_curve.cpp +++ b/common/math/geometry/test/src/spline/test_hermite_curve.cpp @@ -16,8 +16,8 @@ #include -#include "expect_eq_macros.hpp" -#include "test_utils.hpp" +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" constexpr double EPS = 1e-3; @@ -320,7 +320,10 @@ TEST(HermiteCurveTest, getSValue) EXPECT_NEAR(s2.value(), 1.0, EPS); } -TEST(HermiteCurveTest, getSValueAutoscale) +/** + * @note Test function correctness with parameter denormalize_s = true. + */ +TEST(HermiteCurveTest, getSValueDenormalized) { const auto curve = makeLine2(); @@ -503,7 +506,10 @@ TEST(HermiteCurveTest, getTangentVector4) EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, 1.0 / std::sqrt(2.0)); } -TEST(HermiteCurveTest, getTangentVectorAutoscale1) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getTangentVectorDenormalized1) { //p(0,0) v(1,0)-> p(1,1) v(0,1) const auto curve = makeCurve1(); constexpr double eps = 0.1; @@ -513,7 +519,10 @@ TEST(HermiteCurveTest, getTangentVectorAutoscale1) EXPECT_NEAR(curve.getTangentVector(0.75, true).y / norm, 1.0 / std::sqrt(2.0), eps); } -TEST(HermiteCurveTest, getTangentVectorAutoscale2) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getTangentVectorDenormalized2) { //p(0,0) v(1,0)-> p(1,-1) v(0,-1) const auto curve = makeCurve2(); constexpr double eps = 0.1; @@ -523,7 +532,10 @@ TEST(HermiteCurveTest, getTangentVectorAutoscale2) EXPECT_NEAR(curve.getTangentVector(0.75, true).y / norm, -1.0 / std::sqrt(2.0), eps); } -TEST(HermiteCurveTest, getTangentVectorAutoscale3) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getTangentVectorDenormalized3) { //p(1,1) v(0,-1)-> p(0,0) v(-1,0) const auto curve = makeCurve3(); constexpr double eps = 0.1; @@ -533,7 +545,10 @@ TEST(HermiteCurveTest, getTangentVectorAutoscale3) EXPECT_NEAR(curve.getTangentVector(0.75, true).y / norm, -1.0 / std::sqrt(2.0), eps); } -TEST(HermiteCurveTest, getTangentVectorAutoscale4) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getTangentVectorDenormalized4) { //p(1,-1) v(0,1)-> p(0,0) v(-1,0) const auto curve = makeCurve4(); constexpr double eps = 0.1; @@ -579,7 +594,10 @@ TEST(HermiteCurveTest, getNormalVector4) EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, -1.0 / std::sqrt(2.0)); } -TEST(HermiteCurveTest, getNormalVectorAutoscale1) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getNormalVectorDenormalized1) { //p(0,0) v(1,0)-> p(1,1) v(0,1) const auto curve = makeCurve1(); constexpr double eps = 0.1; @@ -589,7 +607,10 @@ TEST(HermiteCurveTest, getNormalVectorAutoscale1) EXPECT_NEAR(curve.getNormalVector(0.75, true).y / norm, 1.0 / std::sqrt(2.0), eps); } -TEST(HermiteCurveTest, getNormalVectorAutoscale2) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getNormalVectorDenormalized2) { //p(0,0) v(1,0)-> p(1,-1) v(0,-1) const auto curve = makeCurve2(); constexpr double eps = 0.1; @@ -599,7 +620,10 @@ TEST(HermiteCurveTest, getNormalVectorAutoscale2) EXPECT_NEAR(curve.getNormalVector(0.75, true).y / norm, 1.0 / std::sqrt(2.0), eps); } -TEST(HermiteCurveTest, getNormalVectorAutoscale3) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getNormalVectorDenormalized3) { //p(1,1) v(0,-1)-> p(0,0) v(-1,0) const auto curve = makeCurve3(); constexpr double eps = 0.1; @@ -609,7 +633,10 @@ TEST(HermiteCurveTest, getNormalVectorAutoscale3) EXPECT_NEAR(curve.getNormalVector(0.75, true).y / norm, -1.0 / std::sqrt(2.0), eps); } -TEST(HermiteCurveTest, getNormalVectorAutoscale4) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getNormalVectorDenormalized4) { //p(1,-1) v(0,1)-> p(0,0) v(-1,0) const auto curve = makeCurve4(); constexpr double eps = 0.1; diff --git a/common/math/geometry/test/test_bounding_box.cpp b/common/math/geometry/test/src/test_bounding_box.cpp similarity index 55% rename from common/math/geometry/test/test_bounding_box.cpp rename to common/math/geometry/test/src/test_bounding_box.cpp index c6df82911d0..b3449f994e4 100644 --- a/common/math/geometry/test/test_bounding_box.cpp +++ b/common/math/geometry/test/src/test_bounding_box.cpp @@ -46,53 +46,64 @@ TEST(BoundingBox, getPointsFromBboxCustom) EXPECT_POINT_EQ(points[3], makePoint(6.5, -2.0, 1.0)); } -TEST(BoundingBox, get2DPolygonZeroPose) +/** + * @note Test obtaining polygon from bounding box with no transformation applied. + */ +TEST(BoundingBox, toPolygon2D_zeroPose) { - geometry_msgs::msg::Pose pose; - traffic_simulator_msgs::msg::BoundingBox bounding_box = makeBbox(2.0, 2.0, 2.0); - boost::geometry::model::polygon> poly = - math::geometry::toPolygon2D(pose, bounding_box); - EXPECT_EQ(poly.inners().size(), size_t(0)); - EXPECT_EQ(poly.outer().size(), size_t(5)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[0], makePoint(1.0, 1.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[1], makePoint(-1.0, 1.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[2], makePoint(-1.0, -1.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[3], makePoint(1.0, -1.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[4], makePoint(1.0, 1.0)); + const auto pose = geometry_msgs::msg::Pose(); + const auto bounding_box = makeBbox(2.0, 2.0, 2.0); + const auto polygon = math::geometry::toPolygon2D(pose, bounding_box); + + ASSERT_EQ(polygon.inners().size(), static_cast(0)); + ASSERT_EQ(polygon.outer().size(), static_cast(5)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[0], makePoint(1.0, 1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[1], makePoint(-1.0, 1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[2], makePoint(-1.0, -1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[3], makePoint(1.0, -1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[4], makePoint(1.0, 1.0)); } -TEST(BoundingBox, get2DPolygonOnlyTranslation) +/** + * @note Test obtaining polygon from bounding box with only translation applied. + */ +TEST(BoundingBox, toPolygon2D_onlyTranslation) { - geometry_msgs::msg::Pose pose = makePose(1.0, 2.0); - traffic_simulator_msgs::msg::BoundingBox bounding_box = makeBbox(2.0, 2.0, 2.0); - boost::geometry::model::polygon> poly = - math::geometry::toPolygon2D(pose, bounding_box); - EXPECT_EQ(poly.inners().size(), size_t(0)); - EXPECT_EQ(poly.outer().size(), size_t(5)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[0], makePoint(2.0, 3.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[1], makePoint(0.0, 3.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[2], makePoint(0.0, 1.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[3], makePoint(2.0, 1.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[4], makePoint(2.0, 3.0)); + const auto pose = makePose(1.0, 2.0); + const auto bounding_box = makeBbox(2.0, 2.0, 2.0); + const auto polygon = math::geometry::toPolygon2D(pose, bounding_box); + + ASSERT_EQ(polygon.inners().size(), static_cast(0)); + ASSERT_EQ(polygon.outer().size(), static_cast(5)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[0], makePoint(2.0, 3.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[1], makePoint(0.0, 3.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[2], makePoint(0.0, 1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[3], makePoint(2.0, 1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[4], makePoint(2.0, 3.0)); } -TEST(BoundingBox, get2DPolygonFullPose) +/** + * @note Test obtaining polygon from bounding box with full transformation applied (translation + rotation). + */ +TEST(BoundingBox, toPolygon2D_fullPose) { - geometry_msgs::msg::Pose pose = makePose(1.0, 2.0); - 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 = - math::geometry::toPolygon2D(pose, bounding_box); - EXPECT_EQ(poly.inners().size(), size_t(0)); - EXPECT_EQ(poly.outer().size(), size_t(5)); + const auto pose = makePose( + 1.0, 2.0, 0.0, + math::geometry::convertEulerAngleToQuaternion( + geometry_msgs::build().x(0.0).y(0.0).z(30.0 * M_PI / 180.0))); + + const auto bounding_box = makeBbox(2.0, 2.0, 2.0); + const auto polygon = math::geometry::toPolygon2D(pose, bounding_box); + ASSERT_EQ(polygon.inners().size(), static_cast(0)); + ASSERT_EQ(polygon.outer().size(), static_cast(5)); + const double x = std::sqrt(2.0) * std::cos((30.0 + 45.0) * M_PI / 180.0); const double y = std::sqrt(2.0) * std::sin((30.0 + 45.0) * M_PI / 180.0); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[0], makePoint(x + 1.0, y + 2.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[1], makePoint(-y + 1.0, x + 2.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[2], makePoint(-x + 1.0, -y + 2.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[3], makePoint(y + 1.0, -x + 2.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[4], makePoint(x + 1.0, y + 2.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[0], makePoint(x + 1.0, y + 2.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[1], makePoint(-y + 1.0, x + 2.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[2], makePoint(-x + 1.0, -y + 2.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[3], makePoint(y + 1.0, -x + 2.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[4], makePoint(x + 1.0, y + 2.0)); } TEST(BoundingBox, getPolygonDistanceWithCollision) diff --git a/common/math/geometry/test/test_distance.cpp b/common/math/geometry/test/src/test_distance.cpp similarity index 100% rename from common/math/geometry/test/test_distance.cpp rename to common/math/geometry/test/src/test_distance.cpp diff --git a/common/math/geometry/test/test_transform.cpp b/common/math/geometry/test/src/test_transform.cpp similarity index 100% rename from common/math/geometry/test/test_transform.cpp rename to common/math/geometry/test/src/test_transform.cpp diff --git a/common/math/geometry/test/test_utils.hpp b/common/math/geometry/test/src/test_utils.hpp similarity index 100% rename from common/math/geometry/test/test_utils.hpp rename to common/math/geometry/test/src/test_utils.hpp diff --git a/common/math/geometry/test/vector3/CMakeLists.txt b/common/math/geometry/test/src/vector3/CMakeLists.txt similarity index 100% rename from common/math/geometry/test/vector3/CMakeLists.txt rename to common/math/geometry/test/src/vector3/CMakeLists.txt diff --git a/common/math/geometry/test/vector3/test_truncate_custom.cpp b/common/math/geometry/test/src/vector3/test_truncate_custom.cpp similarity index 100% rename from common/math/geometry/test/vector3/test_truncate_custom.cpp rename to common/math/geometry/test/src/vector3/test_truncate_custom.cpp diff --git a/common/math/geometry/test/vector3/test_truncate_msg.cpp b/common/math/geometry/test/src/vector3/test_truncate_msg.cpp similarity index 100% rename from common/math/geometry/test/vector3/test_truncate_msg.cpp rename to common/math/geometry/test/src/vector3/test_truncate_msg.cpp diff --git a/common/math/geometry/test/vector3/test_vector3.cpp b/common/math/geometry/test/src/vector3/test_vector3.cpp similarity index 93% rename from common/math/geometry/test/vector3/test_vector3.cpp rename to common/math/geometry/test/src/vector3/test_vector3.cpp index b07cfede363..c75f7177d3e 100644 --- a/common/math/geometry/test/vector3/test_vector3.cpp +++ b/common/math/geometry/test/src/vector3/test_vector3.cpp @@ -54,6 +54,9 @@ TEST(Vector3, hypot_customVector) EXPECT_DOUBLE_EQ(math::geometry::hypot(vec0, vec1), 6.0); } +/** + * @note Test function correctness with parameter that is ros message vector. + */ TEST(Vector3, norm_msgVector) { geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); @@ -75,6 +78,12 @@ TEST(Vector3, normalize_msgVector) const double norm = std::sqrt(14.0); EXPECT_VECTOR3_EQ( math::geometry::normalize(vec0), makeVector(1.0 / norm, 2.0 / norm, 3.0 / norm)); + + geometry_msgs::msg::Vector3 vec1; + EXPECT_DOUBLE_EQ(math::geometry::norm(vec1), 0.0); + + geometry_msgs::msg::Vector3 vec2 = makeVector(1.0, 0.0, 3.0); + EXPECT_DOUBLE_EQ(math::geometry::norm(vec2), std::sqrt(10.0)); } TEST(Vector3, normalize_customVector) @@ -184,25 +193,16 @@ TEST(Vector3, additionAssignment_customVector) EXPECT_VECTOR3_EQ(vec0, makeVector(0.0, 0.0, 2.0)); } -TEST(Vector3, vector3_getSizeZero) -{ - geometry_msgs::msg::Vector3 vec; - EXPECT_DOUBLE_EQ(math::geometry::norm(vec), 0.0); -} - -TEST(Vector3, vector3_getSize) -{ - geometry_msgs::msg::Vector3 vec = makeVector(1.0, 0.0, 3.0); - EXPECT_DOUBLE_EQ(math::geometry::norm(vec), std::sqrt(10.0)); -} - -TEST(Vector3, vector3_normalizeZero) +/** + * @note Test function correctness when one of the vectors has length of 0. + */ +TEST(Vector3, normalize_zeroLength) { geometry_msgs::msg::Vector3 vec; EXPECT_THROW(math::geometry::normalize(vec), common::SimulationError); } -TEST(Vector3, vector3_normalize) +TEST(Vector3, normalize) { geometry_msgs::msg::Vector3 vec = makeVector(1.0, 0.0, 3.0); vec = math::geometry::normalize(vec); diff --git a/common/math/geometry/test/test_line_segment.cpp b/common/math/geometry/test/test_line_segment.cpp deleted file mode 100644 index 4c21cc35626..00000000000 --- a/common/math/geometry/test/test_line_segment.cpp +++ /dev/null @@ -1,642 +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 "expect_eq_macros.hpp" -#include "test_utils.hpp" - -TEST(LineSegment, initializeDifferentPoints) -{ - geometry_msgs::msg::Point point0 = makePoint(0.0, 0.0); - geometry_msgs::msg::Point point1 = makePoint(1.0, 1.0); - EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point0, point1)); - const math::geometry::LineSegment line_segment(point0, point1); - EXPECT_POINT_EQ(line_segment.start_point, point0); - EXPECT_POINT_EQ(line_segment.end_point, point1); -} - -TEST(LineSegment, initializeIdenticalPoints) -{ - geometry_msgs::msg::Point point = makePoint(0.0, 0.0); - EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point, point)); - const math::geometry::LineSegment line_segment(point, point); - EXPECT_POINT_EQ(line_segment.start_point, point); - EXPECT_POINT_EQ(line_segment.end_point, point); -} - -TEST(LineSegment, initializeVector) -{ - geometry_msgs::msg::Point point = makePoint(0.0, 0.0); - geometry_msgs::msg::Vector3 vec = makeVector(1.0, 1.0); - EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point, vec, 1.0)); - const math::geometry::LineSegment line_segment(point, vec, 1.0); - EXPECT_POINT_EQ(line_segment.start_point, point); - EXPECT_POINT_EQ(line_segment.end_point, makePoint(std::sqrt(2.0) / 2.0, std::sqrt(2.0) / 2.0)); -} - -TEST(LineSegment, initializeVectorZero) -{ - geometry_msgs::msg::Point point = makePoint(0.0, 0.0); - geometry_msgs::msg::Vector3 vec = makeVector(0.0, 0.0); - EXPECT_THROW( - const math::geometry::LineSegment line_segment(point, vec, 1.0), common::SimulationError); -} - -TEST(LineSegment, initializeVectorZeroLength) -{ - geometry_msgs::msg::Point point = makePoint(0.0, 0.0); - geometry_msgs::msg::Vector3 vec = makeVector(1.0, 1.0); - EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point, vec, 0.0)); - const math::geometry::LineSegment line_segment(point, vec, 0.0); - EXPECT_POINT_EQ(line_segment.start_point, point); - EXPECT_POINT_EQ(line_segment.end_point, point); -} - -TEST(LineSegment, isIntersect2DDisjoint) -{ - const math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); - const math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); - EXPECT_FALSE(line0.isIntersect2D(line1)); -} - -TEST(LineSegment, isIntersect2DIntersect) -{ - const math::geometry::LineSegment line0(makePoint(1.0, 1.0), makePoint(2.0, 1.0)); - const math::geometry::LineSegment line1(makePoint(1.0, 0.0), makeVector(1.0, 1.0), 3.0); - EXPECT_TRUE(line0.isIntersect2D(line1)); -} - -TEST(LineSegment, isIntersect2DIdentical) -{ - const math::geometry::LineSegment line(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); - EXPECT_TRUE(line.isIntersect2D(line)); -} - -TEST(LineSegment, getIntersection2DDisjoint) -{ - const math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); - const math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); - EXPECT_FALSE(line0.getIntersection2D(line1)); -} - -TEST(LineSegment, getVector) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); - EXPECT_VECTOR3_EQ(line.getVector(), makeVector(1.0, 1.0, 1.0)); -} - -TEST(LineSegment, getVectorZeroLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); - EXPECT_VECTOR3_EQ(line.getVector(), makeVector(0.0, 0.0, 0.0)); -} - -TEST(LineSegment, getNormalVector) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); - EXPECT_VECTOR3_EQ(line.getNormalVector(), makeVector(-1.0, 1.0, 0.0)); -} - -TEST(LineSegment, getNormalVector_zeroLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); - EXPECT_VECTOR3_EQ(line.getNormalVector(), makeVector(0.0, 0.0, 0.0)); -} - -TEST(LineSegment, get2DVector) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); - EXPECT_VECTOR3_EQ(line.get2DVector(), makeVector(1.0, 1.0, 0.0)); -} - -TEST(LineSegment, get2DVectorZeroLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); - EXPECT_VECTOR3_EQ(line.get2DVector(), makeVector(0.0, 0.0, 0.0)); -} - -TEST(LineSegment, getLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); - EXPECT_DOUBLE_EQ(line.getLength(), std::sqrt(3.0)); -} - -TEST(LineSegment, getLengthZeroLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); - EXPECT_DOUBLE_EQ(line.getLength(), 0.0); -} - -TEST(LineSegment, get2DLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); - EXPECT_DOUBLE_EQ(line.get2DLength(), std::sqrt(2.0)); -} - -TEST(LineSegment, get2DLengthZeroLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); - EXPECT_DOUBLE_EQ(line.get2DLength(), 0.0); -} - -TEST(LineSegment, getSlope) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 3.0, 4.0)); - EXPECT_DOUBLE_EQ(line.getSlope(), 0.5); -} - -TEST(LineSegment, getSlopeZeroLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); - EXPECT_TRUE(std::isnan(line.getSlope())); -} - -TEST(LineSegment, getSquaredDistanceIn2D) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); - EXPECT_DOUBLE_EQ(line.getSquaredDistanceIn2D(makePoint(0.0, 1.0, 2.0), 0.5, false), 8.0); -} - -TEST(LineSegment, getSquaredDistanceIn2D_denormalize) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); - EXPECT_DOUBLE_EQ( - line.getSquaredDistanceIn2D(makePoint(0.0, 1.0, 2.0), std::sqrt(3.0), true), 8.0); -} - -TEST(LineSegment, getSquaredDistanceVector) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); - EXPECT_VECTOR3_EQ( - line.getSquaredDistanceVector(makePoint(0.0, 1.0, 2.0), 0.5, false), - makeVector(-2.0, -2.0, -2.0)); -} - -TEST(LineSegment, getSquaredDistanceVector_denormalize) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); - EXPECT_VECTOR3_EQ( - line.getSquaredDistanceVector(makePoint(0.0, 1.0, 2.0), std::sqrt(3.0), true), - makeVector(-2.0, -2.0, -2.0)); -} - -TEST(LineSegment, getLineSegments) -{ - const std::vector points{ - makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0), makePoint(3.0, 4.0, 5.0), - makePoint(4.0, 5.0, 6.0)}; - const std::vector lines = - math::geometry::getLineSegments(points, false); - EXPECT_EQ(lines.size(), size_t(3)); - EXPECT_POINT_EQ(lines[0].start_point, points[0]); - EXPECT_POINT_EQ(lines[0].end_point, points[1]); - EXPECT_POINT_EQ(lines[1].start_point, points[1]); - EXPECT_POINT_EQ(lines[1].end_point, points[2]); - EXPECT_POINT_EQ(lines[2].start_point, points[2]); - EXPECT_POINT_EQ(lines[2].end_point, points[3]); -} - -TEST(LineSegment, getLineSegments_closeStartEnd) -{ - const std::vector points{ - makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0), makePoint(3.0, 4.0, 5.0), - makePoint(4.0, 5.0, 6.0)}; - const std::vector lines = - math::geometry::getLineSegments(points, true); - EXPECT_EQ(lines.size(), size_t(4)); - EXPECT_POINT_EQ(lines[0].start_point, points[0]); - EXPECT_POINT_EQ(lines[0].end_point, points[1]); - EXPECT_POINT_EQ(lines[1].start_point, points[1]); - EXPECT_POINT_EQ(lines[1].end_point, points[2]); - EXPECT_POINT_EQ(lines[2].start_point, points[2]); - EXPECT_POINT_EQ(lines[2].end_point, points[3]); - EXPECT_POINT_EQ(lines[3].start_point, points[3]); - EXPECT_POINT_EQ(lines[3].end_point, points[0]); -} - -TEST(LineSegment, getLineSegmentsVectorEmpty) -{ - const std::vector points; - const std::vector lines = math::geometry::getLineSegments(points); - EXPECT_EQ(lines.size(), size_t(0)); -} - -TEST(LineSegment, getLineSegmentsVectorIdentical) -{ - geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); - const std::vector points{point, point, point, point}; - const std::vector lines = - math::geometry::getLineSegments(points, false); - EXPECT_EQ(lines.size(), size_t(3)); - EXPECT_POINT_EQ(lines[0].start_point, point); - EXPECT_POINT_EQ(lines[0].end_point, point); - EXPECT_POINT_EQ(lines[1].start_point, point); - EXPECT_POINT_EQ(lines[1].end_point, point); - EXPECT_POINT_EQ(lines[2].start_point, point); - EXPECT_POINT_EQ(lines[2].end_point, point); -} - -TEST(LineSegment, getLineSegmentsVectorIdentical_closeStartEnd) -{ - geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); - const std::vector points{point, point, point, point}; - const std::vector lines = - math::geometry::getLineSegments(points, true); - EXPECT_EQ(lines.size(), size_t(4)); - EXPECT_POINT_EQ(lines[0].start_point, point); - EXPECT_POINT_EQ(lines[0].end_point, point); - EXPECT_POINT_EQ(lines[1].start_point, point); - EXPECT_POINT_EQ(lines[1].end_point, point); - EXPECT_POINT_EQ(lines[2].start_point, point); - EXPECT_POINT_EQ(lines[2].end_point, point); - EXPECT_POINT_EQ(lines[3].start_point, point); - EXPECT_POINT_EQ(lines[3].end_point, point); -} - -TEST(LineSegment, getSValue) -{ - math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); - const auto s = line.getSValue(makePose(2.0, 2.0, 2.0), 1.0, false); - EXPECT_TRUE(s); - if (s) { - EXPECT_DOUBLE_EQ(s.value(), 2.0 / 3.0); - } -} - -TEST(LineSegment, getSValue_denormalize) -{ - math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); - const auto s = line.getSValue(makePose(2.0, 2.0, 2.0), 1.0, true); - EXPECT_TRUE(s); - if (s) { - EXPECT_DOUBLE_EQ(s.value(), std::hypot(2.0, 2.0, 2.0)); - } -} - -TEST(LineSegment, getSValue_outOfRange) -{ - math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); - const auto s = line.getSValue(makePose(4.0, 4.0, 4.0), 1.0, false); - EXPECT_FALSE(s); -} - -TEST(LineSegment, getSValue_outOfRangeDenormalize) -{ - math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); - const auto s = line.getSValue(makePose(4.0, 4.0, 4.0), 1.0, true); - EXPECT_FALSE(s); -} - -TEST(LineSegment, getSValue_parallel) -{ - math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 0.0)); - const auto s = line.getSValue( - makePose( - 1.0, 0.0, 0.0, - math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4 * 3.0))), - 1000.0, false); - EXPECT_FALSE(s); -} - -TEST(LineSegment, getSValue_parallelDenormalize) -{ - math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 0.0)); - const auto s = line.getSValue( - makePose( - 1.0, 0.0, 0.0, - math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4 * 3.0))), - 1000.0, true); - EXPECT_FALSE(s); -} - -/// @brief In this test case, testing the `LineSegment::getPoint` function can find the point on the line segment with start point (x,y,z) = (0,0,0) and end point (x,y,z) = (1,1,1) in the cartesian coordinate system. (variable name `line`). -TEST(LineSegment, GetPoint) -{ - { - math::geometry::LineSegment line( - geometry_msgs::build().x(0).y(0).z(0), - geometry_msgs::build().x(1).y(1).z(1)); - /// @note If s = 0 and denormalize_s = false, the return value of the `getPoint` function should be a start point of the `line`. - /// If the `denormalize_s = false`, the return value `s` of the function is normalized and in range `s = [0,1]`. - // [Snippet_getPoint_with_s_0] - EXPECT_POINT_EQ( - line.getPoint(0, false), geometry_msgs::build().x(0).y(0).z(0)); - // [Snippet_getPoint_with_s_0] - /// @snippet test/test_line_segment.cpp Snippet_getPoint_with_s_0 - - /// @note If s = 0 and denormalize_s = false, the return value of the `LineSegment::getPoint` function should be a end point of the `line`. - /// If the `denormalize_s = false`, the return value `s` of the function is normalized and in range `s = [0,1]`. - // [Snippet_getPoint_with_s_1] - EXPECT_POINT_EQ( - line.getPoint(1, false), geometry_msgs::build().x(1).y(1).z(1)); - // [Snippet_getPoint_with_s_1] - /// @snippet test/test_line_segment.cpp Snippet_getPoint_with_s_1 - - /// @note If s = sqrt(3) and denormalize_s = true, the return value of the `LineSegment::getPoint` function should be a end point of the `line`. - /// If the `denormalize_s = true`, the return value `s` is denormalized and it returns the value `s` times the length of the curve. - // [Snippet_getPoint_with_sqrt_3_denormalized] - EXPECT_POINT_EQ( - line.getPoint(std::sqrt(3), true), - geometry_msgs::build().x(1).y(1).z(1)); - // [Snippet_getPoint_with_sqrt_3_denormalized] - /// @snippet test/test_line_segment.cpp Snippet_getPoint_with_sqrt_3_denormalized - - /// @note If s is not in range s = [0,1], testing the `LineSegment::getPoint` function can throw error. If s is not in range s = [0,1], it means the point is not on the line. - // [Snippet_getPoint_out_of_range] - EXPECT_THROW(line.getPoint(2, false), common::SimulationError); - EXPECT_THROW(line.getPoint(-1, false), common::SimulationError); - // [Snippet_getPoint_out_of_range] - /// @snippet test/test_line_segment.cpp Snippet_getPoint_out_of_range - - /// @note If s is not in range s = [0,sqrt(3)] and denormalize, testing the `LineSegment::getPoint` function can throw error. If s is not in range s = [0,sqrt(3)], it means the point is not on the line. - // [Snippet_getPoint_out_of_range_with_denormalize] - EXPECT_THROW(line.getPoint(2, true), common::SimulationError); - EXPECT_THROW(line.getPoint(-1, true), common::SimulationError); - EXPECT_NO_THROW(line.getPoint(0, true)); - EXPECT_NO_THROW(line.getPoint(std::sqrt(3.0), true)); - // [Snippet_getPoint_out_of_range_with_denormalize] - /// @snippet test/test_line_segment.cpp Snippet_getPoint_out_of_range_with_denormalize - } -} - -/// @brief In this test case, testing the `LineSegment::getPose` function can find the pose on the line segment with start point (x,y,z) = (0,0,0) and end point (x,y,z) = (0,0,1) in the cartesian coordinate system. (variable name `line`). -TEST(LineSegment, GetPose) -{ - { - math::geometry::LineSegment line( - geometry_msgs::build().x(0).y(0).z(0), - geometry_msgs::build().x(0).y(0).z(1)); - /// @note When the `s = 0`(1st argument of the `LineSegment::getPose` function), the return value of the `LineSegment::getPose` function should be a start point of the `line`. - /// Orientation should be defined by the direction of the `line`, so the orientation should be parallel to the z axis. - // [Snippet_getPose_with_s_0] - EXPECT_POSE_EQ( - line.getPose(0, false, false), - 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))); - EXPECT_POSE_EQ( - line.getPose(0, false, true), - geometry_msgs::build() - .position(geometry_msgs::build().x(0).y(0).z(0)) - .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 - - /// @note When the `s = 1`(1st argument of the `LineSegment::getPose` function), the return value of the `LineSegment::getPose` function should be an end point of the `line`. - /// Orientation should be defined by the direction of the `line`, so the orientation should be parallel to the z axis. - // [Snippet_getPose_with_s_1] - EXPECT_POSE_EQ( - line.getPose(1, false, false), - geometry_msgs::build() - .position(geometry_msgs::build().x(0).y(0).z(1)) - .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1))); - EXPECT_POSE_EQ( - line.getPose(1, false, true), - geometry_msgs::build() - .position(geometry_msgs::build().x(0).y(0).z(1)) - .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 - } -} - -/// @brief In this test case, we check collision with the point and the line segment with start point (x,y,z) = (0,-1,0) and end point (x,y,z) = (0,1,0) in the cartesian coordinate system. (variable name `line`). -TEST(LineSegment, isIntersect2D) -{ - { - math::geometry::LineSegment line( - geometry_msgs::build().x(0).y(-1).z(0), - geometry_msgs::build().x(0).y(1).z(0)); - /// @note Testing the `LineSegment::isIntersect2D` function can find intersection with point (0,0,0) and `line`. - // [Snippet_isIntersect2D_with_point_0_0_0] - EXPECT_TRUE( - line.isIntersect2D(geometry_msgs::build().x(0).y(0).z(0))); - // [Snippet_isIntersect2D_with_point_0_0_0] - /// @snippet test/test_line_segment.cpp Snippet_isIntersect2D_with_point_0_0_0 - - /// @note Testing the `LineSegment::isIntersect2D` function can find intersection with point (0,0,-1) - // [Snippet_isIntersect2D_with_point_0_0_-1] - EXPECT_TRUE( - line.isIntersect2D(geometry_msgs::build().x(0).y(0).z(-1))); - // [Snippet_isIntersect2D_with_point_0_0_-1] - /// @snippet test/test_line_segment.cpp Snippet_isIntersect2D_with_point_0_0_-1 - - /// @note Testing the `LineSegment::isIntersect2D` function can find intersection with point (0,1,0) and `line`. - // [Snippet_isIntersect2D_with_point_0_1_0] - EXPECT_TRUE( - line.isIntersect2D(geometry_msgs::build().x(0).y(1).z(0))); - // [Snippet_isIntersect2D_with_point_0_1_0] - /// @snippet test/test_line_segment.cpp Snippet_isIntersect2D_with_point_0_1_0 - - /// @note Testing the `LineSegment::isIntersect2D` function can find that there is no intersection with point (0,2,0) and `line`. - // [Snippet_isIntersect2D_with_point_0_2_0] - EXPECT_FALSE( - line.isIntersect2D(geometry_msgs::build().x(0).y(2).z(0))); - // [Snippet_isIntersect2D_with_point_0_2_0] - /// @snippet test/test_line_segment.cpp Snippet_isIntersect2D_with_point_0_2_0 - - /// @note Testing the `LineSegment::isIntersect2D` function can find that there is no intersection with point (0,-2,0) and `line`. - // [Snippet_isIntersect2D_with_point_0_-2_0] - EXPECT_FALSE( - line.isIntersect2D(geometry_msgs::build().x(0).y(-2).z(0))); - // [Snippet_isIntersect2D_with_point_0_-2_0] - /// @snippet test/test_line_segment.cpp Snippet_isIntersect2D_with_point_0_-2_0 - } -} - -/// @brief In this test case, we check collision with the line segment with start point (x,y,z) = (0,-1,0) and end point (x,y,z) = (0,1,0) in the cartesian coordinate system. (variable name `line`). -TEST(LineSegment, getIntersection2DSValue) -{ - { - math::geometry::LineSegment line( - geometry_msgs::build().x(0).y(-1).z(0), - geometry_msgs::build().x(0).y(1).z(0)); - /** - * @note Testing the `LineSegment::getIntersection2DSValue` function can find a collision with the point with (x,y,z) = (0,0,0) in the cartesian coordinate system. - * In the frenet coordinate system along the `line`, the s value should be 0.5. - * If so, the variable `collision_s` should be `std::optional(0.5)`. - */ - // [Snippet_getIntersection2DSValue_with_point_0_0_0] - { - const auto collision_s = line.getIntersection2DSValue( - geometry_msgs::build().x(0).y(0).z(0), false); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.5); - } - } - // [Snippet_getIntersection2DSValue_with_point_0_0_0] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2DSValue_with_point_0_0_0 - - /** - * @note Testing the `LineSegment::getIntersection2DSValue` function can find a collision with the point with (x,y,z) = (0,1,0) in the cartesian coordinate system. - * In the frenet coordinate system along the `line`, the s value should be 1.0. - * If so, the variable `collision_s` should be `std::optional(1.0)`. - */ - // [Snippet_getIntersection2DSValue_with_point_0_1_0] - { - const auto collision_s = line.getIntersection2DSValue( - geometry_msgs::build().x(0).y(1).z(0), false); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 1.0); - } - } - // [Snippet_getIntersection2DSValue_with_point_0_1_0] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2DSValue_with_point_0_1_0 - - /** - * @note Testing the `LineSegment::getIntersection2DSValue` function can find a collision with the point with (x,y,z) = (0,1,0) in the cartesian coordinate system. - * In the frenet coordinate system along the `line`, the s value should be 1.0. - * And, the 2nd argument of the `LineSegment::getIntersection2DSValue` (denormalized_s) is true, so the return value should be 1.0 (normalized s value.) * 2.0 (length of the `line`) = 2.0. - * If so, the variable `collision_s` should be `std::optional(2.0)`. - */ - // [Snippet_getIntersection2DSValue_with_point_0_1_0_denormalized] - { - const auto collision_s = line.getIntersection2DSValue( - geometry_msgs::build().x(0).y(1).z(0), true); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 2.0); - } - } - // [Snippet_getIntersection2DSValue_with_point_0_1_0_denormalized] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2DSValue_with_point_0_1_0_denormalized - - /** - * @note Testing the `LineSegment::getIntersection2DSValue` function can find a collision with the point with (x,y,z) = (0,0,0) in the cartesian coordinate system. - * In the frenet coordinate system along the `line`, the s value should be 0.5. - * And, the 2nd argument of the `LineSegment::getIntersection2DSValue` (denormalized_s) is true, so the return value should be 0.5 (normalized s value.) * 2.0 (length of the `line`) = 1.0. - * If so, the variable `collision_s` should be `std::optional(1.0)`. - */ - // [Snippet_getIntersection2DSValue_with_point_0_0_0_denormalized] - { - const auto collision_s = line.getIntersection2DSValue( - geometry_msgs::build().x(0).y(0).z(0), true); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 1.0); - } - } - // [Snippet_getIntersection2DSValue_with_point_0_0_0_denormalized] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2DSValue_with_point_0_0_0_denormalized - - /** - * @note Testing the `LineSegment::getIntersection2DSValue` function can find that the point with (x,y,z) = (1,0,0) in the cartesian coordinate system does not collide to `line.`. - * If the function works valid, the variable `collision_s` should be `std::nullopt`. - */ - // [Snippet_getIntersection2DSValue_with_point_1_0_0] - { - const auto collision_s = line.getIntersection2DSValue( - geometry_msgs::build().x(1).y(0).z(0), false); - EXPECT_FALSE(collision_s); - } - // [Snippet_getIntersection2DSValue_with_point_1_0_0] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2DSValue_with_point_1_0_0 - - { // parallel no denormalize - EXPECT_THROW( - line.getIntersection2DSValue( - math::geometry::LineSegment(makePoint(0.0, -1.0), makePoint(0.0, 1.0)), false), - common::SimulationError); - } - { // parallel denormalize - EXPECT_THROW( - line.getIntersection2DSValue( - math::geometry::LineSegment(makePoint(0.0, -1.0), makePoint(0.0, 1.0)), true), - common::SimulationError); - } - { // intersect no denormalize - const auto collision_s = line.getIntersection2DSValue( - math::geometry::LineSegment(makePoint(-1.0, 0.5), makePoint(1.0, 0.5)), false); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.75); - } - } - { // intersect denormalize - const auto collision_s = line.getIntersection2DSValue( - math::geometry::LineSegment(makePoint(-1.0, 0.5), makePoint(1.0, 0.5)), true); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 1.5); - } - } - { // no intersect no denormalize - const auto collision_s = line.getIntersection2DSValue( - math::geometry::LineSegment(makePoint(-1.0, 1.5), makePoint(1.0, 1.5)), false); - EXPECT_FALSE(collision_s); - } - { // no intersect denormalize - const auto collision_s = line.getIntersection2DSValue( - math::geometry::LineSegment(makePoint(-1.0, 1.5), makePoint(1.0, 1.5)), true); - EXPECT_FALSE(collision_s); - } - - /** - * @note Testing the `LineSegment::getIntersection2D` function can throw error getting intersection with exact same line segment. - * In this case, any s value can be a intersection point, so we cannot return single value. - */ - // [Snippet_getIntersection2D_with_line] - { - EXPECT_THROW(line.getIntersection2D(line), common::SimulationError); - } - // [Snippet_getIntersection2D_with_line] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2D_with_line - - /** - * @note Testing the `LineSegment::getIntersection2D` function can throw error getting intersection with part of the line segment `line`. - * In this case, any s value can be a intersection point, so we cannot return single value. - */ - // [Snippet_getIntersection2D_with_part_of_line] - { - EXPECT_THROW( - line.getIntersection2D(math::geometry::LineSegment( - geometry_msgs::build().x(0).y(-1).z(0), - geometry_msgs::build().x(0).y(0).z(0))), - common::SimulationError); - } - // [Snippet_getIntersection2D_with_part_of_line] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2D_with_part_of_line - - /// @note @note Testing the `LineSegment::getIntersection2D` function can find collision with the line segment with start point (x,y,z) = (1,0,0) and end point (x,y,z) = (-1,0,0) in the cartesian coordinate system and `line`. - // [Snippet_getIntersection2D_line_1_0_0_-1_0_0] - { - const auto point = line.getIntersection2D(math::geometry::LineSegment( - geometry_msgs::build().x(1).y(0).z(0), - geometry_msgs::build().x(-1).y(0).z(0))); - EXPECT_TRUE(point); - if (point) { - EXPECT_POINT_EQ( - point.value(), geometry_msgs::build().x(0).y(0).z(0)); - } - } - // [Snippet_getIntersection2D_line_1_0_0_-1_0_0] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2D_line_1_0_0_-1_0_0 - } -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 63833bad90b..554de126098 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,115 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 3ef06abc051..ef3fd96b1eb 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 3.5.5 + 4.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 4e42bf48f5b..3ae5e0e607e 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,115 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 208e16e125e..52b345ecc29 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 3.5.5 + 4.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 ef178805641..977da120e29 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,115 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 1ea1e1962d5..6e9f7ddfe55 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 3.5.5 + 4.2.2 none Tatsuya Yamasaki Apache License 2.0 diff --git a/docs/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md b/docs/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md index 6a721ea6fef..b7c7ae97ae7 100644 --- a/docs/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md +++ b/docs/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md @@ -1,7 +1,7 @@ # Lane pose calculation when getting longitudinal distance Attempts to calculate the pose for adjacent lane coordinate systems when measuring longitudinal distance. -The length of the horizontal bar must intersect with the adjacent lanelet, [so it is always 10 m regardless of the entity type.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/entity_manager.cpp#L375C21-L442) +The length of the horizontal bar must intersect with the adjacent lanelet, [so it is always 10 m regardless of the entity type.](https://github.com/tier4/scenario_simulator_v2/blob/f577103d685523a3e8e19fd72c9a1c3c2d032704/simulation/traffic_simulator/src/utils/distance.cpp#L48-L122) ![Get longitudinal distance](../../image/longitudinal_distance.png "Getting longitudinal distance.") diff --git a/docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md b/docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md index b00317b8735..445e789dc18 100644 --- a/docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md +++ b/docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md @@ -12,19 +12,23 @@ These are the [timing immediately after the spawn of the Entity](Spawn.md), the ## Lane coordinate system calculation algorithm for a specific lane !!! summary - - The calculation of the pose in the lane coordinate system involves two steps: first, filtering by "which lanes can be matched," and then "calculating the pose in the specific lane coordinate system. + - The calculation of the pose in the lane coordinate system involves two steps: first, filtering by "which lanes can be matched," and then "calculating the pose in the specific lane coordinate system". - The parameter that most affects the matching results is the length of the horizontal bar. This length depends on the type of entity and the timing of the calculation. If you want to check the length quickly, please check [this section](#quick-guide-to-horizontal-bar-lengths). ### Quick guide to horizontal bar lengths The unit of the table is meter. -| EntityType \ Timing of calculation | [Spawn](Spawn.md) | [UpdateFrame](UpdateFrame.md) | [Getting longitudinal distance](GetLongitudinalDistance.md) | -| ---------------------------------- | --------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------- | ----------------------------------------------------------- | -| EgoEntity | [(tread of the entity) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system) | [(tread of the entity) + 2.0](UpdateFrame.md#calculate-pose-in-lane-coordinate-system) | 10 | -| VehicleEntity | [(tread of the entity) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system) | N/A | 10 | -| PedestrianEntity | [(width of the entity bounding box) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system) | [2.0 or 4.0](UpdateFrame.md#pedestrian-entity-with-behavior-tree) | 10 | -| MiscObject | [(width of the entity bounding box) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system) | N/A | 10 | +Tread of the Entity is the maximum value of the tread from its front and rear wheels divided by two. +Tread of the front wheels is $t_f$ and the tread of the rear wheels is $t_r$. +$tread = \frac{max(t_f, t_r)}{2}$ + +| EntityType \ Timing of calculation | [Spawn](Spawn.md) | [UpdateFrame](UpdateFrame.md) | [Getting longitudinal distance](GetLongitudinalDistance.md) | +| ---------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| EgoEntity | [(tread of the entity) + 1.0](Spawn.md#calculate-pose-in-lane-coordinate-system) ([code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L458-L468)) | [(tread of the entity) + 1.0](UpdateFrame.md#calculate-pose-in-lane-coordinate-system) ([code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/vehicle_entity.cpp#L78-L85), [code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp#L453-L456)) | 10 ([code](https://github.com/tier4/scenario_simulator_v2/blob/f577103d685523a3e8e19fd72c9a1c3c2d032704/simulation/traffic_simulator/src/utils/distance.cpp#L48-L122)) | +| VehicleEntity | [(tread of the entity) + 1.0](Spawn.md#calculate-pose-in-lane-coordinate-system) ([code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L458-L468)) | [(tread of the entity) + 1.0](UpdateFrame.md#calculate-pose-in-lane-coordinate-system) ([code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/vehicle_entity.cpp#L78-L85)) | 10 ([code](https://github.com/tier4/scenario_simulator_v2/blob/f577103d685523a3e8e19fd72c9a1c3c2d032704/simulation/traffic_simulator/src/utils/distance.cpp#L48-L122)) | +| PedestrianEntity | [(width of the entity bounding box) + 1.0](Spawn.md#calculate-pose-in-lane-coordinate-system) ([code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L458-L468)) | [(width of the entity bounding box) + 1.0](UpdateFrame.md#calculate-pose-in-lane-coordinate-system) ([code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/entity_base.cpp#L86-L89)) | 10 ([code](https://github.com/tier4/scenario_simulator_v2/blob/f577103d685523a3e8e19fd72c9a1c3c2d032704/simulation/traffic_simulator/src/utils/distance.cpp#L48-L122)) | +| MiscObject | [(width of the entity bounding box) + 1.0](Spawn.md#calculate-pose-in-lane-coordinate-system) ([code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L458-L468)) | N/A | 10 ([code](https://github.com/tier4/scenario_simulator_v2/blob/f577103d685523a3e8e19fd72c9a1c3c2d032704/simulation/traffic_simulator/src/utils/distance.cpp#L48-L122)) | ### Detail of the lane coordinate system calculation algorithm for a specific lane diff --git a/docs/developer_guide/lane_pose_calculation/Spawn.md b/docs/developer_guide/lane_pose_calculation/Spawn.md index a78890e3fad..7531148f5dc 100644 --- a/docs/developer_guide/lane_pose_calculation/Spawn.md +++ b/docs/developer_guide/lane_pose_calculation/Spawn.md @@ -1,15 +1,15 @@ # Lane pose calculation when spawning -If you spawn entities by specifying in the lane coordinate system, lane pose calculation is skipped. +If you spawn entities by specifying in the lane coordinate system, [lane pose calculation is skipped](https://github.com/tier4/scenario_simulator_v2/blob/0ddeacd1eab22ffc334202ff9a5c458b5569dd32/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L473-L485). Internally, spawning entities by specifying in the lane coordinate system means specifying LaneletPose as the argument pose of the [API::spawn function](https://tier4.github.io/scenario_simulator_v2-api-docs/classtraffic__simulator_1_1API.html). In the OpenSCENARIO, it is to execute a TeleportAction by specifying the LanePosition. ([e.g](https://github.com/tier4/scenario_simulator_v2/blob/9f03394f80e5de05cf087db3d00c7be73d27e963/test_runner/scenario_test_runner/)) -If you spawn entities by specifying in the map coordinate system, [lane pose calculation](https://github.com/tier4/scenario_simulator_v2/blob/9f03394f80e5de05cf087db3d00c7be73d27e963/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L521-L529) is performed. +If you spawn entities by specifying in the map coordinate system, [lane pose calculation is performed](https://github.com/tier4/scenario_simulator_v2/blob/0ddeacd1eab22ffc334202ff9a5c458b5569dd32/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L473-L485). There are several internal implementations of pose calculation in the lane coordinate system implemented in the [HDmapUtils class](https://tier4.github.io/scenario_simulator_v2-api-docs/classhdmap__utils_1_1HdMapUtils.html). The one used in this case is [here](https://github.com/tier4/scenario_simulator_v2/blob/9f03394f80e5de05cf087db3d00c7be73d27e963/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp#L602-L630). -In this function, the following procedure is used to calculate the pose in the lane coordinate system. +[This function](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/utils/pose.cpp#L101-L122) and its overloads are commonly used to convert map pose to the lane coordinate system. ## Search for matching lanes @@ -34,10 +34,10 @@ After narrowing down the candidate lanes by the above procedure, the pose of the Let $L_m$ be the length of the horizontal bar used in the lane coordinate system calculation and the tread of the front wheels be $t_f$ and the tread of the rear wheels be $t_r$. -$$L_m = max(t_r, t_f) + 2.0$$ +$$L_m = max(t_r, t_f) + 1.0$$ ### Pedestrian / MiscObject Entity Let $L_m$ be the length of the horizontal bar used in the lane coordinate system calculation and width of the bounding box be $L_w$ -$$L_m = L_w + 2.0$$ +$$L_m = L_w + 1.0$$ diff --git a/docs/developer_guide/lane_pose_calculation/UpdateFrame.md b/docs/developer_guide/lane_pose_calculation/UpdateFrame.md index bc201cc8d2f..09098fb3df8 100644 --- a/docs/developer_guide/lane_pose_calculation/UpdateFrame.md +++ b/docs/developer_guide/lane_pose_calculation/UpdateFrame.md @@ -5,16 +5,14 @@ If the entity's behavioral logic is planned in the lane coordinate system, skip ## Ego Entity Since EgoEntity is controlled in map coordinates by Autoware rather than in lane coordinates using a motion plugin, this process is performed at each frame update. -This process is implemented [here](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/api/api.cpp#L240C9-L240C19) and [here](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L276-L312). +This process is implemented [API](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/api/api.cpp#L302) -> [EgoEntity::setMapPose](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/ego_entity.cpp#L294-L300) -> [EgoEntity::setStatus](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/ego_entity.cpp#L309-L320). ### Search for matching lanes -This process branches off into 2 or 3 depending on the outcome of 1. -If 3 is executed and fails, fallbacks to 4. +This process branches off into 2. -1. [Obtain a candidate lane for matching from Autoware planner output.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L276) -2. [If Autoware has no planner output, try obtaining candidate lanes for matching considering the bounding box.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L286-L287) -3. [If Autoware has planner output, try matching considering the Autoware planner output.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L289-L290) -4. [If the step 3 failed, try obtaining candidate lanes for matching considering the bounding box.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L292-L293) +1. [Obtain a candidate lane for matching from Autoware planner output.](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/ego_entity.cpp#L296) +2. [If Autoware has planner output, try matching considering the Autoware planner output.](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/utils/pose.cpp#L110-L111) +3. [If Autoware has no planner output or 2 fails, try obtaining candidate lanes for matching considering the bounding box.](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/utils/pose.cpp#L114-L115) ### Calculate pose in lane coordinate system @@ -27,12 +25,14 @@ $$L_m = max(t_r, t_f) + 2.0$$ ## Non-Ego Entity -For non-EgoEntity, the process diverges depending on whether planning is done in the lane coordinate system inside BehaviorPlugin. -If planning is done in the lane coordinate system, there is no need to convert from the map coordinate system to the lane coordinate system after update frame function is executed. +For non-Ego Entities, the pose calculation process is carried out immediately after getting the updated EntityStatus from BehaviorTree. +If the planning is done in the lane coordinate system, the value of `lanelet_pose_valid` is equal to true, which causes [only position canonicalization](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/data_type/entity_status.cpp#L75-L76) to be performed. ### VehicleEntity (with Behavior-Tree) -Only during `follow_polyline_trajectory` execution is planning performed in map coordinate system, but [lane coordinate system calculations are not performed](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp#L546) +Only during `follow_polyline_trajectory` execution is planning performed in map coordinate system, but [lane coordinate system calculations are not performed](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp#L561) + +This process is implemented [VehicleEntity::onUpdate](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/vehicle_entity.cpp#L164-L165) -> [EntityBase::setStatus](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/entity_base.cpp#L520-L523) -> [CanonicalizedEntityStatus::set](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/data_type/entity_status.cpp#L66-L84). ### Vehicle Entity (with Do-Nothing) @@ -42,21 +42,22 @@ While the do-nothing behavior plugin is running, Entity does not move, so the la Planning is done in map coordinates in the `walk_straight` and `follow_polyline_trajectory` actions. -In the `walk_straight` Action, [the pose in the lane coordinate system is calculated.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L56) +In the `walk_straight` Action, [the pose in the lane coordinate system is calculated.](https://github.com/tier4/scenario_simulator_v2/blob/6f87603e2aaaddfd8ceb08645cb3933fe3b74515/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L59-L62) The procedure for calculating the pose in the lane coordinate system at this time is as follows. -If lane matching was successful in the previous frame, do 1, otherwise do 2 +If lane matching was successful in the previous frame (entity is currently on some unique lane), [do 1, otherwise do 2](https://github.com/tier4/scenario_simulator_v2/blob/6f87603e2aaaddfd8ceb08645cb3933fe3b74515/simulation/traffic_simulator/src/utils/pose.cpp#L293-L297): -1.[Set the length of the horizontal bar to 2.0 + the width of the bounding box and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/a2c04ee2446f80aeacfe59fc87a6737ae18692cc/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L72-L77) -2.[Calculate the pose in the lane coordinate system considering the size of the BoundingBox of the Entity](https://github.com/tier4/scenario_simulator_v2/blob/a2c04ee2446f80aeacfe59fc87a6737ae18692cc/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L79-L85) +1.[Set the length of the horizontal bar (to the width of the bounding box + 1.0) and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/utils/pose.cpp#L110-L111) +2.[Calculate the pose in the lane coordinate system considering the size of the BoundingBox of the Entity](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/utils/pose.cpp#L114-L115) If calculation 1 or 2 fails, -3.[Set the length of the horizontal bar to 4.0 and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L86-L91) +3.[Set the length of the horizontal bar to **2.0** and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/6f87603e2aaaddfd8ceb08645cb3933fe3b74515/simulation/traffic_simulator/src/utils/pose.cpp#L302-L303) If 1 or 2 are successful, then 3 is skipped. -If the pose could not be calculated in the lane coordinate system by considering up to the result of 3, [the pose calculation in the lane coordinate system is a failure](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L125). +If the pose could not be calculated in the lane coordinate system by considering up to the result of 3, [the pose calculation in the lane coordinate system is a failure](https://github.com/tier4/scenario_simulator_v2/blob/6f87603e2aaaddfd8ceb08645cb3933fe3b74515/simulation/traffic_simulator/src/utils/pose.cpp#L335). -[Canonicalize pose in lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L94) to determine the final pose in the lane coordinate system. +[Canonicalize pose in lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/6f87603e2aaaddfd8ceb08645cb3933fe3b74515/simulation/traffic_simulator/src/utils/pose.cpp#L303) to determine the final pose in the lane coordinate system. +If final canonicalize failed, [set end of road lanelet pose](https://github.com/tier4/scenario_simulator_v2/blob/6f87603e2aaaddfd8ceb08645cb3933fe3b74515/simulation/traffic_simulator/src/utils/pose.cpp#L302-L332). In the `follow_polyline_trajectory` Action, [lane coordinate system calculations are not performed](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp#L546) diff --git a/docs/image/realtime_factor/panel.png b/docs/image/realtime_factor/panel.png new file mode 100644 index 00000000000..46d25a6333b Binary files /dev/null and b/docs/image/realtime_factor/panel.png differ diff --git a/docs/image/realtime_factor/slider.png b/docs/image/realtime_factor/slider.png new file mode 100644 index 00000000000..90e50131323 Binary files /dev/null and b/docs/image/realtime_factor/slider.png differ diff --git a/docs/image/realtime_factor/video.mp4 b/docs/image/realtime_factor/video.mp4 new file mode 100644 index 00000000000..52f487d9484 Binary files /dev/null and b/docs/image/realtime_factor/video.mp4 differ diff --git a/docs/user_guide/scenario_test_runner/.pages b/docs/user_guide/scenario_test_runner/.pages index a4999386dbe..ea72fd1cbe4 100644 --- a/docs/user_guide/scenario_test_runner/.pages +++ b/docs/user_guide/scenario_test_runner/.pages @@ -2,4 +2,5 @@ nav: - Overview: ScenarioTestRunner.md - ScenarioFormatConversion.md - HowToWriteWorkflowFile.md + - RealtimeFactor.md - Tips.md diff --git a/docs/user_guide/scenario_test_runner/RealtimeFactor.md b/docs/user_guide/scenario_test_runner/RealtimeFactor.md new file mode 100644 index 00000000000..e8b1cbe5d66 --- /dev/null +++ b/docs/user_guide/scenario_test_runner/RealtimeFactor.md @@ -0,0 +1,78 @@ +# How to use realtime factor + +It is possible to modify the speed of simulation (the speed of time published on the /clock topic is not affected): + +- from the start of the simulation (using parameter), +- during the simulation (using the GUI slider). + +## Use parameter + + - When you run simulations on the command line, add an `global_real_time_factor` parameter with a custom value (the default is 1.0). + + ```bash + ros2 launch scenario_test_runner scenario_test_runner.launch.py \ + architecture_type:=awf/universe/20230906 \ + record:=false \ + scenario:='$(find-pkg-share scenario_test_runner)/scenario/sample.yaml' \ + sensor_model:=sample_sensor_kit \ + vehicle_model:=sample_vehicle \ + global_real_time_factor:="0.5" + ``` + + - The smaller the value you specify, the slower the simulation will progress. + +## Use slider on run time + +- When the simulation is started you can add the `RViz` panel by clicking `Panels -> Add new panel` in the top left corner of RViz. + +- Then in the pop up window please select `RealTimeFactorSliderPanel` and double click on it. + +![Panel](../../image/realtime_factor/panel.png) + +- Slider controlling the speed of simulation time should be visible on the left side of the screen. + +![Slider](../../image/realtime_factor/slider.png) + +- The process of adding the panel is also visible in the video: + + + + +## Configure `use_sim_time` parameter + +Parameter `use_sim_time` of `openscenario_interpreter` is **false** by default and can be modified by passing it using command line. + + + ```bash + ros2 launch scenario_test_runner scenario_test_runner.launch.py \ + architecture_type:=awf/universe/20230906 \ + record:=false \ + scenario:='$(find-pkg-share scenario_test_runner)/scenario/sample.yaml' \ + sensor_model:=sample_sensor_kit \ + vehicle_model:=sample_vehicle \ + global_real_time_factor:="0.5" \ + use_sim_time:=true + ``` + +However, this impacts the time published on the `/clock` topic and the time used by `Autoware`. +Details are shown in the table below: + +| use_sim_time launch parameter | /clock time published by scenario_simulator_v2 | AWF Autoware Time | +| ----------------------------- | ---------------------------------------------- | ---------------------- | +| false (default) | walltime | walltime from /clock | +| true | simulation | simulation from /clock | + +Below are also some bullet points explaining the impact of the `use_sim_time` parameter on `scenario_simulator_v2` and `Autoware`: + + - **`use_sim_time:=True` passed using command line** + - Both Autoware and scenario_simulator_v2 are launched with `use_sim_time=true`. + - Time published on `/clock` is the **simulation time** (starting from 0). + - Time published on `/clock` **can be** controlled by RViz plugin. + - Simulation time **can be** controlled by RViz plugin. + - **`use_sim_time:=False` passed using command line (default value)** + - Both Autoware and scenario_simulator_v2 are launched with `use_sim_time=false`. + - Time published on `/clock` is the **walltime**. + - Time published on `/clock` **cannot be** controlled by RViz plugin. + - Simulation time **can be** controlled by RViz plugin. diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 496b5cc1478..182b8a56937 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,118 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ +* Merge pull request `#1362 `_ from tier4/feature/ros2-parameter-forwarding +* Add feature to forward parameters prefixed with `autoware.` to Autoware +* Contributors: Kotaro Yoshimoto, yamacir-kit + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/external/concealer/include/concealer/launch.hpp b/external/concealer/include/concealer/launch.hpp index 99ba2556654..54f1459f1b3 100644 --- a/external/concealer/include/concealer/launch.hpp +++ b/external/concealer/include/concealer/launch.hpp @@ -33,8 +33,9 @@ namespace concealer { -template -auto ros2_launch(const std::string & package, const std::string & file, Ts &&... xs) +template +auto ros2_launch( + const std::string & package, const std::string & file, const Parameters & parameters) { #ifdef CONCEALER_ISOLATE_STANDARD_OUTPUT const std::string log_filename = "/tmp/scenario_test_runner/autoware-output.txt"; @@ -44,17 +45,23 @@ auto ros2_launch(const std::string & package, const std::string & file, Ts &&... ::close(fd); #endif - const auto process_id = fork(); + const auto argv = [&]() { + auto argv = std::vector(); - const std::vector argv{ - "python3", - boost::algorithm::replace_all_copy(dollar("which ros2"), "\n", ""), - "launch", // NOTE: The command 'ros2' is a Python script. - package, - file, - std::forward(xs)...}; + argv.push_back("python3"); + argv.push_back(boost::algorithm::replace_all_copy(dollar("which ros2"), "\n", "")); + argv.push_back("launch"); + argv.push_back(package); + argv.push_back(file); - if (process_id < 0) { + for (const auto & parameter : parameters) { + argv.push_back(parameter); + } + + return argv; + }(); + + if (const auto process_id = fork(); process_id < 0) { throw std::system_error(errno, std::system_category()); } else if (process_id == 0 and execute(argv) < 0) { std::cout << std::system_error(errno, std::system_category()).what() << std::endl; diff --git a/external/concealer/package.xml b/external/concealer/package.xml index f9d1b400e66..ff2fb906b99 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 3.5.5 + 4.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 8500522b236..fac779b36c5 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,115 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index c190c5d3bb9..009829846a5 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 3.5.5 + 4.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 7356e061c79..ba56daf1487 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,115 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 6ca6d218b1b..69327d2912f 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 3.5.5 + 4.2.2 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/map/simple_cross_map/CHANGELOG.rst b/map/simple_cross_map/CHANGELOG.rst index 506f5b401e8..34c60095023 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,93 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index 916ccdb0f93..e0702113e58 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 3.5.5 + 4.2.2 map package for simple cross Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index 2ad6a3324e6..5cf1013e47f 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,120 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge pull request `#1320 `_ from tier4/ref/RJD-1053-set-update-canonicalized-entity-status + ref(behavior_tree, traffic_simulator): move responsibility for canonicalization to traffic_simulator, simplify +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(traffic_simulator): use getCanonicalizedStatus, remove getStatus +* feat(cpp_mock_scenarios): add isPedestrain and isVehicle - use it +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(cpp_mack_utils): adapt define_traffic_source scenarios to getEntity() +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp index 5989f077922..aa7df2b5dea 100644 --- a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp +++ b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp @@ -52,6 +52,10 @@ class CppScenarioNode : public rclcpp::Node const std::vector & goal_lanelet_pose, const traffic_simulator_msgs::msg::VehicleParameters & parameters); + auto isVehicle(const std::string & name) const -> bool; + + auto isPedestrian(const std::string & name) const -> bool; + protected: traffic_simulator::API api_; common::junit::JUnit5 junit_; diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 51337dcd9d9..0fea79cf6ed 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 3.5.5 + 4.2.2 C++ mock scenarios masaya Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index dff52151963..47ca8c25d98 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -131,6 +131,18 @@ void CppScenarioNode::spawnEgoEntity( } } +auto CppScenarioNode::isVehicle(const std::string & name) const -> bool +{ + return api_.getEntity(name)->getEntityType().type == + traffic_simulator_msgs::msg::EntityType::VEHICLE; +} + +auto CppScenarioNode::isPedestrian(const std::string & name) const -> bool +{ + return api_.getEntity(name)->getEntityType().type == + traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; +} + void CppScenarioNode::checkConfiguration(const traffic_simulator::Configuration & configuration) { try { diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 30f552af916..e1369f81976 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -138,9 +138,8 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode } /// Checking the ego entity overs the lane change position. if (const auto entity = api_.getEntity("ego"); entity->laneMatchingSucceed()) { - if ( - entity->getStatus().getLaneletId() == 34684 && - std::abs(entity->getStatus().getLaneletPose().s) >= lane_change_position) { + const auto lanelet_pose = entity->getCanonicalizedStatus().getLaneletPose(); + if (lanelet_pose.lanelet_id == 34684 && std::abs(lanelet_pose.s) >= lane_change_position) { api_.requestLaneChange(entity_name, traffic_simulator::lane_change::Direction::RIGHT); lane_change_requested = true; } 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 6028842df1c..46532152560 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 @@ -75,19 +75,14 @@ class DefineTrafficSourceDelay : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); + if (const auto entity = api_.getEntity(name)) { + const bool valid_vehicle_lanelet = + api_.isInLanelet(name, static_cast(34705), 50.0) || + api_.isInLanelet(name, static_cast(34696), 50.0); - const bool is_vehicle = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::VEHICLE; - - const bool valid_vehicle_lanelet = - api_.isInLanelet(name, static_cast(34705), 50.0) || - api_.isInLanelet(name, static_cast(34696), 50.0); - - if (!entity_status.laneMatchingSucceed() || !valid_vehicle_lanelet || !is_vehicle) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if (!entity->laneMatchingSucceed() || !valid_vehicle_lanelet || !isVehicle(name)) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } } stop(cpp_mock_scenarios::Result::SUCCESS); 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 6fff5eeb1c5..7a5c8d97a83 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 @@ -52,19 +52,14 @@ class DefineTrafficSourceHighRate : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); + if (const auto entity = api_.getEntity(name)) { + const bool valid_vehicle_lanelet = + api_.isInLanelet(name, static_cast(34705), 50.0) || + api_.isInLanelet(name, static_cast(34696), 50.0); - const bool is_vehicle = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::VEHICLE; - - const bool valid_vehicle_lanelet = - api_.isInLanelet(name, static_cast(34705), 50.0) || - api_.isInLanelet(name, static_cast(34696), 50.0); - - if (!entity_status.laneMatchingSucceed() || !valid_vehicle_lanelet || !is_vehicle) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if (!entity->laneMatchingSucceed() || !valid_vehicle_lanelet || !isVehicle(name)) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } } stop(cpp_mock_scenarios::Result::SUCCESS); 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 735e09b09e7..6d7976cfa05 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 @@ -52,15 +52,10 @@ class DefineTrafficSourceLarge : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); - - const bool is_vehicle = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::VEHICLE; - - if (!entity_status.laneMatchingSucceed() || !is_vehicle) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if (const auto entity = api_.getEntity(name)) { + if (!entity->laneMatchingSucceed() || !isVehicle(name)) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } } stop(cpp_mock_scenarios::Result::SUCCESS); 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 5c11cf4e960..4f9141d9d92 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 @@ -53,28 +53,20 @@ class DefineTrafficSourceMixed : public cpp_mock_scenarios::CppScenarioNode } unsigned int vehicle_count = 0u, pedestrian_count = 0u; for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); + if (const auto entity = api_.getEntity(name)) { + const bool valid_vehicle_lanelet = + api_.isInLanelet(name, static_cast(34705), 50.0) || + api_.isInLanelet(name, static_cast(34696), 50.0); - const bool is_vehicle = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::VEHICLE; - const bool is_pedestrian = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; + if (isVehicle(name)) { + ++vehicle_count; + } else if (isPedestrian(name)) { + ++pedestrian_count; + } - const bool valid_vehicle_lanelet = - api_.isInLanelet(name, static_cast(34705), 50.0) || - api_.isInLanelet(name, static_cast(34696), 50.0); - - if (is_vehicle) { - ++vehicle_count; - } else if (is_pedestrian) { - ++pedestrian_count; - } - - if (!entity_status.laneMatchingSucceed() || !valid_vehicle_lanelet) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if (!entity->laneMatchingSucceed() || !valid_vehicle_lanelet) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } } if (vehicle_count == 0u || pedestrian_count == 0u) { 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 0cab69203d9..f4ba78518ff 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 @@ -53,37 +53,29 @@ class DefineTrafficSourceMultiple : public cpp_mock_scenarios::CppScenarioNode } unsigned int vehicle_count = 0u, pedestrian_count = 0u; for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); + if (const auto entity = api_.getEntity(name)) { + const bool valid_vehicle_lanelet = + api_.isInLanelet(name, static_cast(34705), 50.0) || + api_.isInLanelet(name, static_cast(34696), 50.0); - const bool is_vehicle = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::VEHICLE; - const bool is_pedestrian = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; + const bool valid_pedestrian_lanelet = + api_.isInLanelet(name, static_cast(34385), 10.0); - const bool valid_vehicle_lanelet = - api_.isInLanelet(name, static_cast(34705), 50.0) || - api_.isInLanelet(name, static_cast(34696), 50.0); + if (isVehicle(name)) { + ++vehicle_count; + } else if (isPedestrian(name)) { + ++pedestrian_count; + } - const bool valid_pedestrian_lanelet = - api_.isInLanelet(name, static_cast(34385), 10.0); - - if (is_vehicle) { - ++vehicle_count; - } else if (is_pedestrian) { - ++pedestrian_count; - } - - if ( - // clang-format off - !entity_status.laneMatchingSucceed() || - (is_vehicle && !valid_vehicle_lanelet) || - (is_pedestrian && !valid_pedestrian_lanelet)) - // clang-format on - { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if ( + // clang-format off + !entity->laneMatchingSucceed() || + (isVehicle(name) && !valid_vehicle_lanelet) || + (isPedestrian(name) && !valid_pedestrian_lanelet)) + // clang-format on + { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } } if (vehicle_count != 6u || pedestrian_count != 15u) { 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 9881f6b3e31..02db2b0bd26 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 @@ -52,18 +52,13 @@ class DefineTrafficSourceOutsideLane : public cpp_mock_scenarios::CppScenarioNod stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); - - const bool is_pedestrian = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; - - if (entity_status.laneMatchingSucceed() || !is_pedestrian) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if (const auto entity = api_.getEntity(name)) { + if (entity->laneMatchingSucceed() || !isPedestrian(name)) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } + stop(cpp_mock_scenarios::Result::SUCCESS); } - stop(cpp_mock_scenarios::Result::SUCCESS); } } 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 b5a31ecb211..02c46bbd755 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 @@ -52,18 +52,13 @@ class DefineTrafficSourcePedestrian : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); + if (const auto entity = api_.getEntity(name)) { + const bool valid_pedestrian_lanelet = + api_.isInLanelet(name, static_cast(34385), 10.0); - const bool is_pedestrian = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; - - const bool valid_pedestrian_lanelet = - api_.isInLanelet(name, static_cast(34385), 10.0); - - if (!entity_status.laneMatchingSucceed() || !valid_pedestrian_lanelet || !is_pedestrian) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if (!entity->laneMatchingSucceed() || !valid_pedestrian_lanelet || !isPedestrian(name)) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } } stop(cpp_mock_scenarios::Result::SUCCESS); 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 699eff9d688..d38b251004f 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 @@ -52,19 +52,14 @@ class DefineTrafficSourceVehicle : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); + if (const auto entity = api_.getEntity(name)) { + const bool valid_vehicle_lanelet = + api_.isInLanelet(name, static_cast(34705), 50.0) || + api_.isInLanelet(name, static_cast(34696), 50.0); - const bool is_vehicle = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::VEHICLE; - - const bool valid_vehicle_lanelet = - api_.isInLanelet(name, static_cast(34705), 50.0) || - api_.isInLanelet(name, static_cast(34696), 50.0); - - if (!entity_status.laneMatchingSucceed() || !valid_vehicle_lanelet || !is_vehicle) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if (!entity->laneMatchingSucceed() || !valid_vehicle_lanelet || !isVehicle(name)) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } } stop(cpp_mock_scenarios::Result::SUCCESS); diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 83ddd710daf..94d30305999 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,115 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 9d0beec7acd..14926ad4413 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 3.5.5 + 4.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 7f9d432ec0b..0d6f45fa1d2 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,117 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index e3675d53fe6..22f7cb5e429 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 3.5.5 + 4.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 19793ec1e0c..bdcf97388e2 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,115 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index cc446b72940..6ada7470f7b 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 3.5.5 + 4.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 1d18fc6d4eb..d59b9fa3a77 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,115 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 991f3c93ce0..d3e550e165e 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 3.5.5 + 4.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 3140170a4b2..41fb89b0237 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,115 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 055c3084485..068ed93f7cb 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 3.5.5 + 4.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 51e7aac4ed4..09161cd0b25 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,115 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index cc559bbb190..47c97df09ee 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 3.5.5 + 4.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 9231435207e..56fddf8f088 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,115 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 38e808f3336..e8cb1ec2f27 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 3.5.5 + 4.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 f3d7e494552..c30c56dc4f7 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,114 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 935ec6e7754..f70cd8b863f 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 3.5.5 + 4.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 6ae1cf64306..79a3ab603c2 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,115 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 169d8665dd2..4b3ead15e69 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 3.5.5 + 4.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 7beaae2ee0b..faa54eacf67 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -21,6 +21,115 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change 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 b6e1e8d7902..7c13c86f6fd 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 - 3.5.5 + 4.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 c110372ee31..24f7862fafa 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,118 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ +* Spelling changes +* Lint changes +* Contributors: Grzegorz Maj + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 1e3b0acf02c..283519dc6a6 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 3.5.5 + 4.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 201793a253b..4e250c4642c 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,129 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge pull request `#1356 `_ from tier4/RJD-1056-remove-current-time-step-time + Remove unused data members: current_time step_time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Trailing return type +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Add const to time argument in behavior +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge pull request `#1320 `_ from tier4/ref/RJD-1053-set-update-canonicalized-entity-status + ref(behavior_tree, traffic_simulator): move responsibility for canonicalization to traffic_simulator, simplify +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(behavior_tree): remove unused variable +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(behavior_tree): use CanonicalizedEntityStatus as shared_ptr inside BT and use ::set +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(behavior_tree_plugin): rename updated_status variable +* ref(traffic_simulator, behavior_tree_plugin): revert unnecessary changes +* fix(traffic_simulator, behavior_tree_plugin): fix returned EntityStatus from bt, fix canonicalize in vehicle_entity and pedestrian_entity +* feat(behavior_tree_plugin, entity_base): move toCanonicalizedEntityPose to traffic_simulator, use EntityStatus as updated_state in BehaviorTree +* feat(traffic_simulator): use CanonicalizedEntityStatus only with single constructor +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* tmp +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index dea788adccd..67d0059618c 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -65,8 +66,6 @@ class ActionNode : public BT::ActionNodeBase -> std::vector; auto stopEntity() const -> void; auto getHorizon() const -> double; - auto getActionStatus() const noexcept -> traffic_simulator_msgs::msg::ActionStatus; - auto getEntityName() const noexcept -> std::string; /// throws if the derived class return RUNNING. auto executeTick() -> BT::NodeStatus override; @@ -84,11 +83,10 @@ class ActionNode : public BT::ActionNodeBase BT::InputPort("route_lanelets"), BT::InputPort>("target_speed"), BT::InputPort>("hdmap_utils"), - BT::InputPort>("entity_status"), + BT::InputPort>("canonicalized_entity_status"), BT::InputPort>("traffic_light_manager"), BT::InputPort("request"), BT::OutputPort>("obstacle"), - BT::OutputPort>("updated_status"), BT::OutputPort("waypoints"), BT::OutputPort("request"), // clang-format on @@ -96,29 +94,30 @@ class ActionNode : public BT::ActionNodeBase } auto getBlackBoardValues() -> void; auto getEntityStatus(const std::string & target_name) const - -> traffic_simulator::CanonicalizedEntityStatus; + -> const traffic_simulator::CanonicalizedEntityStatus &; auto getDistanceToTargetEntityPolygon( const math::geometry::CatmullRomSplineInterface & spline, const std::string target_name, double width_extension_right = 0.0, double width_extension_left = 0.0, double length_extension_front = 0.0, double length_extension_rear = 0.0) const -> std::optional; + + auto setCanonicalizedEntityStatus(const traffic_simulator::EntityStatus & entity_status) -> void; auto calculateUpdatedEntityStatus( double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const - -> traffic_simulator::CanonicalizedEntityStatus; + -> traffic_simulator::EntityStatus; auto calculateUpdatedEntityStatusInWorldFrame( double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const - -> traffic_simulator::CanonicalizedEntityStatus; + -> traffic_simulator::EntityStatus; protected: traffic_simulator::behavior::Request request; std::shared_ptr hdmap_utils; std::shared_ptr traffic_light_manager; - std::shared_ptr entity_status; + std::shared_ptr canonicalized_entity_status; double current_time; double step_time; double default_matching_distance_for_lanelet_pose_calculation; std::optional target_speed; - std::shared_ptr updated_status; EntityStatusDict other_entity_status; lanelet::Ids route_lanelets; diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp index c241e49b9e0..edc05eb5d44 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp @@ -39,7 +39,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase { public: void configure(const rclcpp::Logger & logger) override; - void update(double current_time, double step_time) override; + auto update(const double current_time, const double step_time) -> void override; const std::string & getCurrentAction() const override; #define DEFINE_GETTER_SETTER(NAME, TYPE) \ @@ -51,24 +51,23 @@ class PedestrianBehaviorTree : public BehaviorPluginBase // clang-format off DEFINE_GETTER_SETTER(BehaviorParameter, traffic_simulator_msgs::msg::BehaviorParameter) + DEFINE_GETTER_SETTER(CanonicalizedEntityStatus, std::shared_ptr) DEFINE_GETTER_SETTER(CurrentTime, double) DEFINE_GETTER_SETTER(DebugMarker, std::vector) DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, double) DEFINE_GETTER_SETTER(GoalPoses, std::vector) - DEFINE_GETTER_SETTER(EntityStatus, std::shared_ptr) - DEFINE_GETTER_SETTER(PolylineTrajectory, std::shared_ptr) 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(PolylineTrajectory, std::shared_ptr) DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr) DEFINE_GETTER_SETTER(Request, traffic_simulator::behavior::Request) DEFINE_GETTER_SETTER(RouteLanelets, lanelet::Ids) DEFINE_GETTER_SETTER(StepTime, double) DEFINE_GETTER_SETTER(TargetSpeed, std::optional) DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) - DEFINE_GETTER_SETTER(UpdatedStatus, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) // clang-format on @@ -76,7 +75,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase #undef DEFINE_GETTER_SETTER private: - BT::NodeStatus tickOnce(double current_time, double step_time); + auto tickOnce(const double current_time, const double step_time) -> BT::NodeStatus; auto createBehaviorTree(const std::string & format_path) -> BT::Tree; BT::BehaviorTreeFactory factory_; BT::Tree tree_; diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp index 89b74eca32d..7d173c0b124 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp @@ -45,9 +45,8 @@ class PedestrianActionNode : public ActionNode } traffic_simulator_msgs::msg::PedestrianParameters pedestrian_parameters; auto calculateUpdatedEntityStatusInWorldFrame(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus; - auto calculateUpdatedEntityStatus(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus; + -> traffic_simulator::EntityStatus; + auto calculateUpdatedEntityStatus(double target_speed) const -> traffic_simulator::EntityStatus; protected: traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp index 6add89d7ed4..59351fe827c 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp @@ -38,7 +38,7 @@ namespace entity_behavior class VehicleBehaviorTree : public BehaviorPluginBase { public: - void update(double current_time, double step_time) override; + auto update(const double current_time, const double step_time) -> void override; void configure(const rclcpp::Logger & logger) override; const std::string & getCurrentAction() const override; @@ -55,10 +55,10 @@ class VehicleBehaviorTree : public BehaviorPluginBase } // clang-format off + DEFINE_GETTER_SETTER(CanonicalizedEntityStatus, std::shared_ptr) DEFINE_GETTER_SETTER(CurrentTime, double) DEFINE_GETTER_SETTER(DebugMarker, std::vector) DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, double) - DEFINE_GETTER_SETTER(EntityStatus, 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) @@ -72,14 +72,13 @@ class VehicleBehaviorTree : public BehaviorPluginBase DEFINE_GETTER_SETTER(StepTime, double) DEFINE_GETTER_SETTER(TargetSpeed, std::optional) DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) - DEFINE_GETTER_SETTER(UpdatedStatus, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) // clang-format on #undef DEFINE_GETTER_SETTER private: - BT::NodeStatus tickOnce(double current_time, double step_time); + auto tickOnce(const double current_time, const double step_time) -> BT::NodeStatus; auto createBehaviorTree(const std::string & format_path) -> BT::Tree; BT::BehaviorTreeFactory factory_; BT::Tree tree_; diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/vehicle_action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/vehicle_action_node.hpp index 1428e9f3199..024ede1096c 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/vehicle_action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/vehicle_action_node.hpp @@ -53,10 +53,9 @@ class VehicleActionNode : public ActionNode } return ports; } - auto calculateUpdatedEntityStatus(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus; + auto calculateUpdatedEntityStatus(double target_speed) const -> traffic_simulator::EntityStatus; auto calculateUpdatedEntityStatusInWorldFrame(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus; + -> traffic_simulator::EntityStatus; virtual const traffic_simulator_msgs::msg::WaypointsArray calculateWaypoints() = 0; virtual const std::optional calculateObstacle( const traffic_simulator_msgs::msg::WaypointsArray & waypoints) = 0; diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 89b5ac53c57..e67b4a928e7 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 3.5.5 + 4.2.2 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 5f9b4501dac..29fcb393d7d 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -59,8 +59,8 @@ auto ActionNode::getBlackBoardValues() -> void THROW_SIMULATION_ERROR("failed to get input traffic_light_manager in ActionNode"); } if (!getInput>( - "entity_status", entity_status)) { - THROW_SIMULATION_ERROR("failed to get input entity_status in ActionNode"); + "canonicalized_entity_status", canonicalized_entity_status)) { + THROW_SIMULATION_ERROR("failed to get input canonicalized_entity_status in ActionNode"); } if (!getInput>("target_speed", target_speed)) { @@ -84,14 +84,21 @@ auto ActionNode::getBlackBoardValues() -> void auto ActionNode::getHorizon() const -> double { - return std::clamp(entity_status->getTwist().linear.x * 5.0, 20.0, 50.0); + return std::clamp(canonicalized_entity_status->getTwist().linear.x * 5.0, 20.0, 50.0); } auto ActionNode::stopEntity() const -> void { - entity_status->setTwist(geometry_msgs::msg::Twist()); - entity_status->setAccel(geometry_msgs::msg::Accel()); - entity_status->setLinearJerk(0); + canonicalized_entity_status->setTwist(geometry_msgs::msg::Twist()); + canonicalized_entity_status->setAccel(geometry_msgs::msg::Accel()); + canonicalized_entity_status->setLinearJerk(0); +} + +auto ActionNode::setCanonicalizedEntityStatus(const traffic_simulator::EntityStatus & entity_status) + -> void +{ + canonicalized_entity_status->set( + entity_status, default_matching_distance_for_lanelet_pose_calculation, hdmap_utils); } auto ActionNode::getOtherEntityStatus(lanelet::Id lanelet_id) const @@ -116,8 +123,8 @@ auto ActionNode::getYieldStopDistance(const lanelet::Ids & following_lanelets) c const auto right_of_way_ids = hdmap_utils->getRightOfWayLaneletIds(lanelet); for (const auto right_of_way_id : right_of_way_ids) { const auto other_status = getOtherEntityStatus(right_of_way_id); - if (!other_status.empty() && entity_status->laneMatchingSucceed()) { - const auto lanelet_pose = entity_status->getLaneletPose(); + if (!other_status.empty() && canonicalized_entity_status->laneMatchingSucceed()) { + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); const auto distance_forward = hdmap_utils->getLongitudinalDistance( lanelet_pose, traffic_simulator::helper::constructLaneletPose(lanelet, 0)); const auto distance_backward = hdmap_utils->getLongitudinalDistance( @@ -167,11 +174,12 @@ auto ActionNode::getRightOfWayEntities(const lanelet::Ids & following_lanelets) auto ActionNode::getRightOfWayEntities() const -> std::vector { - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { return {}; } std::vector ret; - const auto lanelet_ids = hdmap_utils->getRightOfWayLaneletIds(entity_status->getLaneletId()); + const auto lanelet_ids = + hdmap_utils->getRightOfWayLaneletIds(canonicalized_entity_status->getLaneletId()); if (lanelet_ids.empty()) { return ret; } @@ -240,7 +248,7 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf for (const auto & each : other_entity_status) { const auto distance = getDistanceToTargetEntityPolygon(spline, each.first); const auto quat = math::geometry::getRotation( - entity_status->getMapPose().orientation, + canonicalized_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. @@ -277,12 +285,13 @@ auto ActionNode::getDistanceToTargetEntityOnCrosswalk( } auto ActionNode::getEntityStatus(const std::string & target_name) const - -> traffic_simulator::CanonicalizedEntityStatus + -> const traffic_simulator::CanonicalizedEntityStatus & { - if (other_entity_status.find(target_name) != other_entity_status.end()) { - return traffic_simulator::CanonicalizedEntityStatus(other_entity_status.at(target_name)); + if (auto it = other_entity_status.find(target_name); it != other_entity_status.end()) { + return it->second; + } else { + THROW_SEMANTIC_ERROR("other entity : ", target_name, " does not exist."); } - THROW_SIMULATION_ERROR("other entity : ", target_name, " does not exist."); } auto ActionNode::getDistanceToTargetEntityPolygon( @@ -290,7 +299,7 @@ auto ActionNode::getDistanceToTargetEntityPolygon( double width_extension_right, double width_extension_left, double length_extension_front, double length_extension_rear) const -> std::optional { - const auto status = getEntityStatus(target_name); + const auto & status = getEntityStatus(target_name); if (status.laneMatchingSucceed()) { return getDistanceToTargetEntityPolygon( spline, status, width_extension_right, width_extension_left, length_extension_front, @@ -397,40 +406,45 @@ auto ActionNode::foundConflictingEntity(const lanelet::Ids & following_lanelets) auto ActionNode::calculateUpdatedEntityStatus( double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const - -> traffic_simulator::CanonicalizedEntityStatus + -> traffic_simulator::EntityStatus { const auto speed_planner = traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner( - step_time, getEntityName()); + step_time, canonicalized_entity_status->getName()); const auto dynamics = speed_planner.getDynamicStates( - target_speed, constraints, entity_status->getTwist(), entity_status->getAccel()); + target_speed, constraints, canonicalized_entity_status->getTwist(), + canonicalized_entity_status->getAccel()); const double linear_jerk_new = std::get<2>(dynamics); const geometry_msgs::msg::Accel accel_new = std::get<1>(dynamics); const geometry_msgs::msg::Twist twist_new = std::get<0>(dynamics); - if (!entity_status->laneMatchingSucceed()) { - THROW_SIMULATION_ERROR("Entity ", entity_status->getName(), " is not matched to the lanelet."); + if (!canonicalized_entity_status->laneMatchingSucceed()) { + THROW_SIMULATION_ERROR( + "Entity ", canonicalized_entity_status->getName(), " is not matched to the lanelet."); } else { - auto lanelet_pose = entity_status->getLaneletPose(); + auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); lanelet_pose.s = - lanelet_pose.s + (twist_new.linear.x + entity_status->getTwist().linear.x) / 2.0 * step_time; + lanelet_pose.s + + (twist_new.linear.x + canonicalized_entity_status->getTwist().linear.x) / 2.0 * step_time; const auto canonicalized = hdmap_utils->canonicalizeLaneletPose(lanelet_pose, route_lanelets); if ( const auto canonicalized_lanelet_pose = std::get>(canonicalized)) { // If canonicalize succeed, set canonicalized pose and set other values. - traffic_simulator::EntityStatus entity_status_updated; + auto entity_status_updated = + static_cast(*canonicalized_entity_status); { entity_status_updated.time = current_time + step_time; entity_status_updated.lanelet_pose = canonicalized_lanelet_pose.value(); + entity_status_updated.lanelet_pose_valid = true; entity_status_updated.action_status.twist = twist_new; entity_status_updated.action_status.accel = accel_new; entity_status_updated.action_status.linear_jerk = linear_jerk_new; entity_status_updated.pose = hdmap_utils->toMapPose(canonicalized_lanelet_pose.value()).pose; } - return traffic_simulator::CanonicalizedEntityStatus(entity_status_updated, hdmap_utils); + return entity_status_updated; } else { // If canonicalize failed, set end of road lanelet pose. if (const auto end_of_road_lanelet_id = std::get>(canonicalized)) { @@ -442,16 +456,18 @@ auto ActionNode::calculateUpdatedEntityStatus( end_of_road_lanelet_pose.offset = lanelet_pose.offset; end_of_road_lanelet_pose.rpy = lanelet_pose.rpy; } - traffic_simulator::EntityStatus entity_status_updated; + auto entity_status_updated = + static_cast(*canonicalized_entity_status); { entity_status_updated.time = current_time + step_time; entity_status_updated.lanelet_pose = end_of_road_lanelet_pose; + entity_status_updated.lanelet_pose_valid = true; entity_status_updated.action_status.twist = twist_new; entity_status_updated.action_status.accel = accel_new; entity_status_updated.action_status.linear_jerk = linear_jerk_new; entity_status_updated.pose = hdmap_utils->toMapPose(end_of_road_lanelet_pose).pose; } - return traffic_simulator::CanonicalizedEntityStatus(entity_status_updated, hdmap_utils); + return entity_status_updated; } else { traffic_simulator::LaneletPose end_of_road_lanelet_pose; { @@ -461,16 +477,18 @@ auto ActionNode::calculateUpdatedEntityStatus( end_of_road_lanelet_pose.offset = lanelet_pose.offset; end_of_road_lanelet_pose.rpy = lanelet_pose.rpy; } - traffic_simulator::EntityStatus entity_status_updated; + auto entity_status_updated = + static_cast(*canonicalized_entity_status); { entity_status_updated.time = current_time + step_time; entity_status_updated.lanelet_pose = end_of_road_lanelet_pose; + entity_status_updated.lanelet_pose_valid = true; entity_status_updated.action_status.twist = twist_new; entity_status_updated.action_status.accel = accel_new; entity_status_updated.action_status.linear_jerk = linear_jerk_new; entity_status_updated.pose = hdmap_utils->toMapPose(end_of_road_lanelet_pose).pose; } - return traffic_simulator::CanonicalizedEntityStatus(entity_status_updated, hdmap_utils); + return entity_status_updated; } } else { THROW_SIMULATION_ERROR("Failed to find trailing lanelet_id."); @@ -481,14 +499,15 @@ auto ActionNode::calculateUpdatedEntityStatus( auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const - -> traffic_simulator::CanonicalizedEntityStatus + -> traffic_simulator::EntityStatus { using math::geometry::operator*; const auto speed_planner = traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner( - step_time, getEntityName()); + step_time, canonicalized_entity_status->getName()); const auto dynamics = speed_planner.getDynamicStates( - target_speed, constraints, entity_status->getTwist(), entity_status->getAccel()); + target_speed, constraints, canonicalized_entity_status->getTwist(), + canonicalized_entity_status->getAccel()); double linear_jerk_new = std::get<2>(dynamics); geometry_msgs::msg::Accel accel_new = std::get<1>(dynamics); geometry_msgs::msg::Twist twist_new = std::get<0>(dynamics); @@ -497,44 +516,35 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( angular_trans_vec.z = twist_new.angular.z * step_time; geometry_msgs::msg::Quaternion angular_trans_quat = math::geometry::convertEulerAngleToQuaternion(angular_trans_vec); - pose_new.orientation = entity_status->getMapPose().orientation * angular_trans_quat; + pose_new.orientation = canonicalized_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 = 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; - pose_new.position.z = trans_vec(2) + entity_status->getMapPose().position.z; - traffic_simulator::EntityStatus entity_status_updated; + pose_new.position.x = trans_vec(0) + canonicalized_entity_status->getMapPose().position.x; + pose_new.position.y = trans_vec(1) + canonicalized_entity_status->getMapPose().position.y; + pose_new.position.z = trans_vec(2) + canonicalized_entity_status->getMapPose().position.z; + auto entity_status_updated = + static_cast(*canonicalized_entity_status); entity_status_updated.time = current_time + step_time; + entity_status_updated.lanelet_pose = traffic_simulator::LaneletPose(); + entity_status_updated.lanelet_pose_valid = false; entity_status_updated.pose = pose_new; entity_status_updated.action_status.twist = twist_new; entity_status_updated.action_status.accel = accel_new; entity_status_updated.action_status.linear_jerk = linear_jerk_new; - entity_status_updated.lanelet_pose_valid = false; - entity_status_updated.lanelet_pose = traffic_simulator::LaneletPose(); - return traffic_simulator::CanonicalizedEntityStatus(entity_status_updated, hdmap_utils); + return entity_status_updated; } auto ActionNode::calculateStopDistance( const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const -> double { return traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner( - step_time, getEntityName()) + step_time, canonicalized_entity_status->getName()) .getRunningDistance( - 0, constraints, entity_status->getTwist(), entity_status->getAccel(), - entity_status->getLinearJerk()); -} - -auto ActionNode::getActionStatus() const noexcept -> traffic_simulator_msgs::msg::ActionStatus -{ - return static_cast(*entity_status).action_status; -} - -auto ActionNode::getEntityName() const noexcept -> std::string -{ - return static_cast(*entity_status).name; + 0, constraints, canonicalized_entity_status->getTwist(), + canonicalized_entity_status->getAccel(), canonicalized_entity_status->getLinearJerk()); } } // namespace entity_behavior diff --git a/simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp b/simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp index bc083253ca0..4cfaf3a8ea5 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp @@ -83,7 +83,7 @@ const std::string & PedestrianBehaviorTree::getCurrentAction() const return logging_event_ptr_->getCurrentAction(); } -void PedestrianBehaviorTree::update(double current_time, double step_time) +auto PedestrianBehaviorTree::update(const double current_time, const double step_time) -> void { tickOnce(current_time, step_time); while (getCurrentAction() == "root") { @@ -91,7 +91,8 @@ void PedestrianBehaviorTree::update(double current_time, double step_time) } } -BT::NodeStatus PedestrianBehaviorTree::tickOnce(double current_time, double step_time) +auto PedestrianBehaviorTree::tickOnce(const double current_time, const double step_time) + -> BT::NodeStatus { setCurrentTime(current_time); setStepTime(step_time); 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 06833c75b3a..c24d13d66b1 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp @@ -38,18 +38,16 @@ BT::NodeStatus FollowLaneAction::tick() request != traffic_simulator::behavior::Request::FOLLOW_LANE) { return BT::NodeStatus::FAILURE; } - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { stopEntity(); - setOutput("updated_status", entity_status); return BT::NodeStatus::RUNNING; } - auto following_lanelets = hdmap_utils->getFollowingLanelets(entity_status->getLaneletId()); + auto following_lanelets = + hdmap_utils->getFollowingLanelets(canonicalized_entity_status->getLaneletId()); if (!target_speed) { target_speed = hdmap_utils->getSpeedLimit(following_lanelets); } - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); return BT::NodeStatus::RUNNING; } } // namespace pedestrian diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 7e3ca5a6592..8e722f5afb0 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -22,7 +22,7 @@ auto FollowPolylineTrajectoryAction::calculateWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray { auto waypoints = traffic_simulator_msgs::msg::WaypointsArray(); - waypoints.waypoints.push_back(entity_status->getMapPose().position); + waypoints.waypoints.push_back(canonicalized_entity_status->getMapPose().position); for (const auto & vertex : polyline_trajectory->shape.vertices) { waypoints.waypoints.push_back(vertex.position.position); } @@ -60,31 +60,26 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus if (target_speed.has_value()) { return target_speed.value(); } else { - return entity_status->getTwist().linear.x; + return canonicalized_entity_status->getTwist().linear.x; } }; - auto getMatchingDistance = [&]() -> double { - return entity_status->getBoundingBox().dimensions.y * 0.5 + 1.0; - }; - if (getBlackBoardValues(); request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or not getInput("polyline_trajectory", polyline_trajectory) or not getInput("target_speed", target_speed) or not polyline_trajectory) { return BT::NodeStatus::FAILURE; - } else if (std::isnan(entity_status->getTime())) { + } else if (std::isnan(canonicalized_entity_status->getTime())) { THROW_SIMULATION_ERROR( - "Time in entity_status is NaN - FollowTrajectoryAction does not support such case."); + "Time in canonicalized_entity_status is NaN - FollowTrajectoryAction does not support such " + "case."); } else if ( - const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus( - static_cast(*entity_status), *polyline_trajectory, - behavior_parameter, hdmap_utils, step_time, getMatchingDistance(), getTargetSpeed())) { - setOutput( - "updated_status", - std::make_shared( - traffic_simulator::CanonicalizedEntityStatus(*updated_status, hdmap_utils))); + const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus( + static_cast(*canonicalized_entity_status), + *polyline_trajectory, behavior_parameter, hdmap_utils, step_time, + default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed())) { + setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); setOutput("obstacle", calculateObstacle(calculateWaypoints())); return BT::NodeStatus::RUNNING; diff --git a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp index 1f6f0630ae0..cbf2f304c22 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp @@ -42,31 +42,30 @@ void PedestrianActionNode::getBlackBoardValues() } auto PedestrianActionNode::calculateUpdatedEntityStatus(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus + -> traffic_simulator::EntityStatus { return ActionNode::calculateUpdatedEntityStatus( target_speed, behavior_parameter.dynamic_constraints); } auto PedestrianActionNode::calculateUpdatedEntityStatusInWorldFrame(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus + -> traffic_simulator::EntityStatus { - auto updated_status = static_cast( - ActionNode::calculateUpdatedEntityStatusInWorldFrame( - target_speed, behavior_parameter.dynamic_constraints)); - + auto entity_status_updated = ActionNode::calculateUpdatedEntityStatusInWorldFrame( + target_speed, behavior_parameter.dynamic_constraints); if ( const auto canonicalized_lanelet_pose = traffic_simulator::pose::pedestrian::transformToCanonicalizedLaneletPose( - updated_status.pose, entity_status->getBoundingBox(), entity_status->getLaneletIds(), true, + entity_status_updated.pose, canonicalized_entity_status->getBoundingBox(), + canonicalized_entity_status->getLaneletIds(), true, default_matching_distance_for_lanelet_pose_calculation, hdmap_utils)) { - updated_status.lanelet_pose_valid = true; - updated_status.lanelet_pose = + entity_status_updated.lanelet_pose_valid = true; + entity_status_updated.lanelet_pose = static_cast(canonicalized_lanelet_pose.value()); } else { - updated_status.lanelet_pose_valid = false; - updated_status.lanelet_pose = traffic_simulator::LaneletPose(); + entity_status_updated.lanelet_pose_valid = false; + entity_status_updated.lanelet_pose = traffic_simulator::LaneletPose(); } - return traffic_simulator::CanonicalizedEntityStatus(updated_status, hdmap_utils); + return entity_status_updated; } } // namespace entity_behavior diff --git a/simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp index a58f7a8a30c..e4b8f5f3a95 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp @@ -47,9 +47,7 @@ BT::NodeStatus WalkStraightAction::tick() if (!target_speed) { target_speed = 1.111; } - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatusInWorldFrame(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatusInWorldFrame(target_speed.value())); return BT::NodeStatus::RUNNING; } } // namespace pedestrian diff --git a/simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp b/simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp index 4a9e22c1c56..fbb49c65d63 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp @@ -142,7 +142,7 @@ const std::string & VehicleBehaviorTree::getCurrentAction() const return logging_event_ptr_->getCurrentAction(); } -void VehicleBehaviorTree::update(double current_time, double step_time) +auto VehicleBehaviorTree::update(const double current_time, const double step_time) -> void { tickOnce(current_time, step_time); while (getCurrentAction() == "root") { @@ -150,7 +150,8 @@ void VehicleBehaviorTree::update(double current_time, double step_time) } } -BT::NodeStatus VehicleBehaviorTree::tickOnce(double current_time, double step_time) +auto VehicleBehaviorTree::tickOnce(const double current_time, const double step_time) + -> BT::NodeStatus { setCurrentTime(current_time); setStepTime(step_time); diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp index 5535262b4bf..8c30fce024a 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp @@ -52,13 +52,13 @@ FollowFrontEntityAction::calculateObstacle(const traffic_simulator_msgs::msg::Wa const traffic_simulator_msgs::msg::WaypointsArray FollowFrontEntityAction::calculateWaypoints() { - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { THROW_SIMULATION_ERROR("failed to assign lane"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { traffic_simulator_msgs::msg::WaypointsArray waypoints; double horizon = getHorizon(); - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); waypoints.waypoints = reference_trajectory->getTrajectory( lanelet_pose.s, lanelet_pose.s + horizon, 1.0, lanelet_pose.offset); trajectory = std::make_unique( @@ -111,15 +111,13 @@ BT::NodeStatus FollowFrontEntityAction::tick() return BT::NodeStatus::FAILURE; } } - auto front_entity_status = getEntityStatus(front_entity_name.value()); + const auto & front_entity_status = getEntityStatus(front_entity_name.value()); if (!target_speed) { target_speed = hdmap_utils->getSpeedLimit(route_lanelets); } const double front_entity_linear_velocity = front_entity_status.getTwist().linear.x; if (target_speed.value() <= front_entity_linear_velocity) { - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); const auto obstacle = calculateObstacle(waypoints); setOutput("waypoints", waypoints); setOutput("obstacle", obstacle); @@ -129,25 +127,19 @@ BT::NodeStatus FollowFrontEntityAction::tick() distance_to_front_entity_.value() >= (calculateStopDistance(behavior_parameter.dynamic_constraints) + vehicle_parameters.bounding_box.dimensions.x + 5)) { - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(front_entity_linear_velocity + 2))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(front_entity_linear_velocity + 2)); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::RUNNING; } else if ( distance_to_front_entity_.value() <= calculateStopDistance(behavior_parameter.dynamic_constraints)) { - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(front_entity_linear_velocity - 2))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(front_entity_linear_velocity - 2)); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::RUNNING; } else { - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(front_entity_linear_velocity))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(front_entity_linear_velocity)); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::RUNNING; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp index 036c5409a04..1b70dd26599 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp @@ -39,12 +39,12 @@ const std::optional FollowLaneAction::cal const traffic_simulator_msgs::msg::WaypointsArray FollowLaneAction::calculateWaypoints() { - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { THROW_SIMULATION_ERROR("failed to assign lane"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { traffic_simulator_msgs::msg::WaypointsArray waypoints; - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); waypoints.waypoints = reference_trajectory->getTrajectory( lanelet_pose.s, lanelet_pose.s + getHorizon(), 1.0, lanelet_pose.offset); trajectory = std::make_unique( @@ -74,9 +74,8 @@ BT::NodeStatus FollowLaneAction::tick() request != traffic_simulator::behavior::Request::FOLLOW_LANE) { return BT::NodeStatus::FAILURE; } - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { stopEntity(); - setOutput("updated_status", entity_status); return BT::NodeStatus::RUNNING; } const auto waypoints = calculateWaypoints(); @@ -129,9 +128,7 @@ BT::NodeStatus FollowLaneAction::tick() if (!target_speed) { target_speed = hdmap_utils->getSpeedLimit(route_lanelets); } - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::RUNNING; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp index 051452aee52..d1412b22029 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp @@ -35,13 +35,13 @@ const std::optional MoveBackwardAction::c const traffic_simulator_msgs::msg::WaypointsArray MoveBackwardAction::calculateWaypoints() { - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { THROW_SIMULATION_ERROR("failed to assign lane"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { return traffic_simulator_msgs::msg::WaypointsArray(); } - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); const auto ids = hdmap_utils->getPreviousLanelets(lanelet_pose.lanelet_id); // DIFFERENT SPLINE - recalculation needed math::geometry::CatmullRomSpline spline(hdmap_utils->getCenterPoints(ids)); @@ -70,7 +70,7 @@ BT::NodeStatus MoveBackwardAction::tick() request != traffic_simulator::behavior::Request::FOLLOW_LANE) { return BT::NodeStatus::FAILURE; } - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { return BT::NodeStatus::FAILURE; } const auto waypoints = calculateWaypoints(); @@ -78,12 +78,11 @@ BT::NodeStatus MoveBackwardAction::tick() return BT::NodeStatus::FAILURE; } if (!target_speed) { - target_speed = - hdmap_utils->getSpeedLimit(hdmap_utils->getPreviousLanelets(entity_status->getLaneletId())); + target_speed = hdmap_utils->getSpeedLimit( + hdmap_utils->getPreviousLanelets(canonicalized_entity_status->getLaneletId())); } - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::RUNNING; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp index 2ca745bd199..c85e6cdf622 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp @@ -54,12 +54,12 @@ StopAtCrossingEntityAction::calculateObstacle(const traffic_simulator_msgs::msg: const traffic_simulator_msgs::msg::WaypointsArray StopAtCrossingEntityAction::calculateWaypoints() { - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { THROW_SIMULATION_ERROR("failed to assign lane"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { traffic_simulator_msgs::msg::WaypointsArray waypoints; - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); waypoints.waypoints = reference_trajectory->getTrajectory( lanelet_pose.s, lanelet_pose.s + getHorizon(), 1.0, lanelet_pose.offset); trajectory = std::make_unique( @@ -92,7 +92,7 @@ BT::NodeStatus StopAtCrossingEntityAction::tick() in_stop_sequence_ = false; return BT::NodeStatus::FAILURE; } - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { in_stop_sequence_ = false; return BT::NodeStatus::FAILURE; } @@ -132,14 +132,12 @@ BT::NodeStatus StopAtCrossingEntityAction::tick() } std::optional target_linear_speed; if (distance_to_stop_target_) { - target_linear_speed = calculateTargetSpeed(entity_status->getTwist().linear.x); + target_linear_speed = calculateTargetSpeed(canonicalized_entity_status->getTwist().linear.x); } else { target_linear_speed = std::nullopt; } if (!distance_to_stop_target_) { - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(0))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(0)); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); in_stop_sequence_ = false; @@ -152,9 +150,7 @@ BT::NodeStatus StopAtCrossingEntityAction::tick() } else { target_speed = target_linear_speed.value(); } - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); in_stop_sequence_ = true; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp index 8e31993f431..23011a89e47 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp @@ -53,13 +53,13 @@ const std::optional StopAtStopLineAction: const traffic_simulator_msgs::msg::WaypointsArray StopAtStopLineAction::calculateWaypoints() { - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { THROW_SIMULATION_ERROR("failed to assign lane"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { traffic_simulator_msgs::msg::WaypointsArray waypoints; double horizon = getHorizon(); - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); waypoints.waypoints = reference_trajectory->getTrajectory( lanelet_pose.s, lanelet_pose.s + horizon, 1.0, lanelet_pose.offset); trajectory = std::make_unique( @@ -128,7 +128,7 @@ BT::NodeStatus StopAtStopLineAction::tick() } } - if (std::fabs(entity_status->getTwist().linear.x) < 0.001) { + if (std::fabs(canonicalized_entity_status->getTwist().linear.x) < 0.001) { if (distance_to_stopline_) { if (distance_to_stopline_.value() <= vehicle_parameters.bounding_box.dimensions.x + 5) { stopped_ = true; @@ -141,21 +141,17 @@ BT::NodeStatus StopAtStopLineAction::tick() } if (!distance_to_stopline_) { stopped_ = false; - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::SUCCESS; } - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::RUNNING; } - auto target_linear_speed = calculateTargetSpeed(entity_status->getTwist().linear.x); + auto target_linear_speed = calculateTargetSpeed(canonicalized_entity_status->getTwist().linear.x); if (!target_linear_speed) { stopped_ = false; return BT::NodeStatus::FAILURE; @@ -167,9 +163,7 @@ BT::NodeStatus StopAtStopLineAction::tick() } else { target_speed = target_linear_speed.value(); } - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); stopped_ = false; setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp index 823ae227cd3..e8033ff1ad7 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp @@ -52,12 +52,12 @@ StopAtTrafficLightAction::calculateObstacle(const traffic_simulator_msgs::msg::W const traffic_simulator_msgs::msg::WaypointsArray StopAtTrafficLightAction::calculateWaypoints() { - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { THROW_SIMULATION_ERROR("failed to assign lane"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { traffic_simulator_msgs::msg::WaypointsArray waypoints; - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); waypoints.waypoints = reference_trajectory->getTrajectory( lanelet_pose.s, lanelet_pose.s + getHorizon(), 1.0, lanelet_pose.offset); trajectory = std::make_unique( @@ -92,7 +92,7 @@ BT::NodeStatus StopAtTrafficLightAction::tick() request != traffic_simulator::behavior::Request::FOLLOW_LANE) { return BT::NodeStatus::FAILURE; } - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { return BT::NodeStatus::FAILURE; } if (!behavior_parameter.see_around) { @@ -114,14 +114,12 @@ BT::NodeStatus StopAtTrafficLightAction::tick() if (distance_to_stop_target_.value() > getHorizon()) { return BT::NodeStatus::FAILURE; } - target_linear_speed = calculateTargetSpeed(entity_status->getTwist().linear.x); + target_linear_speed = calculateTargetSpeed(canonicalized_entity_status->getTwist().linear.x); } else { return BT::NodeStatus::FAILURE; } if (!distance_to_stop_target_) { - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(0))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(0)); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::SUCCESS; @@ -133,9 +131,7 @@ BT::NodeStatus StopAtTrafficLightAction::tick() } else { target_speed = target_linear_speed.value(); } - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); const auto obstacle = calculateObstacle(waypoints); setOutput("waypoints", waypoints); setOutput("obstacle", obstacle); diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/yield_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/yield_action.cpp index 74e44e45393..002db268065 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/yield_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/yield_action.cpp @@ -52,13 +52,13 @@ const std::optional YieldAction::calculat const traffic_simulator_msgs::msg::WaypointsArray YieldAction::calculateWaypoints() { - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { THROW_SIMULATION_ERROR("failed to assign lane"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { traffic_simulator_msgs::msg::WaypointsArray waypoints; double horizon = getHorizon(); - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); waypoints.waypoints = reference_trajectory->getTrajectory( lanelet_pose.s, lanelet_pose.s + horizon, 1.0, lanelet_pose.offset); trajectory = std::make_unique( @@ -82,7 +82,7 @@ std::optional YieldAction::calculateTargetSpeed() if (rest_distance < calculateStopDistance(behavior_parameter.dynamic_constraints)) { return 0; } - return entity_status->getTwist().linear.x; + return canonicalized_entity_status->getTwist().linear.x; } BT::NodeStatus YieldAction::tick() @@ -96,7 +96,7 @@ BT::NodeStatus YieldAction::tick() if (!behavior_parameter.see_around) { return BT::NodeStatus::FAILURE; } - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { return BT::NodeStatus::FAILURE; } const auto right_of_way_entities = getRightOfWayEntities(route_lanelets); @@ -104,9 +104,7 @@ BT::NodeStatus YieldAction::tick() if (!target_speed) { target_speed = hdmap_utils->getSpeedLimit(route_lanelets); } - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); const auto waypoints = calculateWaypoints(); if (waypoints.waypoints.empty()) { return BT::NodeStatus::FAILURE; @@ -121,9 +119,7 @@ BT::NodeStatus YieldAction::tick() if (!target_speed) { target_speed = hdmap_utils->getSpeedLimit(route_lanelets); } - setOutput( - "updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); const auto waypoints = calculateWaypoints(); if (waypoints.waypoints.empty()) { return BT::NodeStatus::FAILURE; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 98466d1c735..c2c097a0da2 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -22,7 +22,7 @@ auto FollowPolylineTrajectoryAction::calculateWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray { auto waypoints = traffic_simulator_msgs::msg::WaypointsArray(); - waypoints.waypoints.push_back(entity_status->getMapPose().position); + waypoints.waypoints.push_back(canonicalized_entity_status->getMapPose().position); for (const auto & vertex : polyline_trajectory->shape.vertices) { waypoints.waypoints.push_back(vertex.position.position); } @@ -60,35 +60,26 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus if (target_speed.has_value()) { return target_speed.value(); } else { - return entity_status->getTwist().linear.x; + return canonicalized_entity_status->getTwist().linear.x; } }; - auto getMatchingDistance = [&]() -> double { - return std::max( - vehicle_parameters.axles.front_axle.track_width, - vehicle_parameters.axles.rear_axle.track_width) * - 0.5 + - 1.0; - }; - if (getBlackBoardValues(); request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or not getInput("polyline_trajectory", polyline_trajectory) or not getInput("target_speed", target_speed) or not polyline_trajectory) { return BT::NodeStatus::FAILURE; - } else if (std::isnan(entity_status->getTime())) { + } else if (std::isnan(canonicalized_entity_status->getTime())) { THROW_SIMULATION_ERROR( - "Time in entity_status is NaN - FollowTrajectoryAction does not support such case."); + "Time in canonicalized_entity_status is NaN - FollowTrajectoryAction does not support such " + "case."); } else if ( - const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus( - static_cast(*entity_status), *polyline_trajectory, - behavior_parameter, hdmap_utils, step_time, getMatchingDistance(), getTargetSpeed())) { - setOutput( - "updated_status", - std::make_shared( - traffic_simulator::CanonicalizedEntityStatus(*updated_status, hdmap_utils))); + const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus( + static_cast(*canonicalized_entity_status), + *polyline_trajectory, behavior_parameter, hdmap_utils, step_time, + default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed())) { + setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); setOutput("obstacle", calculateObstacle(calculateWaypoints())); return BT::NodeStatus::RUNNING; diff --git a/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp index f5b381ab989..593915b94b0 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp @@ -46,7 +46,7 @@ const traffic_simulator_msgs::msg::WaypointsArray LaneChangeAction::calculateWay if (!lane_change_parameters_) { THROW_SIMULATION_ERROR("lane change parameter is null"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { traffic_simulator_msgs::msg::WaypointsArray waypoints; double horizon = getHorizon(); auto following_lanelets = @@ -102,10 +102,10 @@ BT::NodeStatus LaneChangeAction::tick() } if (!curve_) { if (request == traffic_simulator::behavior::Request::LANE_CHANGE) { - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { return BT::NodeStatus::FAILURE; } - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); if (!hdmap_utils->canChangeLane( lanelet_pose.lanelet_id, lane_change_parameters_->target.lanelet_id)) { return BT::NodeStatus::FAILURE; @@ -115,7 +115,7 @@ BT::NodeStatus LaneChangeAction::tick() switch (lane_change_parameters_->constraint.type) { case traffic_simulator::lane_change::Constraint::Type::NONE: /** - @note Hard coded parameter, + @note Hard coded parameter, 10.0 is a maximum_curvature_threshold (If the curvature of the trajectory is over 10.0, the trajectory was not selected.) 20.0 is a target_trajectory_length (The one with the closest length to 20 m is selected from the candidate trajectories.) 1.0 is a forward_distance_threshold (If the goal x position in the cartesian coordinate was under 1.0, the goal was rejected.) @@ -156,14 +156,14 @@ BT::NodeStatus LaneChangeAction::tick() .position.y); switch (lane_change_parameters_->constraint.type) { case traffic_simulator::lane_change::Constraint::Type::NONE: - lane_change_velocity_ = entity_status->getTwist().linear.x; + lane_change_velocity_ = canonicalized_entity_status->getTwist().linear.x; break; case traffic_simulator::lane_change::Constraint::Type::LATERAL_VELOCITY: lane_change_velocity_ = curve_->getLength() / (offset / lane_change_parameters_->constraint.value); break; case traffic_simulator::lane_change::Constraint::Type::LONGITUDINAL_DISTANCE: - lane_change_velocity_ = entity_status->getTwist().linear.x; + lane_change_velocity_ = canonicalized_entity_status->getTwist().linear.x; break; case traffic_simulator::lane_change::Constraint::Type::TIME: lane_change_velocity_ = curve_->getLength() / lane_change_parameters_->constraint.value; @@ -181,17 +181,18 @@ BT::NodeStatus LaneChangeAction::tick() * @brief Force changing speed in order to fulfill constraint. */ case traffic_simulator::lane_change::Constraint::Policy::FORCE: - entity_status->setTwist(geometry_msgs::msg::Twist()); - entity_status->setAccel(geometry_msgs::msg::Accel()); - entity_status->setLinearVelocity(lane_change_velocity_); - current_s_ = current_s_ + entity_status->getTwist().linear.x * step_time; + canonicalized_entity_status->setTwist(geometry_msgs::msg::Twist()); + canonicalized_entity_status->setAccel(geometry_msgs::msg::Accel()); + canonicalized_entity_status->setLinearVelocity(lane_change_velocity_); + current_s_ = current_s_ + canonicalized_entity_status->getTwist().linear.x * step_time; break; /** * @brief Changing linear speed and try to fulfill constraint. */ case traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT: - target_accel = (lane_change_velocity_ - entity_status->getTwist().linear.x) / step_time; - if (entity_status->getTwist().linear.x > target_speed) { + target_accel = + (lane_change_velocity_ - canonicalized_entity_status->getTwist().linear.x) / step_time; + if (canonicalized_entity_status->getTwist().linear.x > target_speed) { target_accel = std::clamp( target_accel, behavior_parameter.dynamic_constraints.max_deceleration * -1.0, 0.0); } else { @@ -205,33 +206,26 @@ BT::NodeStatus LaneChangeAction::tick() * @note Hard coded parameter, -10.0 is a minimum linear velocity of the entity. */ twist_new.linear.x = std::clamp( - entity_status->getTwist().linear.x + accel_new.linear.x * step_time, -10.0, + canonicalized_entity_status->getTwist().linear.x + accel_new.linear.x * step_time, -10.0, vehicle_parameters.performance.max_speed); twist_new.linear.y = 0.0; twist_new.linear.z = 0.0; twist_new.angular.x = 0.0; twist_new.angular.y = 0.0; twist_new.angular.z = 0.0; - entity_status->setTwist(twist_new); - entity_status->setAccel(accel_new); - current_s_ = current_s_ + entity_status->getTwist().linear.x * step_time; + canonicalized_entity_status->setTwist(twist_new); + canonicalized_entity_status->setAccel(accel_new); + current_s_ = current_s_ + canonicalized_entity_status->getTwist().linear.x * step_time; break; } if (current_s_ < curve_->getLength()) { geometry_msgs::msg::Pose pose = curve_->getPose(current_s_, true); - traffic_simulator::EntityStatus entity_status_updated; + auto entity_status_updated = + static_cast(*canonicalized_entity_status); entity_status_updated.pose = pose; - auto lanelet_pose = hdmap_utils->toLaneletPose(pose, entity_status->getBoundingBox(), false); - if (lanelet_pose) { - entity_status_updated.lanelet_pose = lanelet_pose.value(); - } else { - entity_status_updated.lanelet_pose_valid = false; - } - entity_status_updated.action_status = getActionStatus(); - setOutput( - "updated_status", - std::make_shared( - traffic_simulator::CanonicalizedEntityStatus(entity_status_updated, hdmap_utils))); + entity_status_updated.lanelet_pose_valid = false; + entity_status_updated.action_status = canonicalized_entity_status->getActionStatus(); + setCanonicalizedEntityStatus(entity_status_updated); const auto waypoints = calculateWaypoints(); if (waypoints.waypoints.empty()) { return BT::NodeStatus::FAILURE; @@ -252,18 +246,17 @@ BT::NodeStatus LaneChangeAction::tick() curve_ = std::nullopt; current_s_ = 0; lane_change_velocity_ = 0; - traffic_simulator::EntityStatus entity_status_updated; + auto entity_status_updated = + static_cast(*canonicalized_entity_status); traffic_simulator::LaneletPose lanelet_pose; lanelet_pose.lanelet_id = lane_change_parameters_->target.lanelet_id; lanelet_pose.s = s; lanelet_pose.offset = 0; - entity_status_updated.pose = hdmap_utils->toMapPose(lanelet_pose).pose; entity_status_updated.lanelet_pose = lanelet_pose; - entity_status_updated.action_status = getActionStatus(); - setOutput( - "updated_status", - std::make_shared( - traffic_simulator::CanonicalizedEntityStatus(entity_status_updated, hdmap_utils))); + entity_status_updated.lanelet_pose_valid = true; + entity_status_updated.pose = hdmap_utils->toMapPose(lanelet_pose).pose; + entity_status_updated.action_status = canonicalized_entity_status->getActionStatus(); + setCanonicalizedEntityStatus(entity_status_updated); return BT::NodeStatus::SUCCESS; } } diff --git a/simulation/behavior_tree_plugin/src/vehicle/vehicle_action_node.cpp b/simulation/behavior_tree_plugin/src/vehicle/vehicle_action_node.cpp index 6fef4b17172..549ba448d97 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/vehicle_action_node.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/vehicle_action_node.cpp @@ -44,19 +44,19 @@ void VehicleActionNode::getBlackBoardValues() } auto VehicleActionNode::calculateUpdatedEntityStatus(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus + -> traffic_simulator::EntityStatus { return ActionNode::calculateUpdatedEntityStatus( target_speed, behavior_parameter.dynamic_constraints); } auto VehicleActionNode::calculateUpdatedEntityStatusInWorldFrame(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus + -> traffic_simulator::EntityStatus { if (target_speed > vehicle_parameters.performance.max_speed) { target_speed = vehicle_parameters.performance.max_speed; } else { - target_speed = entity_status->getTwist().linear.x; + target_speed = canonicalized_entity_status->getTwist().linear.x; } return ActionNode::calculateUpdatedEntityStatusInWorldFrame( target_speed, behavior_parameter.dynamic_constraints); diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index bce71b331ee..933d7bc003a 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,127 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge pull request `#1354 `_ from tier4/fix/follow_trajectory + Fix follow trajectory action and timestamp in entity status +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* add angular velocity / acceleration as z axis value +* enabel calculate angular accel and jerk +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* fix interpolate ration calculation +* Contributors: Masaya Kataoka, Tatsuya Yamasaki + +4.0.0 (2024-08-27) +------------------ +* Merge pull request `#1320 `_ from tier4/ref/RJD-1053-set-update-canonicalized-entity-status + ref(behavior_tree, traffic_simulator): move responsibility for canonicalization to traffic_simulator, simplify +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(behavior_tree): use CanonicalizedEntityStatus as shared_ptr inside BT and use ::set +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(do_notching_plugin): remove unecessary comment +* feat(behavior_tree_plugin, entity_base): move toCanonicalizedEntityPose to traffic_simulator, use EntityStatus as updated_state in BehaviorTree +* feat(traffic_simulator): use CanonicalizedEntityStatus only with single constructor +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* tmp +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change 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 7f82deee99a..482e3411ff7 100644 --- a/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp +++ b/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp @@ -70,14 +70,13 @@ public: \ private: \ TYPE FIELD_NAME; // clang-format off - 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_) + DEFINE_GETTER_SETTER(BehaviorParameter, traffic_simulator_msgs::msg::BehaviorParameter, behavior_parameter_) + DEFINE_GETTER_SETTER(CanonicalizedEntityStatus, std::shared_ptr, canonicalized_entity_status_) + DEFINE_GETTER_SETTER(CurrentTime, double, current_time_) + 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_) // clang-format on #undef DEFINE_GETTER_SETTER }; diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index ce8938e06e9..32673e7fe19 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 3.5.5 + 4.2.2 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 ca84dc72406..8cc19f6a727 100644 --- a/simulation/do_nothing_plugin/src/plugin.cpp +++ b/simulation/do_nothing_plugin/src/plugin.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include +#include +#include #include #include #include @@ -98,23 +100,36 @@ auto interpolateEntityStatusFromPolylineTrajectory( const auto linear_jerk = (entity_status->getAccel().linear.x - linear_acceleration) / (v1.time - v0.time); + const double angular_velocity = + math::geometry::convertQuaternionToEulerAngle( + math::geometry::getRotation(v0.position.orientation, v1.position.orientation)) + .z / + (v1.time - v0.time); + const auto angular_acceleration = + (entity_status->getTwist().angular.x - angular_velocity) / (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)); + .angular(geometry_msgs::build().x(0).y(0).z(angular_velocity)); 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)); + .angular( + geometry_msgs::build().x(0).y(0).z(angular_acceleration)); interpolated_entity_status.action_status.linear_jerk = linear_jerk; return interpolated_entity_status; }; - if ((current_time + step_time) <= trajectory->shape.vertices.begin()->time) { + if ( + (current_time + step_time) <= + (trajectory->base_time + trajectory->shape.vertices.begin()->time)) { return std::nullopt; } - if (trajectory->shape.vertices.back().time <= (current_time + step_time)) { + if ( + (trajectory->base_time + 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)); @@ -149,24 +164,28 @@ void DoNothingBehavior::update(double current_time, double step_time) 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())); + getPolylineTrajectory(), getCanonicalizedEntityStatus(), getCurrentTime(), + getStepTime())) { + return interpolated_status.value(); } else { - return entity_status_; + return static_cast(*canonicalized_entity_status_); } }; - entity_status_->setTime(current_time); + canonicalized_entity_status_->setTime(current_time); if (getRequest() == traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY) { - setUpdatedStatus(interpolate_entity_status_on_polyline_trajectory()); + canonicalized_entity_status_->set( + interpolate_entity_status_on_polyline_trajectory(), + getDefaultMatchingDistanceForLaneletPoseCalculation(), getHdMapUtils()); if ( getCurrentTime() + getStepTime() >= do_nothing_behavior::follow_trajectory::getLastVertexTimestamp(getPolylineTrajectory())) { setRequest(traffic_simulator::behavior::Request::NONE); } } else { - setUpdatedStatus(entity_status_); + canonicalized_entity_status_->set( + static_cast(*canonicalized_entity_status_), + getDefaultMatchingDistanceForLaneletPoseCalculation(), getHdMapUtils()); } } diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 0a6bd102f62..507f914cbf5 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,141 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Revert "feat(params): set use_sim_time default as True" + This reverts commit da85edf4956083563715daa3d60f0da1f94a423d. +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* feat(params): set use_sim_time default as True +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge pull request `#1301 `_ from tier4/feature/simple_sensor_simulator_unit_tests_lidar + Test: [RJD-937] to Implement Unit tests on simple_sensor_simulator +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Test: [RJD-937] to Implement Unit tests on simple_sensor_simulator + - Removed dummy class + - Updated unit tests +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Test: [RJD-937] to Implement Unit tests on simple_sensor_simulator + - Added missed header file +* Test: [RJD-937] to Implement Unit tests on simple_sensor_simulator + - Added unit tests to LidarSensor + - Addede unit tests to Primitive + - Refactored Raycaster unit tests +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge pull request `#1358 `_ from tier4/RJD-1056-remove-npc-logic-started + Remove unused data members: npc_logic_started +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* ref(ego_entity_simulation): slight improve - add const, rename current_time +* fix(ego_entity): fix autoware update when not npc_logic_started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Masaya Kataoka, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge pull request `#1320 `_ from tier4/ref/RJD-1053-set-update-canonicalized-entity-status + ref(behavior_tree, traffic_simulator): move responsibility for canonicalization to traffic_simulator, simplify +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* doc(developer_guide, traffic_simulator): update doc after review changes, add code notes +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* doc(developer_guide, pose utils): adopt lane_pose_calculation doc to canonicalization laneletpose in CanonicalizedEntityStatus +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(traffic_simulator): move toCanonicalizeLaneletPose to CanonicalizedEntityStatus::set, little tidy up +* ref(traffic_simulator): remove operator= for CanonicalizedEntityStatus, use set and assertions +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp index c10a521ba84..782d495a08c 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp @@ -92,10 +92,11 @@ class EgoEntitySimulation const bool consider_acceleration_by_road_slope); auto overwrite( - const traffic_simulator_msgs::msg::EntityStatus & status, double current_scenario_time, - double step_time, bool npc_logic_started) -> void; + const traffic_simulator_msgs::msg::EntityStatus & status, const double current_time, + const double step_time, bool is_npc_logic_started) -> void; - auto update(double time, double step_time, bool npc_logic_started) -> void; + auto update(const double current_time, const double step_time, const bool is_npc_logic_started) + -> void; auto requestSpeedChange(double value) -> void; @@ -103,7 +104,7 @@ class EgoEntitySimulation auto setStatus(const traffic_simulator_msgs::msg::EntityStatus & status) -> void; - auto updateStatus(double time, double step_time) -> void; + auto updateStatus(const double current_time, const double step_time) -> void; }; } // namespace vehicle_simulation diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 428a93898de..ed4fdbe2a13 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 3.5.5 + 4.2.2 simple_sensor_simulator package masaya kataoka 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 78790e6a589..78d75715a79 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 @@ -218,9 +218,9 @@ void EgoEntitySimulation::requestSpeedChange(double value) vehicle_model_ptr_->setState(v); } -void EgoEntitySimulation::overwrite( - const traffic_simulator_msgs::msg::EntityStatus & status, double current_scenario_time, - double step_time, bool npc_logic_started) +auto EgoEntitySimulation::overwrite( + const traffic_simulator_msgs::msg::EntityStatus & status, const double current_time, + const double step_time, const bool is_npc_logic_started) -> void { using math::geometry::convertQuaternionToEulerAngle; using math::geometry::getRotationMatrix; @@ -238,7 +238,7 @@ void EgoEntitySimulation::overwrite( status.pose.position.y - initial_pose_.position.y, status.pose.position.z - initial_pose_.position.z); - if (npc_logic_started) { + if (is_npc_logic_started) { const auto yaw = [&]() { const auto q = Eigen::Quaterniond( getRotationMatrix(initial_pose_.orientation).transpose() * @@ -280,12 +280,12 @@ void EgoEntitySimulation::overwrite( "Unsupported simulation model ", toString(vehicle_model_type_), " specified"); } } - updateStatus(current_scenario_time, step_time); + updateStatus(current_time, step_time); updatePreviousValues(); } void EgoEntitySimulation::update( - double current_scenario_time, double step_time, bool npc_logic_started) + const double current_time, const double step_time, const bool is_npc_logic_started) { using math::geometry::getRotationMatrix; @@ -302,7 +302,7 @@ void EgoEntitySimulation::update( status_.getMapPose().position.y - initial_pose_.position.y, status_.getMapPose().position.z - initial_pose_.position.z); - if (npc_logic_started) { + if (is_npc_logic_started) { auto input = Eigen::VectorXd(vehicle_model_ptr_->getDimU()); auto acceleration_by_slope = [this]() { @@ -345,7 +345,7 @@ void EgoEntitySimulation::update( // only the position in the Oz axis is left unchanged, the rest is taken from SimModelInterface world_relative_position_.x() = vehicle_model_ptr_->getX(); world_relative_position_.y() = vehicle_model_ptr_->getY(); - updateStatus(current_scenario_time, step_time); + updateStatus(current_time, step_time); updatePreviousValues(); } @@ -464,6 +464,8 @@ auto EgoEntitySimulation::getStatus() const -> const traffic_simulator_msgs::msg auto EgoEntitySimulation::setStatus(const traffic_simulator_msgs::msg::EntityStatus & status) -> void { + /// @note The lanelet matching algorithm should be equivalent to the one used in + /// EgoEntity::setStatus const auto unique_route_lanelets = traffic_simulator::helper::getUniqueValues(autoware->getRouteLanelets()); const auto matching_distance = std::max( @@ -471,17 +473,20 @@ auto EgoEntitySimulation::setStatus(const traffic_simulator_msgs::msg::EntitySta vehicle_parameters.axles.rear_axle.track_width) * 0.5 + 1.0; + /// @note Ego uses the unique_route_lanelets get from Autoware, instead of the current lanelet_id + /// value from EntityStatus, therefore canonicalization has to be done in advance, + /// not inside CanonicalizedEntityStatus const auto canonicalized_lanelet_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( status.pose, status.bounding_box, unique_route_lanelets, false, matching_distance, hdmap_utils_ptr_); - status_ = traffic_simulator::CanonicalizedEntityStatus(status, canonicalized_lanelet_pose); + status_.set(traffic_simulator::CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); setAutowareStatus(); } -auto EgoEntitySimulation::updateStatus(double current_scenario_time, double step_time) -> void +auto EgoEntitySimulation::updateStatus(const double current_time, const double step_time) -> void { auto status = static_cast(status_); - status.time = std::isnan(current_scenario_time) ? 0 : current_scenario_time; + status.time = std::isnan(current_time) ? 0 : current_time; status.pose = getCurrentPose(); status.action_status.twist = getCurrentTwist(); status.action_status.accel = getCurrentAccel(step_time); diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt index e7f6401dfef..5db9f69e675 100644 --- a/simulation/simple_sensor_simulator/test/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/CMakeLists.txt @@ -1,3 +1,6 @@ +find_package(Protobuf REQUIRED) +include_directories(${Protobuf_INCLUDE_DIRS}) + 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/lidar/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt index eeb9c2f9abc..7d16e7f14cf 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt @@ -1,5 +1,5 @@ -find_package(Protobuf REQUIRED) -include_directories(${Protobuf_INCLUDE_DIRS}) - ament_add_gtest(test_raycaster test_raycaster.cpp) target_link_libraries(test_raycaster simple_sensor_simulator_component ${Protobuf_LIBRARIES}) + +ament_add_gtest(test_lidar_sensor test_lidar_sensor.cpp) +target_link_libraries(test_lidar_sensor simple_sensor_simulator_component ${Protobuf_LIBRARIES}) 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..2a225940738 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp @@ -0,0 +1,84 @@ +// 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_lidar_sensor.hpp" + +/** + * @note Test function behavior when called on a scene without Ego entity added - the goal is to + * test error throwing. + */ +TEST_F(LidarSensorTest, update_noEgo) +{ + status_.clear(); // Remove ego + EXPECT_THROW( + lidar_->update(current_simulation_time_, status_, current_ros_time_), std::runtime_error); +} + +/** + * @note Test basic functionality. Test lidar sensor correctness on a sample scene with some vehicle + * - the goal is to check if the correct pointcloud is published on the correct topic. + */ +TEST_F(LidarSensorTest, update_correct) +{ + lidar_->update(current_simulation_time_, status_, current_ros_time_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + ASSERT_NE(received_msg_, nullptr); + const auto total_num_of_points = received_msg_->width * received_msg_->height; + EXPECT_GT(total_num_of_points, 0); + EXPECT_EQ(received_msg_->header.frame_id, "base_link"); +} + +/** + * @note Test function behavior when called with a current_time significantly smaller than one call + * earlier - the goal is to test whether the function clears detected_objects. + */ +TEST_F(LidarSensorTest, update_goBackInTime) +{ + lidar_->update(current_simulation_time_, status_, rclcpp::Time(1000)); + + // Ensure there are detected objects + ASSERT_FALSE(lidar_->getDetectedObjects().empty()); + + lidar_->update(current_simulation_time_, status_, rclcpp::Time(1)); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + EXPECT_TRUE(lidar_->getDetectedObjects().empty()); +} + +/** + * @note Test basic functionality. Test detected objects obtaining from the statuses list containing + * Ego. + */ +TEST_F(LidarSensorTest, getDetectedObjects) +{ + const std::set expected_objects = {status_[1].name(), status_[2].name()}; + + lidar_->update(current_simulation_time_, status_, current_ros_time_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + const auto & detected_objects = lidar_->getDetectedObjects(); + + // LidarSensor returns duplicates. To avoid them, a std::set is used. + const std::set unique_objects(detected_objects.begin(), detected_objects.end()); + + ASSERT_FALSE(unique_objects.empty()); + EXPECT_EQ(unique_objects, expected_objects); +} diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp new file mode 100644 index 00000000000..54bf61a03b0 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp @@ -0,0 +1,84 @@ +// 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_LIDAR_SENSOR_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__TEST__TEST_LIDAR_SENSOR_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "../../utils/helper_functions.hpp" + +using namespace simple_sensor_simulator; + +class LidarSensorTest : public ::testing::Test +{ +protected: + LidarSensorTest() : config_(utils::constructLidarConfiguration("ego", "awf/universe", 0.0, 0.5)) + { + rclcpp::init(0, nullptr); + node_ = std::make_shared("lidar_sensor_test_node"); + makeRosInterface(); + initializeEntityStatuses(); + + lidar_ = std::make_unique>(0.0, config_, publisher_); + } + + ~LidarSensorTest() { rclcpp::shutdown(); } + + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Subscription::SharedPtr subscription_; + + std::vector status_; + + std::unique_ptr lidar_; + simulation_api_schema::LidarConfiguration config_; + sensor_msgs::msg::PointCloud2::SharedPtr received_msg_; + + double current_simulation_time_{1.0}; + rclcpp::Time current_ros_time_{1}; + +private: + auto initializeEntityStatuses() -> void + { + const auto dimensions = utils::makeDimensions(4.5, 2.0, 1.5); + + const auto ego_status = utils::makeEntity( + "ego", EntityType::EGO, utils::makePose(5.0, 5.0, 0.0, 0.0, 0.0, 0.0, 1.0), dimensions); + const auto other1_status = utils::makeEntity( + "other1", EntityType::VEHICLE, utils::makePose(-3.0, -3.0, 0.0, 0.0, 0.0, 0.0, 1.0), + dimensions); + const auto other2_status = utils::makeEntity( + "other2", EntityType::VEHICLE, utils::makePose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), + dimensions); + + status_ = {ego_status, other1_status, other2_status}; + } + + auto makeRosInterface() -> void + { + publisher_ = node_->create_publisher("lidar_output", 10); + subscription_ = node_->create_subscription( + "lidar_output", 10, + [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { received_msg_ = msg; }); + } +}; +#endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_LIDAR_SENSOR_HPP_ 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 index f159ea9b575..d168ec0c80b 100644 --- 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 @@ -76,8 +76,8 @@ TEST_F(RaycasterTest, setDirection_oneBox) 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 + config.add_vertical_angles(0.0); // Only one vertical angle for a horizontal ring + config.set_horizontal_resolution(utils::degToRad(1.0)); // Set horizontal resolution to 1 degree raycaster_->setDirection(config); @@ -103,13 +103,7 @@ TEST_F(RaycasterTest, setDirection_manyBoxes) 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.0)) - .orientation( - geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0)); + utils::makePose(radius * cos(angle), radius * sin(angle), 0.0, 0.0, 0.0, 0.0, 1.0); const std::string name = "box" + std::to_string(i); raycaster_->addPrimitive(name, box_depth_, box_width_, box_height_, box_pose); @@ -117,7 +111,7 @@ TEST_F(RaycasterTest, setDirection_manyBoxes) 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.0)); + config.set_horizontal_resolution(utils::degToRad(5.0)); raycaster_->setDirection(config); diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.hpp index d3e9beb511d..641e59cf104 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.hpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.hpp @@ -23,6 +23,8 @@ #include #include +#include "../../utils/helper_functions.hpp" + using namespace simple_sensor_simulator; using namespace geometry_msgs::msg; @@ -31,20 +33,13 @@ constexpr static double degToRad(double deg) { return deg * M_PI / 180.0; } class RaycasterTest : public ::testing::Test { protected: - RaycasterTest() : raycaster_(std::make_unique()) + RaycasterTest() + : raycaster_(std::make_unique()), + config_(utils::constructLidarConfiguration("ego", "awf/universe", 0.0, 0.1)), + origin_(utils::makePose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)), + box_pose_(utils::makePose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)) { - configureLidar(); - - origin_ = - geometry_msgs::build() - .position(geometry_msgs::build().x(0.0).y(0.0).z(0.0)) - .orientation( - geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0)); - box_pose_ = - geometry_msgs::build() - .position(geometry_msgs::build().x(5.0).y(0.0).z(0.0)) - .orientation( - geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0)); + raycaster_->setDirection(config_); } std::unique_ptr raycaster_; @@ -60,19 +55,6 @@ class RaycasterTest : public ::testing::Test 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/sensor_simulation/primitives/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt index fb424659262..b5ae088e7a7 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt @@ -3,3 +3,6 @@ target_link_libraries(test_box simple_sensor_simulator_component) 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 ${Protobuf_LIBRARIES}) 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 c5b501075d7..25e64cf1022 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 @@ -19,7 +19,7 @@ #include #include -#include "../expect_eq_macros.hpp" +#include "../../utils/expect_eq_macros.hpp" using namespace simple_sensor_simulator; using namespace simple_sensor_simulator::primitives; 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..6438a0b2bb7 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.cpp @@ -0,0 +1,253 @@ +// 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_primitive.hpp" + +#include +#include +#include + +#include "../../utils/expect_eq_macros.hpp" + +/** + * @note Test basic functionality. Test adding to scene correctness with a sample primitive. + */ +TEST_F(PrimitiveTest, addToScene_sample) +{ + const std::vector expected_vertices = { + {0.0f, 1.0f, -1.0f}, {0.0f, 1.0f, 1.0f}, {0.0f, 3.0f, -1.0f}, {0.0f, 3.0f, 1.0f}, + {2.0f, 1.0f, -1.0f}, {2.0f, 1.0f, 1.0f}, {2.0f, 3.0f, -1.0f}, {2.0f, 3.0f, 1.0f}}; + const auto expected_triangles = primitive_->getTriangles(); + + RTCDevice device = rtcNewDevice(nullptr); + RTCScene scene = rtcNewScene(device); + + const unsigned int geom_id = primitive_->addToScene(device, scene); + ASSERT_NE(geom_id, RTC_INVALID_GEOMETRY_ID); + + const RTCGeometry geom = rtcGetGeometry(scene, geom_id); + ASSERT_NE(geom, nullptr); + + rtcCommitScene(scene); + + // Check vertices + const Vertex * vertex_buffer = + static_cast(rtcGetGeometryBufferData(geom, RTC_BUFFER_TYPE_VERTEX, 0)); + + for (size_t i = 0; i < expected_vertices.size(); ++i) { + EXPECT_VERTEX_EQ(vertex_buffer[i], expected_vertices[i]) + } + + // Check triangles + const Triangle * triangle_buffer = + static_cast(rtcGetGeometryBufferData(geom, RTC_BUFFER_TYPE_INDEX, 0)); + + for (size_t i = 0; i < expected_vertices.size(); ++i) { + EXPECT_TRIANGLE_EQ(triangle_buffer[i], expected_triangles[i]); + } + + rtcReleaseScene(scene); + rtcReleaseDevice(device); +} + +/** + * @note Test function behavior with vertices set to only zeros. + */ +TEST_F(PrimitiveTest, addToScene_zeros) +{ + const std::vector expected_triangles = {{0, 1, 2}}; + const std::vector expected_vertices = {{0.0f, 0.0f, 0.0f}}; + + primitive_ = + std::make_unique(0.0f, 0.0f, 0.0f, utils::makePose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)); + + RTCDevice device = rtcNewDevice(nullptr); + RTCScene scene = rtcNewScene(device); + + const unsigned int geom_id = primitive_->addToScene(device, scene); + ASSERT_NE(geom_id, RTC_INVALID_GEOMETRY_ID); + + const RTCGeometry geom = rtcGetGeometry(scene, geom_id); + ASSERT_NE(geom, nullptr); + + rtcCommitScene(scene); + + // Check vertices + const Vertex * vertex_buffer = + static_cast(rtcGetGeometryBufferData(geom, RTC_BUFFER_TYPE_VERTEX, 0)); + + for (size_t i = 0; i < expected_vertices.size(); ++i) { + EXPECT_VERTEX_EQ(vertex_buffer[i], expected_vertices[i]) + } + + // Check triangles + const Triangle * triangle_buffer = + static_cast(rtcGetGeometryBufferData(geom, RTC_BUFFER_TYPE_INDEX, 0)); + + for (size_t i = 0; i < expected_triangles.size(); ++i) { + EXPECT_TRIANGLE_EQ(triangle_buffer[i], expected_triangles[i]); + } + + rtcReleaseScene(scene); + rtcReleaseDevice(device); +} + +/** + * @note Test basic functionality. Test obtaining triangles correctness. + */ +TEST_F(PrimitiveTest, getTriangles) +{ + 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 triangles = primitive_->getTriangles(); + + ASSERT_EQ(triangles.size(), expected_triangles.size()); + for (size_t i = 0; i < triangles.size(); ++i) { + EXPECT_TRIANGLE_EQ(triangles[i], expected_triangles[i]); + } +} + +/** + * @note Test basic functionality. Test obtaining vertexes correctness. + */ +TEST_F(PrimitiveTest, getVertex) +{ + const std::vector expected_vertices = { + {0.0f, 1.0f, -1.0f}, {0.0f, 1.0f, 1.0f}, {0.0f, 3.0f, -1.0f}, {0.0f, 3.0f, 1.0f}, + {2.0f, 1.0f, -1.0f}, {2.0f, 1.0f, 1.0f}, {2.0f, 3.0f, -1.0f}, {2.0f, 3.0f, 1.0f}}; + + const auto vertices = primitive_->getVertex(); + + ASSERT_EQ(vertices.size(), expected_vertices.size()); + for (size_t i = 0; i < vertices.size(); ++i) { + EXPECT_VERTEX_EQ(vertices[i], expected_vertices[i]); + } +} + +/** + * @note Test basic functionality. Test conversion to a convex hull of some concave primitive. + */ +TEST_F(PrimitiveTest, get2DConvexHull_normal) +{ + const std::vector expected_hull = { + utils::makePoint(0.0, 1.0, 0.0), utils::makePoint(0.0, 3.0, 0.0), + utils::makePoint(2.0, 3.0, 0.0), utils::makePoint(2.0, 1.0, 0.0), + utils::makePoint(0.0, 1.0, 0.0)}; + + const auto hull = primitive_->get2DConvexHull(); + + EXPECT_GT(hull.size(), 0); + ASSERT_EQ(hull.size(), expected_hull.size()); + for (size_t i = 0; i < hull.size(); ++i) { + EXPECT_POINT_NEAR(hull[i], expected_hull[i], std::numeric_limits::epsilon()) + } +} + +/** + * @note Test basic functionality. Test conversion to a convex hull of some concave primitive with + * an additional sensor pose transformation - the goal is to test the transformation of convex hull. + */ +TEST_F(PrimitiveTest, get2DConvexHull_withTransform) +{ + const std::vector expected_hull = { + utils::makePoint(-1.0, 0.0, 0.0), utils::makePoint(-1.0, 2.0, 0.0), + utils::makePoint(1.0, 2.0, 0.0), utils::makePoint(1.0, 0.0, 0.0), + utils::makePoint(-1.0, 0.0, 0.0)}; + + const auto sensor_pose = utils::makePose(1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0); + + const auto hull = primitive_->get2DConvexHull(sensor_pose); + + EXPECT_GT(hull.size(), 0); + ASSERT_EQ(hull.size(), expected_hull.size()); + for (size_t i = 0; i < hull.size(); ++i) { + EXPECT_POINT_NEAR(hull[i], expected_hull[i], std::numeric_limits::epsilon()) + } +} + +/** + * @note Test basic functionality. Test min value obtaining in a given axis with a sample primitive. + */ +TEST_F(PrimitiveTest, getMin) +{ + const auto min_x = primitive_->getMin(math::geometry::Axis::X); + + ASSERT_TRUE(min_x.has_value()); + EXPECT_NEAR(min_x.value(), 0.0f, std::numeric_limits::epsilon()); +} + +/** + * @note Test basic functionality. Test min value obtaining in a given axis with a transformation + * with a sample primitive and non trivial sensor pose. + */ +TEST_F(PrimitiveTest, getMin_withTransform) +{ + const auto sensor_pose = utils::makePose(5.0, 2.0, 1.0, 0.0, 0.0, 0.0, 1.0); + + const auto min_x = primitive_->getMin(math::geometry::Axis::X, sensor_pose); + + ASSERT_TRUE(min_x.has_value()); + EXPECT_NEAR(min_x.value(), -5.0f, std::numeric_limits::epsilon()); +} + +/** + * @note Test function behavior when vertices is an empty vector - the goal is to get a nullopt. + */ +TEST_F(PrimitiveTest, getMin_empty) +{ + primitive_ = std::make_unique("Unknown", pose_); + + const auto min_x = primitive_->getMin(math::geometry::Axis::X); + + EXPECT_FALSE(min_x.has_value()); +} + +/** + * @note Test basic functionality. Test max value obtaining in a given axis with a sample primitive. + */ +TEST_F(PrimitiveTest, getMax) +{ + const auto max_x = primitive_->getMax(math::geometry::Axis::X); + + ASSERT_TRUE(max_x.has_value()); + EXPECT_NEAR(max_x.value(), 2.0f, std::numeric_limits::epsilon()); +} + +/** + * @note Test basic functionality. Test max value obtaining in a given axis with a transformation + * with a sample primitive and non trivial sensor pose. + */ +TEST_F(PrimitiveTest, getMax_withTransform) +{ + const auto sensor_pose = utils::makePose(6.0, 3.0, 2.0, 0.0, 0.0, 0.0, 1.0); + + const auto max_x = primitive_->getMax(math::geometry::Axis::X, sensor_pose); + + ASSERT_TRUE(max_x.has_value()); + EXPECT_NEAR(max_x.value(), -4.0f, std::numeric_limits::epsilon()); +} + +/** + * @note Test function behavior when vertices is an empty vector - the goal is to get a nullopt. + */ +TEST_F(PrimitiveTest, getMax_empty) +{ + primitive_ = std::make_unique("Unknown", pose_); + + const auto max_x = primitive_->getMax(math::geometry::Axis::X); + + EXPECT_FALSE(max_x.has_value()); +} diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.hpp new file mode 100644 index 00000000000..2343104aa81 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.hpp @@ -0,0 +1,42 @@ +// 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_PRIMITIVE_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__TEST__TEST_PRIMITIVE_HPP_ + +#include + +#include +#include +#include + +#include "../../utils/helper_functions.hpp" + +using namespace simple_sensor_simulator::primitives; +using namespace simple_sensor_simulator; + +class PrimitiveTest : public ::testing::Test +{ +protected: + PrimitiveTest() + : pose_(utils::makePose(1.0, 2.0, 0.0, 0.0, 0.0, 0.0, 1.0)), + primitive_(std::make_unique(2.0f, 2.0f, 2.0f, pose_)) + { + } + + geometry_msgs::msg::Pose pose_; + std::unique_ptr primitive_; +}; + +#endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_PRIMITIVE_HPP_ 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 66da446cec3..c0fe3d57057 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 @@ -19,7 +19,7 @@ #include #include -#include "../expect_eq_macros.hpp" +#include "../../utils/expect_eq_macros.hpp" using namespace simple_sensor_simulator; diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/expect_eq_macros.hpp b/simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp similarity index 90% rename from simulation/simple_sensor_simulator/test/src/sensor_simulation/expect_eq_macros.hpp rename to simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp index c93387428cc..03346fdc438 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/expect_eq_macros.hpp +++ b/simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #define EXPECT_POINT_EQ(DATA0, DATA1) \ @@ -65,4 +66,9 @@ EXPECT_FLOAT_EQ(VERTEX.y, POINT.y); \ EXPECT_FLOAT_EQ(VERTEX.z, POINT.z); +#define EXPECT_POSITION_NEAR(POSITION0, POSITION1, TOLERANCE) \ + EXPECT_NEAR(POSITION0.x, POSITION1.x, TOLERANCE); \ + EXPECT_NEAR(POSITION0.y, POSITION1.y, TOLERANCE); \ + EXPECT_NEAR(POSITION0.z, POSITION1.z, TOLERANCE); + #endif // SIMPLE_SENSOR_SIMULATOR__TEST__UTILS__EXPECT_EQ_MACROS_HPP_ diff --git a/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp b/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp new file mode 100644 index 00000000000..39adaeee487 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp @@ -0,0 +1,142 @@ +// 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__HELPER_FUNCTIONS_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__TEST__UTILS__HELPER_FUNCTIONS_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using EntitySubtype = traffic_simulator_msgs::EntitySubtype; +using EntityType = traffic_simulator_msgs::EntityType; +using EntityStatus = traffic_simulator_msgs::EntityStatus; + +namespace utils +{ + +constexpr auto degToRad(const double deg) -> double { return deg * M_PI / 180.0; } + +inline auto makePoint(const double px, const double py, const double pz) + -> geometry_msgs::msg::Point +{ + return geometry_msgs::build().x(px).y(py).z(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 +{ + return geometry_msgs::build() + .position(geometry_msgs::build().x(px).y(py).z(pz)) + .orientation(geometry_msgs::build().x(ox).y(oy).z(oz).w(ow)); +} + +inline auto makeDimensions(const double x, const double y, const double z) + -> geometry_msgs::msg::Vector3 +{ + return geometry_msgs::build().x(x).y(y).z(z); +} + +inline auto constructLidarConfiguration( + const std::string & entity, const std::string & architecture_type, + const double lidar_sensor_delay, const double horizontal_resolution) + -> const simulation_api_schema::LidarConfiguration +{ + simulation_api_schema::LidarConfiguration configuration; + configuration.set_horizontal_resolution(horizontal_resolution); + configuration.set_architecture_type(architecture_type); + configuration.set_entity(entity); + configuration.set_lidar_sensor_delay(lidar_sensor_delay); + + configuration.set_scan_duration(0.1); + configuration.add_vertical_angles(degToRad(-15.0)); + configuration.add_vertical_angles(degToRad(-13.0)); + configuration.add_vertical_angles(degToRad(-11.0)); + configuration.add_vertical_angles(degToRad(-9.0)); + configuration.add_vertical_angles(degToRad(-7.0)); + configuration.add_vertical_angles(degToRad(-5.0)); + configuration.add_vertical_angles(degToRad(-3.0)); + configuration.add_vertical_angles(degToRad(-1.0)); + configuration.add_vertical_angles(degToRad(1.0)); + configuration.add_vertical_angles(degToRad(3.0)); + configuration.add_vertical_angles(degToRad(5.0)); + configuration.add_vertical_angles(degToRad(7.0)); + configuration.add_vertical_angles(degToRad(9.0)); + configuration.add_vertical_angles(degToRad(11.0)); + configuration.add_vertical_angles(degToRad(13.0)); + configuration.add_vertical_angles(degToRad(15.0)); + return configuration; +} + +inline auto createEntityStatus( + const std::string & name, const EntityType::Enum type, + const std::optional & subtype, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Vector3 & dimensions) -> EntityStatus +{ + EntityStatus status; + status.set_name(name); + status.mutable_type()->set_type(type); + + if (subtype) { + status.mutable_subtype()->set_value(*subtype); + } + + auto new_pose = status.mutable_pose(); + auto new_position = new_pose->mutable_position(); + new_position->set_x(pose.position.x); + new_position->set_y(pose.position.y); + new_position->set_z(pose.position.z); + + auto new_orientation = new_pose->mutable_orientation(); + new_orientation->set_x(pose.orientation.x); + new_orientation->set_y(pose.orientation.y); + new_orientation->set_z(pose.orientation.z); + new_orientation->set_w(pose.orientation.w); + + auto new_bounding_box = status.mutable_bounding_box(); + auto new_dimensions = new_bounding_box->mutable_dimensions(); + new_dimensions->set_x(dimensions.x); + new_dimensions->set_y(dimensions.y); + new_dimensions->set_z(dimensions.z); + + return status; +} + +inline auto makeEntity( + const std::string & name, const EntityType::Enum type, const EntitySubtype::Enum subtype, + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & dimensions) + -> EntityStatus +{ + return createEntityStatus(name, type, subtype, pose, dimensions); +} + +inline auto makeEntity( + const std::string & name, const EntityType::Enum type, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Vector3 & dimensions) -> EntityStatus +{ + return createEntityStatus(name, type, std::nullopt, pose, dimensions); +} + +} // namespace utils + +#endif // SIMPLE_SENSOR_SIMULATOR__TEST__UTILS__HELPER_FUNCTIONS_HPP_ diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 8627664b6d1..b1f379c1363 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,120 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(conversions, behavior_plugin_base): add new line at the end +* ref(traffic_simulator, behavior_tree_plugin): revert unnecessary changes +* feat(traffic_simulator): use CanonicalizedEntityStatus only with single constructor +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index adc700b1520..0ef989cf9e4 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 3.5.5 + 4.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 779539af957..4dff0674c5a 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,211 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ +* Merge pull request `#1367 `_ from tier4/RJD-1197/canonicalized_lanelet_pose + Rjd 1197/canonicalized lanelet pose +* Spelling changes +* Lint changes +* Change assert check from bool to has_value +* Change invalid test cases to more obvious values +* Unit tests review changes +* CanonicalizedLaneletPose unit tests +* Contributors: Grzegorz Maj, Masaya Kataoka + +4.2.0 (2024-09-09) +------------------ +* Merge pull request `#1362 `_ from tier4/feature/ros2-parameter-forwarding +* Cleanup +* Move parameter `use_sim_time` into function `make_parameters` +* Remove data member `traffic_simulator::Configuration::rviz_config_path` +* Add feature to forward parameters prefixed with `autoware.` to Autoware +* Contributors: Kotaro Yoshimoto, yamacir-kit + +4.1.1 (2024-09-03) +------------------ +* Merge pull request `#1207 `_ from tier4/fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* feat(use_sim_time): set default as false +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* use_sim_time used in concealer initialization +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto, Paweł Lech + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge pull request `#1358 `_ from tier4/RJD-1056-remove-npc-logic-started + Remove unused data members: npc_logic_started +* feat(traffic_simulator): apply clang reforamt +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* fix(traffic_simulator): remove unnecessary misc_object tests +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* fix(ego_entity): fix autoware update when not npc_logic_started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Corrections for adapting removed npc logic started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Correct EntityManager::getCurrentAction +* Add else +* Trailing return type +* Use member instead of getter +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Forward isNpcLogicStarted to API and restore TestExecutor +* Add else +* Move implementation to cpp file +* Remove npc_logic_started from API +* Update NPC logic only when it has been started +* Correct style +* Restore previous getCurrentAction behavior +* Remove npc_logic_started from Entities +* Contributors: DMoszynski, Dawid Moszynski, Masaya Kataoka, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge pull request `#1356 `_ from tier4/RJD-1056-remove-current-time-step-time + Remove unused data members: current_time step_time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Fix after using pointer to store entity status +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Resolve conflicts with stored time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Trailing return type +* Correct style - add else +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Add const to time argument in behavior +* Add const to time argument +* Use member instead of getter +* Revert to previous macro definition +* Move requestSpeedChange time check responsibility to EgoEntity and simplify +* Correct style +* Remove step_time\_ and current_time\_ data members from EntityManager + Adjust the code so that the time is managed in API only +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge pull request `#1354 `_ from tier4/fix/follow_trajectory + Fix follow trajectory action and timestamp in entity status +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* fix timestamp +* fix timestamp in status_with_trajectory +* Contributors: Masaya Kataoka, Tatsuya Yamasaki + +4.0.0 (2024-08-27) +------------------ +* Merge pull request `#1320 `_ from tier4/ref/RJD-1053-set-update-canonicalized-entity-status + ref(behavior_tree, traffic_simulator): move responsibility for canonicalization to traffic_simulator, simplify +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* doc(distance): use doxygen format + Co-authored-by: Masaya Kataoka +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Fix EgoEntity bug where status time was incremented only when Ego was controlled by simulator + This bug lead to problems in other checks which relied on the correct status time +* Fix EgoEntity error where map pose was unable to be set after scenario start, which should be possible for Ego +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(behavior_tree): use CanonicalizedEntityStatus as shared_ptr inside BT and use ::set +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(ego_entity): fix setMapPose +* ref(traffic_simulator): remove souvenir +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(spell): fix string in api +* feat(traffic_simulator, entity_base): improve setStatus - add passing lanelet_id - use it +* ref(traffic_simulator): use getCanonicalizedStatus, remove getStatus +* ref(traffic_simulator): remove virutla getEntityType, tidy up CanonicalizedEntityStatus getters +* feat(cpp_mock_scenarios): add isPedestrain and isVehicle - use it +* ref(utils, pose): global use pose:: namespace +* fix(traffic_simulator): apply clang reformat +* ref(traffic_simulator): improve CanonicalizedEntityStatus getters - use const ref +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(entity_base): fix retuned type def, fix typo +* doc(developer_guide, traffic_simulator): update doc after review changes, add code notes +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(traffic_simulator): fix helper_functions for tests and misc object tests +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(follow_trajectory_action): fix after merge FTA changes +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* doc(developer_guide, pose utils): adopt lane_pose_calculation doc to canonicalization laneletpose in CanonicalizedEntityStatus +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(traffic_simulator): improve assertions in CanonicalizedEntityStatus::set +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(conversions, behavior_plugin_base): add new line at the end +* ref(traffic_simulator): move toCanonicalizeLaneletPose to CanonicalizedEntityStatus::set, little tidy up +* fix(traffic_simulator): fix behavior_plugin_base +* ref(traffic_simulator, behavior_tree_plugin): revert unnecessary changes +* ref(traffic_simulator, behavior_tree_plugin): revert unnecessary changes +* ref(traffic_simulator): remove operator= for CanonicalizedEntityStatus, use set and assertions +* fix(traffic_simulator, behavior_tree_plugin): fix returned EntityStatus from bt, fix canonicalize in vehicle_entity and pedestrian_entity +* feat(behavior_tree_plugin, entity_base): move toCanonicalizedEntityPose to traffic_simulator, use EntityStatus as updated_state in BehaviorTree +* feat(traffic_simulator): use CanonicalizedEntityStatus only with single constructor +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* tmp +* ref(traffic_simulator): remove unnecessary cast to EnrityStatus in EntityManager and MicObjectEntity +* ref(traffic_simulator): remove unecessary casts to EntityStatus in EntityBase +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge pull request `#1348 `_ from tier4/fix/distance-with-lane-change diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index fb7133cac65..28014db05be 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -146,10 +146,10 @@ class API if (behavior == VehicleBehavior::autoware()) { return entity_manager_ptr_->entityExists(name) or entity_manager_ptr_->spawnEntity( - name, pose, parameters, configuration, node_parameters_); + name, pose, parameters, getCurrentTime(), configuration, node_parameters_); } else { return entity_manager_ptr_->spawnEntity( - name, pose, parameters, behavior); + name, pose, parameters, getCurrentTime(), behavior); } }; @@ -184,7 +184,7 @@ class API { auto register_to_entity_manager = [&]() { return entity_manager_ptr_->spawnEntity( - name, pose, parameters, behavior); + name, pose, parameters, getCurrentTime(), behavior); }; auto register_to_environment_simulator = [&]() { @@ -213,7 +213,8 @@ class API const std::string & model3d = "") { auto register_to_entity_manager = [&]() { - return entity_manager_ptr_->spawnEntity(name, pose, parameters); + return entity_manager_ptr_->spawnEntity( + name, pose, parameters, getCurrentTime()); }; auto register_to_environment_simulator = [&]() { @@ -250,8 +251,7 @@ class API const std::string & name, const LaneletPose & lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void; auto setEntityStatus( - const std::string & name, - const std::optional & canonicalized_lanelet_pose, + const std::string & name, const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status = helper::constructActionStatus()) -> void; auto setEntityStatus( @@ -335,6 +335,8 @@ class API const bool allow_spawn_outside_lane = false, const bool require_footprint_fitting = false, const bool random_orientation = false, std::optional random_seed = std::nullopt) -> void; + auto getEntity(const std::string & name) const -> std::shared_ptr; + // clang-format off #define FORWARD_TO_ENTITY_MANAGER(NAME) \ /*! \ @@ -364,10 +366,9 @@ class API FORWARD_TO_ENTITY_MANAGER(getCurrentAction); FORWARD_TO_ENTITY_MANAGER(getCurrentTwist); FORWARD_TO_ENTITY_MANAGER(getEgoName); - FORWARD_TO_ENTITY_MANAGER(getEntity); FORWARD_TO_ENTITY_MANAGER(getEntityNames); FORWARD_TO_ENTITY_MANAGER(getEntityStatus); - FORWARD_TO_ENTITY_MANAGER(getEntityStatusBeforeUpdate); + FORWARD_TO_ENTITY_MANAGER(getCanonicalizedStatusBeforeUpdate); FORWARD_TO_ENTITY_MANAGER(getHdmapUtils); FORWARD_TO_ENTITY_MANAGER(getLinearJerk); FORWARD_TO_ENTITY_MANAGER(getStandStillDuration); diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index 3e17d2f5873..155fab255b9 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -16,7 +16,6 @@ #define TRAFFIC_SIMULATOR__API__CONFIGURATION_HPP_ #include -#include #include #include #include @@ -65,10 +64,6 @@ struct Configuration Pathname scenario_path = ""; - Pathname rviz_config_path = // - ament_index_cpp::get_package_share_directory("traffic_simulator") + - "/config/scenario_simulator_v2.rviz"; - explicit Configuration(const Pathname & map_path) // : map_path(assertMapPath(map_path)), lanelet2_map_file(findLexicographicallyFirstFilenameOf(map_path, ".osm")), diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index 1b09d5b3c3f..0d4847f7d5c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -41,7 +41,7 @@ class BehaviorPluginBase public: virtual ~BehaviorPluginBase() = default; virtual void configure(const rclcpp::Logger & logger) = 0; - virtual void update(double current_time, double step_time) = 0; + virtual auto update(const double current_time, const double step_time) -> void = 0; virtual const std::string & getCurrentAction() const = 0; #define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ @@ -55,10 +55,10 @@ class BehaviorPluginBase // clang-format off DEFINE_GETTER_SETTER(BehaviorParameter, "behavior_parameter", traffic_simulator_msgs::msg::BehaviorParameter) + DEFINE_GETTER_SETTER(CanonicalizedEntityStatus, "canonicalized_entity_status", std::shared_ptr) DEFINE_GETTER_SETTER(CurrentTime, "current_time", double) DEFINE_GETTER_SETTER(DebugMarker, "debug_marker", std::vector) DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, "matching_distance_for_lanelet_pose_calculation", double) - DEFINE_GETTER_SETTER(EntityStatus, "entity_status", std::shared_ptr) DEFINE_GETTER_SETTER(GoalPoses, "goal_poses", std::vector) DEFINE_GETTER_SETTER(HdMapUtils, "hdmap_utils", std::shared_ptr) DEFINE_GETTER_SETTER(LaneChangeParameters, "lane_change_parameters", traffic_simulator::lane_change::Parameter) @@ -72,7 +72,6 @@ class BehaviorPluginBase DEFINE_GETTER_SETTER(StepTime, "step_time", double) DEFINE_GETTER_SETTER(TargetSpeed, "target_speed", std::optional) DEFINE_GETTER_SETTER(TrafficLightManager, "traffic_light_manager", std::shared_ptr) - DEFINE_GETTER_SETTER(UpdatedStatus, "updated_status", std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, "vehicle_parameters", traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, "waypoints", traffic_simulator_msgs::msg::WaypointsArray) // clang-format on diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp index 0665ccd8f3a..da3293fc67e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp @@ -16,6 +16,7 @@ #define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY_HPP_ #include +#include #include #include #include @@ -30,8 +31,7 @@ auto makeUpdatedStatus( traffic_simulator_msgs::msg::PolylineTrajectory &, const traffic_simulator_msgs::msg::BehaviorParameter &, const std::shared_ptr &, double, double, - std::optional target_speed = std::nullopt) - -> std::optional; + std::optional target_speed = std::nullopt) -> std::optional; } // namespace follow_trajectory } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp index 79d3d516699..95df82b7990 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp @@ -26,7 +26,7 @@ namespace longitudinal_speed_planning class LongitudinalSpeedPlanner { public: - explicit LongitudinalSpeedPlanner(double step_time, const std::string & entity); + explicit LongitudinalSpeedPlanner(const double step_time, const std::string & entity); auto getDynamicStates( double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &, const geometry_msgs::msg::Twist & current_twist, diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp index 53897ac0add..68a781808a3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp @@ -17,60 +17,75 @@ #include #include +#include #include namespace traffic_simulator { using EntityStatus = traffic_simulator_msgs::msg::EntityStatus; +using EntityType = traffic_simulator_msgs::msg::EntityType; +using EntitySubtype = traffic_simulator_msgs::msg::EntitySubtype; inline namespace entity_status { class CanonicalizedEntityStatus { public: - explicit CanonicalizedEntityStatus( - const EntityStatus & may_non_canonicalized_entity_status, - const std::shared_ptr & hdmap_utils); explicit CanonicalizedEntityStatus( const EntityStatus & may_non_canonicalized_entity_status, const std::optional & canonicalized_lanelet_pose); - explicit CanonicalizedEntityStatus( - const EntityStatus & may_non_canonicalized_entity_status, - const std::shared_ptr & hdmap_utils, - const lanelet::Ids & route_lanelets); explicit CanonicalizedEntityStatus(const CanonicalizedEntityStatus & obj); explicit operator EntityStatus() const noexcept { return entity_status_; } - auto operator=(const CanonicalizedEntityStatus & obj) -> CanonicalizedEntityStatus &; + + auto set(const CanonicalizedEntityStatus & status) -> void; + auto set( + const EntityStatus & status, const lanelet::Ids & lanelet_ids, const double matching_distance, + const std::shared_ptr & hdmap_utils_ptr) -> void; + auto set( + const EntityStatus & status, const double matching_distance, + const std::shared_ptr & hdmap_utils_ptr) -> void; + auto setAction(const std::string & action) -> void; - auto getName() const noexcept -> const std::string & { return entity_status_.name; }; - auto getBoundingBox() const noexcept -> traffic_simulator_msgs::msg::BoundingBox; - auto laneMatchingSucceed() const noexcept -> bool; - auto getMapPose() const noexcept -> geometry_msgs::msg::Pose; - auto getLaneletPose() const -> LaneletPose; - auto getLaneletId() const -> lanelet::Id; - auto getLaneletIds() const -> lanelet::Ids; - auto getCanonicalizedLaneletPose() const -> std::optional; + auto getActionStatus() const noexcept -> const traffic_simulator_msgs::msg::ActionStatus &; + + auto getTime() const noexcept -> double; + auto setTime(double) -> void; + + auto getMapPose() const noexcept -> const geometry_msgs::msg::Pose &; + auto setMapPose(const geometry_msgs::msg::Pose & pose) -> void; + + auto getTwist() const noexcept -> const geometry_msgs::msg::Twist &; auto setTwist(const geometry_msgs::msg::Twist & twist) -> void; - auto getTwist() const noexcept -> geometry_msgs::msg::Twist; auto setLinearVelocity(double linear_velocity) -> void; + + auto getAccel() const noexcept -> const geometry_msgs::msg::Accel &; auto setAccel(const geometry_msgs::msg::Accel & accel) -> void; auto setLinearAcceleration(double linear_acceleration) -> void; - auto getAccel() const noexcept -> geometry_msgs::msg::Accel; - auto setLinearJerk(double) -> void; + auto getLinearJerk() const noexcept -> double; - auto setTime(double) -> void; - auto getTime() const noexcept -> double; + auto setLinearJerk(double) -> void; + + auto laneMatchingSucceed() const noexcept -> bool; + auto getLaneletId() const noexcept -> lanelet::Id; + auto getLaneletIds() const noexcept -> lanelet::Ids; + auto getLaneletPose() const noexcept -> const LaneletPose &; + auto getCanonicalizedLaneletPose() const noexcept + -> const std::optional &; + auto getName() const noexcept -> const std::string & { return entity_status_.name; } + auto getType() const noexcept -> const EntityType & { return entity_status_.type; } + auto getSubtype() const noexcept -> const EntitySubtype & { return entity_status_.subtype; } + auto getBoundingBox() const noexcept -> const traffic_simulator_msgs::msg::BoundingBox &; private: - auto canonicalize() -> void; std::optional canonicalized_lanelet_pose_; EntityStatus entity_status_; }; } // namespace entity_status - -auto isSameLaneletId(const CanonicalizedEntityStatus &, const CanonicalizedEntityStatus &) -> bool; -auto isSameLaneletId(const CanonicalizedEntityStatus &, const lanelet::Id lanelet_id) -> bool; - +auto isSameLaneletId( + const CanonicalizedEntityStatus & first_status, const CanonicalizedEntityStatus & second_status) + -> bool; +auto isSameLaneletId(const CanonicalizedEntityStatus & status, const lanelet::Id lanelet_id) + -> bool; } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__DATA_TYPE__ENTITY_STATUS_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp index a22d43a7afd..a1d9d83b7bf 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp @@ -37,6 +37,7 @@ class CanonicalizedLaneletPose CanonicalizedLaneletPose & operator=(const CanonicalizedLaneletPose & obj); explicit operator LaneletPose() const noexcept { return lanelet_pose_; } explicit operator geometry_msgs::msg::Pose() const noexcept { return map_pose_; } + auto getLaneletPose() const -> const LaneletPose & { return lanelet_pose_; } auto hasAlternativeLaneletPose() const -> bool { return lanelet_poses_.size() > 1; } auto getAlternativeLaneletPoseBaseOnShortestRouteFrom( LaneletPose from, const std::shared_ptr & hdmap_utils, 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 8575ee3588e..dbe2668f758 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -69,7 +69,7 @@ class EgoEntity : public VehicleEntity auto getCurrentAction() const -> std::string override; - auto getCurrentPose() const -> geometry_msgs::msg::Pose; + auto getCurrentPose() const -> const geometry_msgs::msg::Pose &; auto getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints & override; @@ -78,8 +78,6 @@ class EgoEntity : public VehicleEntity auto getEntityStatus(const double, const double) const -> const CanonicalizedEntityStatus; - auto getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & override; - auto getEntityTypename() const -> const std::string & override; auto getObstacle() -> std::optional override; @@ -88,6 +86,8 @@ class EgoEntity : public VehicleEntity auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override; + auto updateFieldOperatorApplication() const -> void; + void onUpdate(double current_time, double step_time) override; void requestAcquirePosition(const CanonicalizedLaneletPose &) override; @@ -133,6 +133,18 @@ class EgoEntity : public VehicleEntity auto setVelocityLimit(double) -> void override; auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void override; + + template + auto setStatus(Ts &&... xs) + { + if (status_->getTime() > 0 && not isControlledBySimulator()) { + THROW_SEMANTIC_ERROR( + "You cannot set entity status to the ego vehicle named ", std::quoted(status_->getName()), + " after starting scenario."); + } else { + EntityBase::setStatus(std::forward(xs)...); + } + } }; } // namespace entity } // namespace traffic_simulator 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 ac53c791a75..c29eddf5722 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -71,17 +71,18 @@ class EntityBase */ \ /* */ auto get##NAME() const noexcept->TYPE { return RETURN_VARIABLE; } - DEFINE_GETTER(BoundingBox, traffic_simulator_msgs::msg::BoundingBox, static_cast(getStatus()).bounding_box) - DEFINE_GETTER(CurrentAccel, geometry_msgs::msg::Accel, static_cast(getStatus()).action_status.accel) - DEFINE_GETTER(CurrentTwist, geometry_msgs::msg::Twist, static_cast(getStatus()).action_status.twist) - DEFINE_GETTER(DynamicConstraints, traffic_simulator_msgs::msg::DynamicConstraints, getBehaviorParameter().dynamic_constraints) - DEFINE_GETTER(EntityStatusBeforeUpdate, const CanonicalizedEntityStatus &, status_before_update_) - DEFINE_GETTER(EntitySubtype, traffic_simulator_msgs::msg::EntitySubtype, static_cast(getStatus()).subtype) - DEFINE_GETTER(LinearJerk, double, static_cast(getStatus()).action_status.linear_jerk) - DEFINE_GETTER(MapPose, geometry_msgs::msg::Pose, static_cast(getStatus()).pose) - DEFINE_GETTER(StandStillDuration, double, stand_still_duration_) - DEFINE_GETTER(Status, const CanonicalizedEntityStatus &, status_) - DEFINE_GETTER(TraveledDistance, double, traveled_distance_) + DEFINE_GETTER(BoundingBox, const traffic_simulator_msgs::msg::BoundingBox &, status_->getBoundingBox()) + DEFINE_GETTER(CanonicalizedStatus, const CanonicalizedEntityStatus &, *status_) + DEFINE_GETTER(CanonicalizedStatusBeforeUpdate, const CanonicalizedEntityStatus &, status_before_update_) + DEFINE_GETTER(CurrentAccel, const geometry_msgs::msg::Accel &, status_->getAccel()) + DEFINE_GETTER(CurrentTwist, const geometry_msgs::msg::Twist &, status_->getTwist()) + DEFINE_GETTER(DynamicConstraints, traffic_simulator_msgs::msg::DynamicConstraints, getBehaviorParameter().dynamic_constraints) + DEFINE_GETTER(EntitySubtype, const traffic_simulator_msgs::msg::EntitySubtype &, status_->getSubtype()) + DEFINE_GETTER(EntityType, const traffic_simulator_msgs::msg::EntityType &, status_->getType()) + DEFINE_GETTER(LinearJerk, double, status_->getLinearJerk()) + DEFINE_GETTER(MapPose, const geometry_msgs::msg::Pose &, status_->getMapPose()) + DEFINE_GETTER(StandStillDuration, double, stand_still_duration_) + DEFINE_GETTER(TraveledDistance, double, traveled_distance_) // clang-format on #undef DEFINE_GETTER @@ -92,8 +93,7 @@ class EntityBase */ \ /* */ auto FUNCTION_NAME() const->bool { return BOOL_VARIABLE; } - DEFINE_CHECK_FUNCTION(isNpcLogicStarted, npc_logic_started_) - DEFINE_CHECK_FUNCTION(laneMatchingSucceed, status_.laneMatchingSucceed()) + DEFINE_CHECK_FUNCTION(laneMatchingSucceed, status_->laneMatchingSucceed()) // clang-format on #undef DEFINE_CHECK_FUNCTION @@ -106,8 +106,6 @@ class EntityBase virtual auto getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints & = 0; - virtual auto getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & = 0; - virtual auto getEntityTypename() const -> const std::string & = 0; virtual auto getGoalPoses() -> std::vector = 0; @@ -117,9 +115,6 @@ class EntityBase /* */ auto getCanonicalizedLaneletPose(double matching_distance) const -> std::optional; - /* */ auto getMapPoseFromRelativePose(const geometry_msgs::msg::Pose &) const - -> geometry_msgs::msg::Pose; - virtual auto getMaxAcceleration() const -> double = 0; virtual auto getMaxDeceleration() const -> double = 0; @@ -132,9 +127,9 @@ class EntityBase virtual auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray = 0; - virtual void onUpdate(double current_time, double step_time); + virtual auto onUpdate(const double current_time, const double step_time) -> void; - virtual void onPostUpdate(double current_time, double step_time); + virtual auto onPostUpdate(const double current_time, const double step_time) -> void; /* */ void resetDynamicConstraints(); @@ -194,7 +189,11 @@ class EntityBase /* */ void setOtherStatus(const std::unordered_map &); - virtual auto setStatus(const CanonicalizedEntityStatus &) -> void; + virtual auto setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void; + + virtual auto setStatus(const EntityStatus & status) -> void; + + /* */ auto setCanonicalizedStatus(const CanonicalizedEntityStatus &) -> void; virtual auto setLinearAcceleration(const double linear_acceleration) -> void; @@ -215,9 +214,9 @@ class EntityBase /* */ auto setAcceleration(const geometry_msgs::msg::Accel & accel) -> void; - /* */ auto setLinearJerk(const double liner_jerk) -> void; + /* */ auto setAction(const std::string & action) -> void; - virtual void startNpcLogic(const double current_time); + /* */ auto setLinearJerk(const double liner_jerk) -> void; /* */ void stopAtCurrentPosition(); @@ -245,14 +244,13 @@ class EntityBase bool verbose; protected: - CanonicalizedEntityStatus status_; + std::shared_ptr status_; CanonicalizedEntityStatus status_before_update_; std::shared_ptr hdmap_utils_ptr_; std::shared_ptr traffic_light_manager_; - bool npc_logic_started_ = false; double stand_still_duration_ = 0.0; double traveled_distance_ = 0.0; double prev_job_duration_ = 0.0; 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 b90788d5437..70ce1b78f40 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -85,10 +85,6 @@ class EntityManager std::unordered_map> entities_; - double step_time_; - - double current_time_; - bool npc_logic_started_; using EntityStatusWithTrajectoryArray = @@ -158,7 +154,6 @@ class EntityManager broadcaster_(node), base_link_broadcaster_(node), clock_ptr_(node->get_clock()), - current_time_(std::numeric_limits::quiet_NaN()), npc_logic_started_(false), entity_status_array_pub_ptr_(rclcpp::create_publisher( node, "entity/status", EntityMarkerQoS(), @@ -263,33 +258,32 @@ class EntityManager static_assert(true, "") // clang-format on + FORWARD_TO_ENTITY(activateOutOfRangeJob, ); FORWARD_TO_ENTITY(asFieldOperatorApplication, const); + FORWARD_TO_ENTITY(cancelRequest, ); FORWARD_TO_ENTITY(get2DPolygon, const); FORWARD_TO_ENTITY(getBehaviorParameter, const); FORWARD_TO_ENTITY(getBoundingBox, const); + FORWARD_TO_ENTITY(getCanonicalizedStatusBeforeUpdate, const); FORWARD_TO_ENTITY(getCurrentAccel, const); - FORWARD_TO_ENTITY(getCurrentAction, const); FORWARD_TO_ENTITY(getCurrentTwist, const); FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const); - FORWARD_TO_ENTITY(getEntityStatusBeforeUpdate, const); FORWARD_TO_ENTITY(getEntityType, const); - FORWARD_TO_ENTITY(reachPosition, const); FORWARD_TO_ENTITY(getEntityTypename, const); FORWARD_TO_ENTITY(getLinearJerk, const); FORWARD_TO_ENTITY(getRouteLanelets, const); FORWARD_TO_ENTITY(getStandStillDuration, const); FORWARD_TO_ENTITY(getTraveledDistance, const); - FORWARD_TO_ENTITY(laneMatchingSucceed, const); - FORWARD_TO_ENTITY(activateOutOfRangeJob, ); - FORWARD_TO_ENTITY(cancelRequest, ); FORWARD_TO_ENTITY(isControlledBySimulator, ); + FORWARD_TO_ENTITY(laneMatchingSucceed, const); + FORWARD_TO_ENTITY(reachPosition, const); FORWARD_TO_ENTITY(requestAcquirePosition, ); FORWARD_TO_ENTITY(requestAssignRoute, ); + FORWARD_TO_ENTITY(requestClearRoute, ); FORWARD_TO_ENTITY(requestFollowTrajectory, ); FORWARD_TO_ENTITY(requestLaneChange, ); FORWARD_TO_ENTITY(requestSynchronize, ); FORWARD_TO_ENTITY(requestWalkStraight, ); - FORWARD_TO_ENTITY(requestClearRoute, ); FORWARD_TO_ENTITY(setAcceleration, ); FORWARD_TO_ENTITY(setAccelerationLimit, ); FORWARD_TO_ENTITY(setAccelerationRateLimit, ); @@ -302,29 +296,18 @@ class EntityManager FORWARD_TO_ENTITY(setMapPose, ); FORWARD_TO_ENTITY(setTwist, ); FORWARD_TO_ENTITY(setVelocityLimit, ); + FORWARD_TO_ENTITY(requestSpeedChange, ); #undef FORWARD_TO_ENTITY + auto getCurrentAction(const std::string & name) const -> std::string; + visualization_msgs::msg::MarkerArray makeDebugMarker() const; bool trafficLightsChanged(); - void requestSpeedChange(const std::string & name, double target_speed, bool continuous); - - void requestSpeedChange( - const std::string & name, const double target_speed, const speed_change::Transition transition, - const speed_change::Constraint constraint, const bool continuous); - - void requestSpeedChange( - const std::string & name, const speed_change::RelativeTargetSpeed & target_speed, - bool continuous); - - void requestSpeedChange( - const std::string & name, const speed_change::RelativeTargetSpeed & target_speed, - const speed_change::Transition transition, const speed_change::Constraint constraint, - const bool continuous); - - auto updateNpcLogic(const std::string & name) -> const CanonicalizedEntityStatus &; + auto updateNpcLogic(const std::string & name, const double current_time, const double step_time) + -> const CanonicalizedEntityStatus &; void broadcastEntityTransform(); @@ -338,14 +321,12 @@ class EntityManager bool entityExists(const std::string & name); - auto getCurrentTime() const noexcept -> double; - auto getEntityNames() const -> const std::vector; auto getEntity(const std::string & name) const -> std::shared_ptr; - auto getEntityStatus(const std::string & name) const -> CanonicalizedEntityStatus; + auto getEntityStatus(const std::string & name) const -> const CanonicalizedEntityStatus &; auto getHdmapUtils() -> const std::shared_ptr &; @@ -357,8 +338,6 @@ class EntityManager auto getPedestrianParameters(const std::string & name) const -> const traffic_simulator_msgs::msg::PedestrianParameters &; - auto getStepTime() const noexcept -> double; - auto getVehicleParameters(const std::string & name) const -> const traffic_simulator_msgs::msg::VehicleParameters &; @@ -379,7 +358,7 @@ class EntityManager } else { std::vector poses; for (const auto & lanelet_pose : getGoalPoses(name)) { - poses.push_back(toMapPose(lanelet_pose)); + poses.push_back(pose::toMapPose(lanelet_pose)); } return poses; } @@ -413,13 +392,12 @@ class EntityManager */ void resetBehaviorPlugin(const std::string & name, const std::string & behavior_plugin_name); - auto setEntityStatus(const std::string & name, const CanonicalizedEntityStatus & status) -> void; - void setVerbose(const bool verbose); template auto spawnEntity( - const std::string & name, const Pose & pose, const Parameters & parameters, Ts &&... xs) + const std::string & name, const Pose & pose, const Parameters & parameters, + const double current_time, Ts &&... xs) { static_assert( std::disjunction< @@ -448,7 +426,7 @@ class EntityManager } entity_status.subtype = parameters.subtype; - entity_status.time = getCurrentTime(); + entity_status.time = current_time; entity_status.name = name; entity_status.bounding_box = parameters.bounding_box; entity_status.action_status = traffic_simulator_msgs::msg::ActionStatus(); @@ -477,11 +455,11 @@ class EntityManager "LaneletPose is not supported type as pose argument. Only CanonicalizedLaneletPose and " "msg::Pose are supported as pose argument of EntityManager::spawnEntity()."); } else if constexpr (std::is_same_v, CanonicalizedLaneletPose>) { - entity_status.pose = toMapPose(pose); + entity_status.pose = pose::toMapPose(pose); return CanonicalizedEntityStatus(entity_status, pose); } else if constexpr (std::is_same_v, geometry_msgs::msg::Pose>) { entity_status.pose = pose; - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( + const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( pose, parameters.bounding_box, include_crosswalk, matching_distance, hdmap_utils_ptr_); return CanonicalizedEntityStatus(entity_status, canonicalized_lanelet_pose); } @@ -494,9 +472,6 @@ class EntityManager success) { // FIXME: this ignores V2I traffic lights iter->second->setTrafficLightManager(conventional_traffic_light_manager_ptr_); - if (npc_logic_started_ && not is(name)) { - iter->second->startNpcLogic(getCurrentTime()); - } return success; } else { THROW_SEMANTIC_ERROR("Entity ", std::quoted(name), " is already exists."); @@ -520,9 +495,9 @@ class EntityManager void updateHdmapMarker(); - void startNpcLogic(const double current_time); + auto startNpcLogic(const double current_time) -> void; - auto isNpcLogicStarted() const { return npc_logic_started_; } + auto isNpcLogicStarted() const -> bool { return npc_logic_started_; } }; } // namespace entity } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp index 6617144d89b..27ac070dc13 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp @@ -38,13 +38,6 @@ class MiscObjectEntity : public EntityBase auto getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints & override; - 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 std::string result = "MiscObjectEntity"; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp index 6c5dec63a36..ce36aa648b0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp @@ -66,11 +66,9 @@ class PedestrianEntity : public EntityBase void appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) override; - auto getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & override; - auto getEntityTypename() const -> const std::string & override; - void onUpdate(double current_time, double step_time) override; + auto onUpdate(const double current_time, const double step_time) -> void override; void requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) override; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp index a0fa023d5b5..d3b6b53567c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp @@ -84,8 +84,6 @@ class VehicleEntity : public EntityBase auto getMaxDeceleration() const -> double override; - auto getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & override; - auto getEntityTypename() const -> const std::string & override; auto getGoalPoses() -> std::vector override; @@ -96,7 +94,7 @@ class VehicleEntity : public EntityBase auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override; - void onUpdate(double current_time, double step_time) override; + auto onUpdate(const double current_time, const double step_time) -> void override; void requestAcquirePosition(const CanonicalizedLaneletPose &); diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 696eb1f705e..8d8b2179152 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 3.5.5 + 4.2.2 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 0bf4e86a742..d02bbfc7c04 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -99,20 +99,22 @@ auto API::respawn( } } +auto API::getEntity(const std::string & name) const -> std::shared_ptr +{ + return entity_manager_ptr_->getEntity(name); +} + auto API::setEntityStatus( - const std::string & name, - const std::optional & canonicalized_lanelet_pose, + const std::string & name, const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void { if (const auto entity = getEntity(name)) { - auto status = static_cast(entity->getStatus()); + auto status = static_cast(entity->getCanonicalizedStatus()); status.action_status = action_status; - if (canonicalized_lanelet_pose) { - status.pose = static_cast(canonicalized_lanelet_pose.value()); - status.lanelet_pose = static_cast(canonicalized_lanelet_pose.value()); - status.lanelet_pose_valid = true; - } - entity->setStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); + status.pose = static_cast(canonicalized_lanelet_pose); + status.lanelet_pose = static_cast(canonicalized_lanelet_pose); + status.lanelet_pose_valid = true; + entity->setCanonicalizedStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); } else { THROW_SIMULATION_ERROR("Cannot set entity \"", name, "\" status - such entity does not exist."); } @@ -121,21 +123,7 @@ auto API::setEntityStatus( auto API::setEntityStatus(const std::string & name, const EntityStatus & status) -> void { if (const auto entity = getEntity(name)) { - if (status.lanelet_pose_valid) { - const auto canonicalized_lanelet_pose = - canonicalize(status.lanelet_pose, entity_manager_ptr_->getHdmapUtils()); - entity->setStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); - } else { - const auto include_crosswalk = [](const auto & entity_type) { - return (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == entity_type.type) || - (traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == entity_type.type); - }(entity->getEntityType()); - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - status.pose, entity->getBoundingBox(), include_crosswalk, - entity->getDefaultMatchingDistanceForLaneletPoseCalculation(), - entity_manager_ptr_->getHdmapUtils()); - entity->setStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); - } + entity->setStatus(status); } else { THROW_SIMULATION_ERROR("Cannot set entity \"", name, "\" status - such entity does not exist."); } @@ -147,7 +135,8 @@ std::optional API::getTimeHeadway( { if (auto from_entity = getEntity(from_entity_name); from_entity) { if (auto to_entity = getEntity(to_entity_name); to_entity) { - if (auto relative_pose = relativePose(from_entity->getMapPose(), to_entity->getMapPose()); + if (auto relative_pose = + pose::relativePose(from_entity->getMapPose(), to_entity->getMapPose()); relative_pose && relative_pose->position.x <= 0) { const double time_headway = (relative_pose->position.x * -1) / getCurrentTwist(to_entity_name).linear.x; @@ -162,8 +151,16 @@ auto API::setEntityStatus( const std::string & name, const LaneletPose & lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void { - setEntityStatus( - name, canonicalize(lanelet_pose, entity_manager_ptr_->getHdmapUtils()), action_status); + if ( + const auto canonicalized_lanelet_pose = + pose::canonicalize(lanelet_pose, entity_manager_ptr_->getHdmapUtils())) { + setEntityStatus(name, canonicalized_lanelet_pose.value(), action_status); + } else { + std::stringstream ss; + ss << "Status can not be set. lanelet pose: " << lanelet_pose + << " cannot be canonicalized for "; + THROW_SEMANTIC_ERROR(ss.str(), " entity named: ", std::quoted(name), "."); + } } auto API::setEntityStatus( @@ -171,7 +168,7 @@ auto API::setEntityStatus( const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void { if (const auto entity = getEntity(name)) { - EntityStatus status = static_cast(entity->getStatus()); + EntityStatus status = static_cast(entity->getCanonicalizedStatus()); status.pose = map_pose; status.action_status = action_status; setEntityStatus(name, status); @@ -187,7 +184,7 @@ auto API::setEntityStatus( { if (const auto reference_entity = getEntity(reference_entity_name)) { setEntityStatus( - name, transformRelativePoseToGlobal(reference_entity->getMapPose(), relative_pose), + name, pose::transformRelativePoseToGlobal(reference_entity->getMapPose(), relative_pose), action_status); } else { THROW_SIMULATION_ERROR( @@ -304,8 +301,9 @@ bool API::updateEntitiesStatusInSim() simulation_api_schema::UpdateEntityStatusRequest req; req.set_npc_logic_started(entity_manager_ptr_->isNpcLogicStarted()); for (const auto & entity_name : entity_manager_ptr_->getEntityNames()) { - auto entity_status = entity_manager_ptr_->getEntityStatus(entity_name); - simulation_interface::toProto(static_cast(entity_status), *req.add_status()); + const auto entity_status = + static_cast(entity_manager_ptr_->getEntityStatus(entity_name)); + simulation_interface::toProto(entity_status, *req.add_status()); if (entity_manager_ptr_->is(entity_name)) { req.set_overwrite_ego_status(entity_manager_ptr_->isControlledBySimulator(entity_name)); } @@ -314,17 +312,18 @@ bool API::updateEntitiesStatusInSim() simulation_api_schema::UpdateEntityStatusResponse res; if (auto res = zeromq_client_.call(req); res.result().success()) { for (const auto & res_status : res.status()) { - auto name = res_status.name(); - auto entity_status = static_cast(entity_manager_ptr_->getEntityStatus(name)); + auto entity_name = res_status.name(); + auto entity_status = + static_cast(entity_manager_ptr_->getEntityStatus(entity_name)); simulation_interface::toMsg(res_status.pose(), entity_status.pose); simulation_interface::toMsg(res_status.action_status(), entity_status.action_status); - if (entity_manager_ptr_->is(name)) { - setMapPose(name, entity_status.pose); - setTwist(name, entity_status.action_status.twist); - setAcceleration(name, entity_status.action_status.accel); + if (entity_manager_ptr_->is(entity_name)) { + setMapPose(entity_name, entity_status.pose); + setTwist(entity_name, entity_status.action_status.twist); + setAcceleration(entity_name, entity_status.action_status.accel); } else { - setEntityStatus(name, entity_status); + setEntityStatus(entity_name, entity_status); } } return true; @@ -361,11 +360,12 @@ bool API::updateFrame() void API::startNpcLogic() { - if (isNpcLogicStarted()) { + if (entity_manager_ptr_->isNpcLogicStarted()) { THROW_SIMULATION_ERROR("NPC logics are already started."); + } else { + clock_.start(); + entity_manager_ptr_->startNpcLogic(getCurrentTime()); } - clock_.start(); - entity_manager_ptr_->startNpcLogic(getCurrentTime()); } void API::requestLaneChange(const std::string & name, const lanelet::Id & lanelet_id) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index c27cabdd538..da2ced4710c 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -47,9 +47,8 @@ auto makeUpdatedStatus( const traffic_simulator_msgs::msg::EntityStatus & entity_status, traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, - const std::shared_ptr & hdmap_utils, double step_time, - double matching_distance, std::optional target_speed) - -> std::optional + const std::shared_ptr & hdmap_utils, const double step_time, + double matching_distance, std::optional target_speed) -> std::optional { using math::arithmetic::isApproximatelyEqualTo; using math::arithmetic::isDefinitelyLessThan; @@ -85,31 +84,6 @@ auto makeUpdatedStatus( return hypot(from, to); }; - auto fill_lanelet_data_and_adjust_orientation = - [&](traffic_simulator_msgs::msg::EntityStatus & status) { - if (const auto lanelet_pose = - hdmap_utils->toLaneletPose(status.pose, status.bounding_box, false, matching_distance); - lanelet_pose) { - 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 = math::geometry::convertQuaternionToEulerAngle(lanelet_quaternion); - const auto entity_rpy = - math::geometry::convertQuaternionToEulerAngle(status.pose.orientation); - // adjust orientation in EntityStatus.pose (only pitch) and in EntityStatus.LaneletPose - 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 = math::geometry::convertQuaternionToEulerAngle( - math::geometry::getRotation(lanelet_quaternion, status.pose.orientation)); - status.lanelet_pose_valid = true; - } else { - status.lanelet_pose_valid = false; - } - }; - auto discard_the_front_waypoint_and_recurse = [&]() { /* The OpenSCENARIO standard does not define the behavior when the value of @@ -595,13 +569,6 @@ auto makeUpdatedStatus( } }(); - /* - After the step, i.e. movement in the direction of designed_velocity, it is necessary to adjust - the pose of the entity to the lane - if possible, the pitch orientation may be - changed as a result because the slope of the lane may be different - */ - fill_lanelet_data_and_adjust_orientation(updated_status); - updated_status.action_status.twist.linear.x = norm(desired_velocity); updated_status.action_status.twist.linear.y = 0; @@ -623,6 +590,8 @@ auto makeUpdatedStatus( updated_status.time = entity_status.time + step_time; + updated_status.lanelet_pose_valid = false; + return updated_status; } } diff --git a/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp b/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp index d2efc60ff89..0fc3c13a0e2 100644 --- a/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp +++ b/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp @@ -23,7 +23,8 @@ namespace traffic_simulator { namespace longitudinal_speed_planning { -LongitudinalSpeedPlanner::LongitudinalSpeedPlanner(double step_time, const std::string & entity) +LongitudinalSpeedPlanner::LongitudinalSpeedPlanner( + const double step_time, const std::string & entity) : step_time(step_time), entity(entity) { } diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index 57869c33917..7bb9052dcea 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -25,47 +25,6 @@ CanonicalizedEntityStatus::CanonicalizedEntityStatus( const std::optional & canonicalized_lanelet_pose) : canonicalized_lanelet_pose_{canonicalized_lanelet_pose}, entity_status_{may_non_canonicalized_entity_status} -{ - canonicalize(); -} - -/// @todo this constructor will be removed (after adaptation of behavior_tree) -CanonicalizedEntityStatus::CanonicalizedEntityStatus( - const EntityStatus & may_non_canonicalized_entity_status, - const std::shared_ptr & hdmap_utils) -: canonicalized_lanelet_pose_{may_non_canonicalized_entity_status.lanelet_pose_valid?std::optional(CanonicalizedLaneletPose( - may_non_canonicalized_entity_status.lanelet_pose, hdmap_utils)):std::nullopt}, - entity_status_{may_non_canonicalized_entity_status} -{ - canonicalize(); -} - -/// @todo this constructor will be removed (after adaptation of behavior_tree) -CanonicalizedEntityStatus::CanonicalizedEntityStatus( - const EntityStatus & may_non_canonicalized_entity_status, - const std::shared_ptr & hdmap_utils, const lanelet::Ids & route_lanelets) -: canonicalized_lanelet_pose_{may_non_canonicalized_entity_status.lanelet_pose_valid?std::optional(CanonicalizedLaneletPose( - may_non_canonicalized_entity_status.lanelet_pose, route_lanelets, hdmap_utils )):std::nullopt}, - entity_status_{may_non_canonicalized_entity_status} -{ - canonicalize(); -} - -CanonicalizedEntityStatus::CanonicalizedEntityStatus(const CanonicalizedEntityStatus & obj) -: canonicalized_lanelet_pose_(obj.canonicalized_lanelet_pose_), - entity_status_(static_cast(obj)) -{ -} - -CanonicalizedEntityStatus & CanonicalizedEntityStatus::operator=( - const CanonicalizedEntityStatus & obj) -{ - this->canonicalized_lanelet_pose_ = obj.canonicalized_lanelet_pose_; - this->entity_status_ = obj.entity_status_; - return *this; -} - -auto CanonicalizedEntityStatus::canonicalize() -> void { assert(entity_status_.lanelet_pose_valid == canonicalized_lanelet_pose_.has_value()); if (canonicalized_lanelet_pose_) { @@ -75,7 +34,7 @@ auto CanonicalizedEntityStatus::canonicalize() -> void The position in Oz axis and orientation based on LaneletPose are rewritten to the used msg::Pose (map_pose) since such adjustment relative to the lanelet is necessary, The position in Ox and Oy axis is not rewritten because the map_pose retrieved via - lanelet_pose = toCanonicalizedLaneletPose(map_pose), then map_pose = toMapPose(lanelet_pose) + lanelet_pose = pose::toCanonicalizedLaneletPose(map_pose), then map_pose pose::toMapPose(lanelet_pose) can be slightly different from the original one (especially if the entity changes lane). */ const auto map_pose_based_on_lanelet_pose = @@ -88,43 +47,102 @@ auto CanonicalizedEntityStatus::canonicalize() -> void } } +CanonicalizedEntityStatus::CanonicalizedEntityStatus(const CanonicalizedEntityStatus & obj) +: canonicalized_lanelet_pose_(obj.canonicalized_lanelet_pose_), + entity_status_(static_cast(obj)) +{ +} + +auto CanonicalizedEntityStatus::set(const CanonicalizedEntityStatus & status) -> void +{ + assert(getType() == status.getType()); + assert(getSubtype() == status.getSubtype()); + assert(getName() == status.getName()); + assert(getBoundingBox() == status.getBoundingBox()); + entity_status_ = status.entity_status_; + canonicalized_lanelet_pose_ = status.canonicalized_lanelet_pose_; +} + +auto CanonicalizedEntityStatus::set( + const EntityStatus & status, const lanelet::Ids & lanelet_ids, const double matching_distance, + const std::shared_ptr & hdmap_utils_ptr) -> void +{ + const auto include_crosswalk = + getType().type == traffic_simulator_msgs::msg::EntityType::PEDESTRIAN || + getType().type == traffic_simulator_msgs::msg::EntityType::MISC_OBJECT; + + std::optional canonicalized_lanelet_pose; + if (status.lanelet_pose_valid) { + canonicalized_lanelet_pose = pose::canonicalize(status.lanelet_pose, hdmap_utils_ptr); + } else { + // prefer the current lanelet + canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + status.pose, getBoundingBox(), lanelet_ids, include_crosswalk, matching_distance, + hdmap_utils_ptr); + } + set(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); +} + +auto CanonicalizedEntityStatus::set( + const EntityStatus & status, const double matching_distance, + const std::shared_ptr & hdmap_utils_ptr) -> void +{ + set(status, getLaneletIds(), matching_distance, hdmap_utils_ptr); +} + +auto CanonicalizedEntityStatus::setAction(const std::string & action) -> void +{ + entity_status_.action_status.current_action = action; +} + +auto CanonicalizedEntityStatus::getActionStatus() const noexcept + -> const traffic_simulator_msgs::msg::ActionStatus & +{ + return entity_status_.action_status; +} + auto CanonicalizedEntityStatus::laneMatchingSucceed() const noexcept -> bool { return canonicalized_lanelet_pose_.has_value(); } auto CanonicalizedEntityStatus::getBoundingBox() const noexcept - -> traffic_simulator_msgs::msg::BoundingBox + -> const traffic_simulator_msgs::msg::BoundingBox & { return entity_status_.bounding_box; } -auto CanonicalizedEntityStatus::getMapPose() const noexcept -> geometry_msgs::msg::Pose +auto CanonicalizedEntityStatus::setMapPose(const geometry_msgs::msg::Pose & pose) -> void +{ + entity_status_.pose = pose; +} + +auto CanonicalizedEntityStatus::getMapPose() const noexcept -> const geometry_msgs::msg::Pose & { return entity_status_.pose; } -auto CanonicalizedEntityStatus::getLaneletPose() const -> LaneletPose +auto CanonicalizedEntityStatus::getLaneletPose() const noexcept -> const LaneletPose & { if (canonicalized_lanelet_pose_) { - return static_cast(canonicalized_lanelet_pose_.value()); + return canonicalized_lanelet_pose_->getLaneletPose(); } else { THROW_SEMANTIC_ERROR("Target entity status did not matched to lanelet pose."); } } -auto CanonicalizedEntityStatus::getLaneletId() const -> lanelet::Id +auto CanonicalizedEntityStatus::getLaneletId() const noexcept -> lanelet::Id { return getLaneletPose().lanelet_id; } -auto CanonicalizedEntityStatus::getLaneletIds() const -> lanelet::Ids +auto CanonicalizedEntityStatus::getLaneletIds() const noexcept -> lanelet::Ids { return laneMatchingSucceed() ? lanelet::Ids{getLaneletId()} : lanelet::Ids{}; } -auto CanonicalizedEntityStatus::getCanonicalizedLaneletPose() const - -> std::optional +auto CanonicalizedEntityStatus::getCanonicalizedLaneletPose() const noexcept + -> const std::optional & { return canonicalized_lanelet_pose_; } @@ -134,7 +152,7 @@ auto CanonicalizedEntityStatus::setTwist(const geometry_msgs::msg::Twist & twist entity_status_.action_status.twist = twist; } -auto CanonicalizedEntityStatus::getTwist() const noexcept -> geometry_msgs::msg::Twist +auto CanonicalizedEntityStatus::getTwist() const noexcept -> const geometry_msgs::msg::Twist & { return entity_status_.action_status.twist; } @@ -154,7 +172,7 @@ auto CanonicalizedEntityStatus::setLinearAcceleration(double linear_acceleration entity_status_.action_status.accel.linear.x = linear_acceleration; } -auto CanonicalizedEntityStatus::getAccel() const noexcept -> geometry_msgs::msg::Accel +auto CanonicalizedEntityStatus::getAccel() const noexcept -> const geometry_msgs::msg::Accel & { return entity_status_.action_status.accel; } @@ -164,11 +182,6 @@ auto CanonicalizedEntityStatus::setLinearJerk(double linear_jerk) -> void entity_status_.action_status.linear_jerk = linear_jerk; } -auto CanonicalizedEntityStatus::setAction(const std::string & action) -> void -{ - entity_status_.action_status.current_action = action; -} - auto CanonicalizedEntityStatus::getLinearJerk() const noexcept -> double { return entity_status_.action_status.linear_jerk; @@ -179,23 +192,15 @@ auto CanonicalizedEntityStatus::setTime(double time) -> void { entity_status_.ti auto CanonicalizedEntityStatus::getTime() const noexcept -> double { return entity_status_.time; } } // namespace entity_status -auto isSameLaneletId(const CanonicalizedEntityStatus & s0, const CanonicalizedEntityStatus & s1) +auto isSameLaneletId( + const CanonicalizedEntityStatus & first_status, const CanonicalizedEntityStatus & second_status) -> bool { - if (const auto s0_canonicalized_lanelet_pose = s0.getCanonicalizedLaneletPose()) { - if (const auto s1_canonicalized_lanelet_pose = s1.getCanonicalizedLaneletPose()) { - return isSameLaneletId( - s0_canonicalized_lanelet_pose.value(), s1_canonicalized_lanelet_pose.value()); - } - } - THROW_SIMULATION_ERROR("There is no Lanelet pose"); + return first_status.getLaneletId() == second_status.getLaneletId(); } -auto isSameLaneletId(const CanonicalizedEntityStatus & s, const lanelet::Id lanelet_id) -> bool +auto isSameLaneletId(const CanonicalizedEntityStatus & status, const lanelet::Id lanelet_id) -> bool { - if (const auto s_canonicalized_lanelet_pose = s.getCanonicalizedLaneletPose()) { - return isSameLaneletId(s_canonicalized_lanelet_pose.value(), lanelet_id); - } - THROW_SIMULATION_ERROR("There is no Lanelet pose"); + return status.getLaneletId() == lanelet_id; } } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp index 5153f119076..140ab39bf15 100644 --- a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp +++ b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp @@ -30,7 +30,7 @@ CanonicalizedLaneletPose::CanonicalizedLaneletPose( : lanelet_pose_(canonicalize(maybe_non_canonicalized_lanelet_pose, hdmap_utils)), lanelet_poses_( hdmap_utils->getAllCanonicalizedLaneletPoses(maybe_non_canonicalized_lanelet_pose)), - map_pose_(toMapPose(lanelet_pose_, hdmap_utils)) + map_pose_(pose::toMapPose(lanelet_pose_, hdmap_utils)) { adjustOrientationAndOzPosition(hdmap_utils); } @@ -41,7 +41,7 @@ CanonicalizedLaneletPose::CanonicalizedLaneletPose( : lanelet_pose_(canonicalize(maybe_non_canonicalized_lanelet_pose, route_lanelets, hdmap_utils)), lanelet_poses_( hdmap_utils->getAllCanonicalizedLaneletPoses(maybe_non_canonicalized_lanelet_pose)), - map_pose_(toMapPose(lanelet_pose_, hdmap_utils)) + map_pose_(pose::toMapPose(lanelet_pose_, hdmap_utils)) { adjustOrientationAndOzPosition(hdmap_utils); } diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index e9277e81124..7ba999c60b1 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -41,23 +41,26 @@ auto EgoEntity::makeFieldOperatorApplication( if (const auto architecture_type = getParameter(node_parameters, "architecture_type", "awf/universe"); architecture_type.find("awf/universe") != std::string::npos) { - std::string rviz_config = getParameter(node_parameters, "rviz_config", ""); + auto parameters = getParameter>(node_parameters, "autoware.", {}); + + // clang-format off + parameters.push_back("map_path:=" + configuration.map_path.string()); + parameters.push_back("lanelet2_map_file:=" + configuration.getLanelet2MapFile()); + parameters.push_back("pointcloud_map_file:=" + configuration.getPointCloudMapFile()); + parameters.push_back("sensor_model:=" + getParameter(node_parameters, "sensor_model")); + parameters.push_back("vehicle_model:=" + getParameter(node_parameters, "vehicle_model")); + parameters.push_back("rviz_config:=" + getParameter(node_parameters, "rviz_config")); + parameters.push_back("scenario_simulation:=true"); + parameters.push_back("use_foa:=false"); + parameters.push_back("perception/enable_traffic_light:=" + std::string(architecture_type >= "awf/universe/20230906" ? "true" : "false")); + parameters.push_back("use_sim_time:=" + std::string(getParameter(node_parameters, "use_sim_time", false) ? "true" : "false")); + // clang-format on + return getParameter(node_parameters, "launch_autoware", true) ? std::make_unique< concealer::FieldOperatorApplicationFor>( 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:=" + 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()), - "scenario_simulation:=true", "use_foa:=false", - "perception/enable_traffic_light:=" + - std::string((architecture_type >= "awf/universe/20230906") ? "true" : "false")) + getParameter(node_parameters, "autoware_launch_file"), parameters) : std::make_unique< concealer::FieldOperatorApplicationFor>(); } else { @@ -103,13 +106,6 @@ auto EgoEntity::getEntityTypename() const -> const std::string & return result; } -auto EgoEntity::getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & -{ - static traffic_simulator_msgs::msg::EntityType type; - type.type = traffic_simulator_msgs::msg::EntityType::EGO; - return type; -} - auto EgoEntity::getObstacle() -> std::optional { return std::nullopt; @@ -131,39 +127,47 @@ auto EgoEntity::getRouteLanelets(double /*unused horizon*/) -> lanelet::Ids return ids; } -auto EgoEntity::getCurrentPose() const -> geometry_msgs::msg::Pose { return status_.getMapPose(); } +auto EgoEntity::getCurrentPose() const -> const geometry_msgs::msg::Pose & +{ + return status_->getMapPose(); +} auto EgoEntity::getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray { return field_operator_application->getWaypoints(); } +void EgoEntity::updateFieldOperatorApplication() const +{ + field_operator_application->rethrow(); + field_operator_application->spinSome(); +} + void EgoEntity::onUpdate(double current_time, double step_time) { EntityBase::onUpdate(current_time, step_time); - if (is_controlled_by_simulator_ && npc_logic_started_) { + if (is_controlled_by_simulator_) { if ( - const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus( - static_cast(status_), *polyline_trajectory_, - behavior_parameter_, hdmap_utils_ptr_, step_time, - getDefaultMatchingDistanceForLaneletPoseCalculation(), - target_speed_ ? target_speed_.value() : status_.getTwist().linear.x)) { - // prefer the current lanelet - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - updated_status.value().pose, status_.getBoundingBox(), status_.getLaneletIds(), false, - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); - setStatus(CanonicalizedEntityStatus(*updated_status, canonicalized_lanelet_pose)); + const auto non_canonicalized_updated_status = + traffic_simulator::follow_trajectory::makeUpdatedStatus( + static_cast(*status_), *polyline_trajectory_, + behavior_parameter_, hdmap_utils_ptr_, step_time, + getDefaultMatchingDistanceForLaneletPoseCalculation(), + target_speed_ ? target_speed_.value() : status_->getTwist().linear.x)) { + // prefer current lanelet on ss2 side + setStatus(non_canonicalized_updated_status.value(), status_->getLaneletIds()); } else { is_controlled_by_simulator_ = false; } } + if (not is_controlled_by_simulator_) { + updateEntityStatusTimestamp(current_time + step_time); + } updateStandStillDuration(step_time); updateTraveledDistance(step_time); - - field_operator_application->rethrow(); - field_operator_application->spinSome(); + updateFieldOperatorApplication(); EntityBase::onPostUpdate(current_time, step_time); } @@ -269,15 +273,22 @@ auto EgoEntity::setBehaviorParameter( behavior_parameter_ = behavior_parameter; } -void EgoEntity::requestSpeedChange(double value, bool) +auto EgoEntity::requestSpeedChange(double value, bool /* continuous */) -> void { - target_speed_ = value; - field_operator_application->restrictTargetSpeed(value); + if (status_->getTime() > 0.0) { + THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario."); + } else { + target_speed_ = value; + field_operator_application->restrictTargetSpeed(value); + } } -void EgoEntity::requestSpeedChange( - const speed_change::RelativeTargetSpeed & /*target_speed*/, bool /*continuous*/) +auto EgoEntity::requestSpeedChange( + const speed_change::RelativeTargetSpeed & /*target_speed*/, bool /*continuous*/) -> void { + THROW_SEMANTIC_ERROR( + "The traffic_simulator's request to set speed to the Ego type entity is for initialization " + "purposes only."); } auto EgoEntity::setVelocityLimit(double value) -> void // @@ -288,13 +299,13 @@ auto EgoEntity::setVelocityLimit(double value) -> void // auto EgoEntity::setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void { - auto status = static_cast(status_); - status.pose = map_pose; - const auto unique_route_lanelets = traffic_simulator::helper::getUniqueValues(getRouteLanelets()); - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - map_pose, status_.getBoundingBox(), unique_route_lanelets, false, + auto entity_status = static_cast(*status_); + entity_status.pose = map_pose; + entity_status.lanelet_pose_valid = false; + // prefer current lanelet on Autoware side + status_->set( + entity_status, helper::getUniqueValues(getRouteLanelets()), getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); - status_ = CanonicalizedEntityStatus(status, canonicalized_lanelet_pose); } } // namespace entity } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index a607f9baa35..ff4b43a4b0f 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -34,10 +34,9 @@ EntityBase::EntityBase( const std::shared_ptr & hdmap_utils_ptr) : name(name), verbose(true), - status_(entity_status), - status_before_update_(status_), - hdmap_utils_ptr_(hdmap_utils_ptr), - npc_logic_started_(false) + status_(std::make_shared(entity_status)), + status_before_update_(*status_), + hdmap_utils_ptr_(hdmap_utils_ptr) { if (name != static_cast(entity_status).name) { THROW_SIMULATION_ERROR( @@ -67,7 +66,7 @@ auto EntityBase::get2DPolygon() const -> std::vector auto EntityBase::getCanonicalizedLaneletPose() const -> std::optional { - return status_.getCanonicalizedLaneletPose(); + return status_->getCanonicalizedLaneletPose(); } auto EntityBase::getCanonicalizedLaneletPose(double matching_distance) const @@ -79,8 +78,8 @@ auto EntityBase::getCanonicalizedLaneletPose(double matching_distance) const }(getEntityType()); // prefer the current lanelet - return toCanonicalizedLaneletPose( - status_.getMapPose(), status_.getBoundingBox(), status_.getLaneletIds(), include_crosswalk, + return pose::toCanonicalizedLaneletPose( + status_->getMapPose(), status_->getBoundingBox(), status_->getLaneletIds(), include_crosswalk, matching_distance, hdmap_utils_ptr_); } @@ -97,20 +96,21 @@ auto EntityBase::isTargetSpeedReached(double target_speed) const -> bool auto EntityBase::isTargetSpeedReached(const speed_change::RelativeTargetSpeed & target_speed) const -> bool { - return isTargetSpeedReached(target_speed.getAbsoluteValue(getStatus(), other_status_)); + return isTargetSpeedReached( + target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_)); } -void EntityBase::onUpdate(double /*current_time*/, double step_time) +auto EntityBase::onUpdate(const double /*current_time*/, const double step_time) -> void { job_list_.update(step_time, job::Event::PRE_UPDATE); step_time_ = step_time; - status_before_update_ = status_; + status_before_update_.set(*status_); speed_planner_ = std::make_unique( step_time, name); } -void EntityBase::onPostUpdate(double /*current_time*/, double step_time) +auto EntityBase::onPostUpdate(const double /*current_time*/, const double step_time) -> void { job_list_.update(step_time, job::Event::POST_UPDATE); } @@ -140,7 +140,7 @@ void EntityBase::requestLaneChange( "Source entity does not assigned to lanelet. Please check source entity name : ", name, " exists on lane."); } - reference_lanelet_id = getStatus().getLaneletId(); + reference_lanelet_id = status_->getLaneletId(); } else { if (other_status_.find(target.entity_name) == other_status_.end()) { THROW_SEMANTIC_ERROR( @@ -318,8 +318,8 @@ void EntityBase::requestSpeedChangeWithConstantAcceleration( * @brief Checking if the entity reaches target speed. */ [this, target_speed, acceleration](double) { - double diff = - target_speed.getAbsoluteValue(getStatus(), other_status_) - getCurrentTwist().linear.x; + double diff = target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_) - + getCurrentTwist().linear.x; /** * @brief Hard coded parameter, threshold for difference */ @@ -346,7 +346,7 @@ void EntityBase::requestSpeedChangeWithConstantAcceleration( } case speed_change::Transition::STEP: { requestSpeedChange(target_speed, continuous); - setLinearVelocity(target_speed.getAbsoluteValue(getStatus(), other_status_)); + setLinearVelocity(target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_)); break; } } @@ -369,17 +369,19 @@ void EntityBase::requestSpeedChangeWithTimeConstraint( switch (transition) { case speed_change::Transition::LINEAR: { requestSpeedChangeWithTimeConstraint( - target_speed.getAbsoluteValue(getStatus(), other_status_), transition, acceleration_time); + target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_), transition, + acceleration_time); break; } case speed_change::Transition::AUTO: { requestSpeedChangeWithTimeConstraint( - target_speed.getAbsoluteValue(getStatus(), other_status_), transition, acceleration_time); + target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_), transition, + acceleration_time); break; } case speed_change::Transition::STEP: { requestSpeedChange(target_speed, false); - setLinearVelocity(target_speed.getAbsoluteValue(getStatus(), other_status_)); + setLinearVelocity(target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_)); break; } } @@ -464,7 +466,7 @@ void EntityBase::requestSpeedChange( if (other_status_.find(target_speed.reference_entity_name) == other_status_.end()) { return true; } - target_speed_ = target_speed.getAbsoluteValue(getStatus(), other_status_); + target_speed_ = target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_); return false; }, [this]() {}, job::Type::LINEAR_VELOCITY, true, job::Event::POST_UPDATE); @@ -478,7 +480,7 @@ void EntityBase::requestSpeedChange( return true; } if (isTargetSpeedReached(target_speed)) { - target_speed_ = target_speed.getAbsoluteValue(getStatus(), other_status_); + target_speed_ = target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_); return true; } return false; @@ -526,37 +528,30 @@ void EntityBase::setOtherStatus( other_status_.erase(name); } -auto EntityBase::setStatus(const CanonicalizedEntityStatus & status) -> void +auto EntityBase::setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void { - auto new_status = static_cast(status); + status_->set( + status, lanelet_ids, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); +} - /* - FIXME: DIRTY HACK!!! +auto EntityBase::setStatus(const EntityStatus & status) -> void +{ + status_->set(status, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); +} - It seems that some operations set an incomplete status without respecting - the original status obtained by getStatus. Below is the code to compensate - for the lack of set status. - */ - new_status.name = name; - new_status.type = getEntityType(); - new_status.subtype = getEntitySubtype(); - new_status.bounding_box = getBoundingBox(); - new_status.action_status.current_action = getCurrentAction(); - status_ = CanonicalizedEntityStatus(new_status, status.getCanonicalizedLaneletPose()); +auto EntityBase::setCanonicalizedStatus(const CanonicalizedEntityStatus & status) -> void +{ + status_->set(status); } auto EntityBase::setLinearVelocity(const double linear_velocity) -> void { - auto status = static_cast(getStatus()); - status.action_status.twist.linear.x = linear_velocity; - setStatus(CanonicalizedEntityStatus(status, status_.getCanonicalizedLaneletPose())); + status_->setLinearVelocity(linear_velocity); } auto EntityBase::setLinearAcceleration(const double linear_acceleration) -> void { - auto status = static_cast(getStatus()); - status.action_status.accel.linear.x = linear_acceleration; - setStatus(CanonicalizedEntityStatus(status, status_.getCanonicalizedLaneletPose())); + status_->setLinearAcceleration(linear_acceleration); } void EntityBase::setTrafficLightManager( @@ -567,25 +562,21 @@ void EntityBase::setTrafficLightManager( auto EntityBase::setTwist(const geometry_msgs::msg::Twist & twist) -> void { - auto new_status = static_cast(getStatus()); - new_status.action_status.twist = twist; - status_ = CanonicalizedEntityStatus(new_status, status_.getCanonicalizedLaneletPose()); + status_->setTwist(twist); } auto EntityBase::setAcceleration(const geometry_msgs::msg::Accel & accel) -> void { - auto new_status = static_cast(getStatus()); - new_status.action_status.accel = accel; - status_ = CanonicalizedEntityStatus(new_status, status_.getCanonicalizedLaneletPose()); + status_->setAccel(accel); } auto EntityBase::setLinearJerk(const double linear_jerk) -> void { - auto new_status = static_cast(getStatus()); - new_status.action_status.linear_jerk = linear_jerk; - status_ = CanonicalizedEntityStatus(new_status, status_.getCanonicalizedLaneletPose()); + status_->setLinearJerk(linear_jerk); } +auto EntityBase::setAction(const std::string & action) -> void { status_->setAction(action); } + auto EntityBase::setMapPose(const geometry_msgs::msg::Pose &) -> void { THROW_SEMANTIC_ERROR( @@ -634,44 +625,30 @@ void EntityBase::activateOutOfRangeJob( [this]() {}, job::Type::OUT_OF_RANGE, true, job::Event::POST_UPDATE); } -void EntityBase::startNpcLogic(const double current_time) -{ - updateEntityStatusTimestamp(current_time); - npc_logic_started_ = true; -} - void EntityBase::stopAtCurrentPosition() { - auto status = static_cast(getStatus()); - status.action_status.twist = geometry_msgs::msg::Twist(); - status.action_status.accel = geometry_msgs::msg::Accel(); - status.action_status.linear_jerk = 0; - setStatus(CanonicalizedEntityStatus(status, status_.getCanonicalizedLaneletPose())); + status_->setTwist(geometry_msgs::msg::Twist()); + status_->setAccel(geometry_msgs::msg::Accel()); + status_->setLinearJerk(0.0); } void EntityBase::updateEntityStatusTimestamp(const double current_time) { - auto status = static_cast(getStatus()); - status.time = current_time; - setStatus(CanonicalizedEntityStatus(status, status_.getCanonicalizedLaneletPose())); + status_->setTime(current_time); } auto EntityBase::updateStandStillDuration(const double step_time) -> double { - if ( - npc_logic_started_ and - std::abs(getCurrentTwist().linear.x) <= std::numeric_limits::epsilon()) { + if (std::abs(getCurrentTwist().linear.x) <= std::numeric_limits::epsilon()) { return stand_still_duration_ += step_time; + } else { + return stand_still_duration_ = 0.0; } - return stand_still_duration_ = 0.0; } auto EntityBase::updateTraveledDistance(const double step_time) -> double { - if (npc_logic_started_) { - traveled_distance_ += std::abs(getCurrentTwist().linear.x) * step_time; - } - return traveled_distance_; + return traveled_distance_ += std::abs(getCurrentTwist().linear.x) * step_time; } bool EntityBase::reachPosition(const std::string & target_name, const double tolerance) const @@ -717,7 +694,7 @@ auto EntityBase::requestSynchronize( ///@brief Check if the entity has already arrived to the target lanelet. if (reachPosition(entity_target, tolerance)) { - if (this->getStatus().getTwist().linear.x < target_speed + getMaxAcceleration() * step_time_) { + if (getCurrentTwist().linear.x < target_speed + getMaxAcceleration() * step_time_) { } else { RCLCPP_WARN_ONCE( rclcpp::get_logger("traffic_simulator"), @@ -765,7 +742,7 @@ auto EntityBase::requestSynchronize( const auto target_entity_velocity = other_status_.find(target_name)->second.getTwist().linear.x; - const auto entity_velocity = this->getStatus().getTwist().linear.x; + const auto entity_velocity = getCurrentTwist().linear.x; const auto target_entity_arrival_time = (std::abs(target_entity_velocity) > std::numeric_limits::epsilon()) ? target_entity_distance.value() / target_entity_velocity diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index ca1e411ddc0..7b730145ccd 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -147,8 +147,6 @@ bool EntityManager::entityExists(const std::string & name) return entities_.find(name) != std::end(entities_); } -auto EntityManager::getCurrentTime() const noexcept -> double { return current_time_; } - auto EntityManager::getEntityNames() const -> const std::vector { std::vector names{}; @@ -175,17 +173,13 @@ auto EntityManager::getEntity(const std::string & name) const } }; -auto EntityManager::getEntityStatus(const std::string & name) const -> CanonicalizedEntityStatus +auto EntityManager::getEntityStatus(const std::string & name) const + -> const CanonicalizedEntityStatus & { - if (const auto iter = entities_.find(name); iter == entities_.end()) { - THROW_SEMANTIC_ERROR("entity ", std::quoted(name), " does not exist."); + if (const auto entity = getEntity(name)) { + return entity->getCanonicalizedStatus(); } else { - auto entity_status = static_cast(iter->second->getStatus()); - assert(entity_status.name == name && "The entity name in status is different from key!"); - entity_status.action_status.current_action = getCurrentAction(name); - entity_status.time = current_time_; - return CanonicalizedEntityStatus( - entity_status, iter->second->getStatus().getCanonicalizedLaneletPose()); + THROW_SEMANTIC_ERROR("entity ", std::quoted(name), " does not exist."); } } @@ -217,10 +211,11 @@ const std::string EntityManager::getEgoName() const auto EntityManager::getObstacle(const std::string & name) -> std::optional { - if (!npc_logic_started_) { + if (not npc_logic_started_) { return std::nullopt; + } else { + return entities_.at(name)->getObstacle(); } - return entities_.at(name)->getObstacle(); } auto EntityManager::getPedestrianParameters(const std::string & name) const @@ -234,8 +229,6 @@ auto EntityManager::getPedestrianParameters(const std::string & name) const "Please check description of the scenario and entity type of the Entity: " + name); } -auto EntityManager::getStepTime() const noexcept -> double { return step_time_; } - auto EntityManager::getVehicleParameters(const std::string & name) const -> const traffic_simulator_msgs::msg::VehicleParameters & { @@ -250,10 +243,11 @@ auto EntityManager::getVehicleParameters(const std::string & name) const auto EntityManager::getWaypoints(const std::string & name) -> traffic_simulator_msgs::msg::WaypointsArray { - if (!npc_logic_started_) { + if (not npc_logic_started_) { return traffic_simulator_msgs::msg::WaypointsArray(); + } else { + return entities_.at(name)->getWaypoints(); } - return entities_.at(name)->getWaypoints(); } bool EntityManager::isEgoSpawned() const @@ -289,7 +283,7 @@ void EntityManager::requestLaneChange( if (const auto entity = getEntity(name); entity && entity->laneMatchingSucceed()) { if ( const auto target = hdmap_utils_ptr_->getLaneChangeableLaneletId( - entity->getStatus().getLaneletId(), direction)) { + entity->getCanonicalizedStatus().getLaneletId(), direction)) { requestLaneChange(name, target.value()); } } @@ -298,7 +292,7 @@ void EntityManager::requestLaneChange( void EntityManager::resetBehaviorPlugin( const std::string & name, const std::string & behavior_plugin_name) { - const auto status = getEntityStatus(name); + const auto & status = getEntityStatus(name); const auto behavior_parameter = getBehaviorParameter(name); if (is(name)) { THROW_SEMANTIC_ERROR( @@ -310,11 +304,13 @@ void EntityManager::resetBehaviorPlugin( } else if (is(name)) { const auto parameters = getVehicleParameters(name); despawnEntity(name); - spawnEntity(name, status.getMapPose(), parameters, behavior_plugin_name); + spawnEntity( + name, status.getMapPose(), parameters, status.getTime(), behavior_plugin_name); } else if (is(name)) { const auto parameters = getPedestrianParameters(name); despawnEntity(name); - spawnEntity(name, status.getMapPose(), parameters, behavior_plugin_name); + spawnEntity( + name, status.getMapPose(), parameters, status.getTime(), behavior_plugin_name); } else { THROW_SIMULATION_ERROR( "Entity :", name, "is unkown entity type.", "Please contact to developer."); @@ -325,61 +321,23 @@ void EntityManager::resetBehaviorPlugin( setBehaviorParameter(name, behavior_parameter); } -bool EntityManager::trafficLightsChanged() -{ - return conventional_traffic_light_manager_ptr_->hasAnyLightChanged() or - v2i_traffic_light_manager_ptr_->hasAnyLightChanged(); -} - -void EntityManager::requestSpeedChange( - const std::string & name, double target_speed, bool continuous) -{ - if (is(name) && getCurrentTime() > 0) { - THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario."); - } - return entities_.at(name)->requestSpeedChange(target_speed, continuous); -} - -void EntityManager::requestSpeedChange( - const std::string & name, const double target_speed, const speed_change::Transition transition, - const speed_change::Constraint constraint, const bool continuous) -{ - if (is(name) && getCurrentTime() > 0) { - THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario."); - } - return entities_.at(name)->requestSpeedChange(target_speed, transition, constraint, continuous); -} - -void EntityManager::requestSpeedChange( - const std::string & name, const speed_change::RelativeTargetSpeed & target_speed, bool continuous) +auto EntityManager::getCurrentAction(const std::string & name) const -> std::string { - if (is(name) && getCurrentTime() > 0) { - THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario."); - } - return entities_.at(name)->requestSpeedChange(target_speed, continuous); -} - -void EntityManager::requestSpeedChange( - const std::string & name, const speed_change::RelativeTargetSpeed & target_speed, - const speed_change::Transition transition, const speed_change::Constraint constraint, - const bool continuous) -{ - if (is(name) && getCurrentTime() > 0) { - THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario."); + if (const auto entity = getEntity(name)) { + if (not npc_logic_started_ and not is(name)) { + return "waiting"; + } else { + return entity->getCurrentAction(); + } + } else { + THROW_SEMANTIC_ERROR("entity : ", name, "does not exist"); } - return entities_.at(name)->requestSpeedChange(target_speed, transition, constraint, continuous); } -auto EntityManager::setEntityStatus( - const std::string & name, const CanonicalizedEntityStatus & status) -> void +bool EntityManager::trafficLightsChanged() { - if (is(name) && getCurrentTime() > 0 && not isControlledBySimulator(name)) { - THROW_SEMANTIC_ERROR( - "You cannot set entity status to the ego vehicle name ", std::quoted(name), - " after starting scenario."); - } else { - entities_.at(name)->setStatus(status); - } + return conventional_traffic_light_manager_ptr_->hasAnyLightChanged() or + v2i_traffic_light_manager_ptr_->hasAnyLightChanged(); } void EntityManager::setVerbose(const bool verbose) @@ -390,21 +348,30 @@ void EntityManager::setVerbose(const bool verbose) } } -auto EntityManager::updateNpcLogic(const std::string & name) -> const CanonicalizedEntityStatus & +auto EntityManager::updateNpcLogic( + const std::string & name, const double current_time, const double step_time) + -> const CanonicalizedEntityStatus & { if (configuration.verbose) { std::cout << "update " << name << " behavior" << std::endl; } - entities_[name]->onUpdate(current_time_, step_time_); - return entities_[name]->getStatus(); + if (const auto entity = getEntity(name)) { + // Update npc completely if logic has started, otherwise update Autoware only - if it is Ego + if (npc_logic_started_) { + entity->onUpdate(current_time, step_time); + } else if (const auto ego_entity = std::dynamic_pointer_cast(entity)) { + ego_entity->updateFieldOperatorApplication(); + } + return entity->getCanonicalizedStatus(); + } else { + THROW_SEMANTIC_ERROR("entity ", std::quoted(name), " does not exist."); + } } void EntityManager::update(const double current_time, const double step_time) { traffic_simulator::helper::StopWatch stop_watch_update( "EntityManager::update", configuration.verbose); - step_time_ = step_time; - current_time_ = current_time; setVerbose(configuration.verbose); if (npc_logic_started_) { conventional_traffic_light_updater_.createTimer( @@ -413,14 +380,14 @@ void EntityManager::update(const double current_time, const double step_time) } std::unordered_map all_status; for (auto && [name, entity] : entities_) { - all_status.emplace(name, entity->getStatus()); + all_status.emplace(name, entity->getCanonicalizedStatus()); } for (auto && [name, entity] : entities_) { entity->setOtherStatus(all_status); } all_status.clear(); for (auto && [name, entity] : entities_) { - all_status.emplace(name, updateNpcLogic(name)); + all_status.emplace(name, updateNpcLogic(name, current_time, step_time)); } for (auto && [name, entity] : entities_) { entity->setOtherStatus(all_status); @@ -439,6 +406,7 @@ void EntityManager::update(const double current_time, const double step_time) status_with_trajectory.obstacle_find = false; } status_with_trajectory.status = static_cast(status); + status_with_trajectory.status.time = current_time + step_time; status_with_trajectory.name = name; status_with_trajectory.time = current_time + step_time; status_array_msg.data.emplace_back(status_with_trajectory); @@ -448,7 +416,6 @@ void EntityManager::update(const double current_time, const double step_time) if (configuration.verbose) { stop_watch_update.print(); } - current_time_ += step_time; } void EntityManager::updateHdmapMarker() @@ -463,14 +430,12 @@ void EntityManager::updateHdmapMarker() lanelet_marker_pub_ptr_->publish(markers); } -void EntityManager::startNpcLogic(const double current_time) +auto EntityManager::startNpcLogic(const double current_time) -> void { npc_logic_started_ = true; - current_time_ = current_time; - for ([[maybe_unused]] auto && [name, entity] : entities_) { - entity->startNpcLogic(current_time_); + entity->updateEntityStatusTimestamp(current_time); } } } // namespace entity diff --git a/simulation/traffic_simulator/src/entity/misc_object_entity.cpp b/simulation/traffic_simulator/src/entity/misc_object_entity.cpp index 94e413348cb..1f05561e4ea 100644 --- a/simulation/traffic_simulator/src/entity/misc_object_entity.cpp +++ b/simulation/traffic_simulator/src/entity/misc_object_entity.cpp @@ -26,27 +26,19 @@ MiscObjectEntity::MiscObjectEntity( { } -void MiscObjectEntity::onUpdate(double, double step_time) +auto MiscObjectEntity::onUpdate(const double /*current_time*/, const double step_time) -> void { - auto status = static_cast(status_); - status.action_status.twist = geometry_msgs::msg::Twist(); - status.action_status.accel = geometry_msgs::msg::Accel(); - status.action_status.linear_jerk = 0; - status.action_status.current_action = "static"; - status_ = CanonicalizedEntityStatus(status, status_.getCanonicalizedLaneletPose()); - status_before_update_ = CanonicalizedEntityStatus(status, status_.getCanonicalizedLaneletPose()); - if (npc_logic_started_) { - updateStandStillDuration(step_time); - } + setTwist(geometry_msgs::msg::Twist()); + setAcceleration(geometry_msgs::msg::Accel()); + setLinearJerk(0.0); + setAction("static"); + updateStandStillDuration(step_time); + status_before_update_.set(*status_); } auto MiscObjectEntity::getCurrentAction() const -> std::string { - if (not npc_logic_started_) { - return "waiting"; - } else { - return static_cast(status_).action_status.current_action; - } + return static_cast(*status_).action_status.current_action; } auto MiscObjectEntity::getBehaviorParameter() const diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp index c92aa11a2be..e9b9572f895 100644 --- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp +++ b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp @@ -70,8 +70,8 @@ void PedestrianEntity::requestAssignRoute(const std::vector route; for (const auto & waypoint : waypoints) { if ( - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - waypoint, status_.getBoundingBox(), true, + const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + waypoint, status_->getBoundingBox(), true, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { route.emplace_back(canonicalized_lanelet_pose.value()); } else { @@ -90,9 +90,6 @@ auto PedestrianEntity::requestFollowTrajectory( std::string PedestrianEntity::getCurrentAction() const { - if (!npc_logic_started_) { - return "waiting"; - } return behavior_plugin_ptr_->getCurrentAction(); } @@ -109,7 +106,7 @@ auto PedestrianEntity::getDefaultDynamicConstraints() const auto PedestrianEntity::getRouteLanelets(double horizon) -> lanelet::Ids { - if (const auto canonicalized_lanelet_pose = status_.getCanonicalizedLaneletPose()) { + if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) { return route_planner_.getRouteLanelets(canonicalized_lanelet_pose.value(), horizon); } else { return {}; @@ -139,7 +136,7 @@ void PedestrianEntity::requestWalkStraight() void PedestrianEntity::requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) { behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); - if (status_.laneMatchingSucceed()) { + if (status_->laneMatchingSucceed()) { route_planner_.setWaypoints({lanelet_pose}); } behavior_plugin_ptr_->setGoalPoses({static_cast(lanelet_pose)}); @@ -149,8 +146,8 @@ void PedestrianEntity::requestAcquirePosition(const geometry_msgs::msg::Pose & m { behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); if ( - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - map_pose, status_.getBoundingBox(), true, + const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + map_pose, status_->getBoundingBox(), true, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { requestAcquirePosition(canonicalized_lanelet_pose.value()); } else { @@ -164,13 +161,6 @@ void PedestrianEntity::cancelRequest() route_planner_.cancelRoute(); } -auto PedestrianEntity::getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & -{ - static traffic_simulator_msgs::msg::EntityType type; - type.type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; - return type; -} - auto PedestrianEntity::getEntityTypename() const -> const std::string & { static const std::string result = "PedestrianEntity"; @@ -256,31 +246,26 @@ void PedestrianEntity::setDecelerationRateLimit(double deceleration_rate) setBehaviorParameter(behavior_parameter); } -void PedestrianEntity::onUpdate(double current_time, double step_time) +auto PedestrianEntity::onUpdate(const double current_time, const double step_time) -> void { EntityBase::onUpdate(current_time, step_time); - if (npc_logic_started_) { - behavior_plugin_ptr_->setOtherEntityStatus(other_status_); - behavior_plugin_ptr_->setEntityStatus( - std::make_shared(status_)); - behavior_plugin_ptr_->setTargetSpeed(target_speed_); - behavior_plugin_ptr_->setRouteLanelets(getRouteLanelets()); - behavior_plugin_ptr_->update(current_time, step_time); - auto status_updated = behavior_plugin_ptr_->getUpdatedStatus(); - if (const auto canonicalized_lanelet_pose = status_updated->getCanonicalizedLaneletPose()) { - if (isAtEndOfLanelets(canonicalized_lanelet_pose.value(), hdmap_utils_ptr_)) { - stopAtCurrentPosition(); - updateStandStillDuration(step_time); - updateTraveledDistance(step_time); - return; - } + behavior_plugin_ptr_->setOtherEntityStatus(other_status_); + behavior_plugin_ptr_->setCanonicalizedEntityStatus(status_); + behavior_plugin_ptr_->setTargetSpeed(target_speed_); + behavior_plugin_ptr_->setRouteLanelets(getRouteLanelets()); + /// @note CanonicalizedEntityStatus is updated here, it is not skipped even if isAtEndOfLanelets return true + behavior_plugin_ptr_->update(current_time, step_time); + if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) { + if (pose::isAtEndOfLanelets(canonicalized_lanelet_pose.value(), hdmap_utils_ptr_)) { + stopAtCurrentPosition(); + updateStandStillDuration(step_time); + updateTraveledDistance(step_time); + return; } - setStatus(*status_updated); - updateStandStillDuration(step_time); - updateTraveledDistance(step_time); - } else { - updateEntityStatusTimestamp(current_time); } + updateStandStillDuration(step_time); + updateTraveledDistance(step_time); + EntityBase::onPostUpdate(current_time, step_time); } } // namespace entity diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index bdc885a0699..deb2a7225cf 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -59,11 +59,7 @@ void VehicleEntity::cancelRequest() auto VehicleEntity::getCurrentAction() const -> std::string { - if (not npc_logic_started_) { - return "waiting"; - } else { - return behavior_plugin_ptr_->getCurrentAction(); - } + return behavior_plugin_ptr_->getCurrentAction(); } auto VehicleEntity::getDefaultDynamicConstraints() const @@ -87,13 +83,6 @@ auto VehicleEntity::getBehaviorParameter() const -> traffic_simulator_msgs::msg: return behavior_plugin_ptr_->getBehaviorParameter(); } -auto VehicleEntity::getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & -{ - static traffic_simulator_msgs::msg::EntityType type; - type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; - return type; -} - auto VehicleEntity::getEntityTypename() const -> const std::string & { static const std::string result = "VehicleEntity"; @@ -112,7 +101,7 @@ auto VehicleEntity::getObstacle() -> std::optional lanelet::Ids { - if (const auto canonicalized_lanelet_pose = status_.getCanonicalizedLaneletPose()) { + if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) { return route_planner_.getRouteLanelets(canonicalized_lanelet_pose.value(), horizon); } else { return {}; @@ -121,13 +110,10 @@ auto VehicleEntity::getRouteLanelets(double horizon) -> lanelet::Ids auto VehicleEntity::getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray { - if (!npc_logic_started_) { - return traffic_simulator_msgs::msg::WaypointsArray(); - } try { return behavior_plugin_ptr_->getWaypoints(); } catch (const std::runtime_error & e) { - if (not status_.laneMatchingSucceed()) { + if (not status_->laneMatchingSucceed()) { THROW_SIMULATION_ERROR( "Failed to calculate waypoints in NPC logics, please check Entity : ", name, " is in a lane coordinate."); @@ -137,51 +123,48 @@ auto VehicleEntity::getWaypoints() -> const traffic_simulator_msgs::msg::Waypoin } } -void VehicleEntity::onUpdate(double current_time, double step_time) +auto VehicleEntity::onUpdate(const double current_time, const double step_time) -> void { EntityBase::onUpdate(current_time, step_time); - if (npc_logic_started_) { - behavior_plugin_ptr_->setOtherEntityStatus(other_status_); - behavior_plugin_ptr_->setEntityStatus(std::make_unique(status_)); - behavior_plugin_ptr_->setTargetSpeed(target_speed_); - auto route_lanelets = getRouteLanelets(); - behavior_plugin_ptr_->setRouteLanelets(route_lanelets); - - // recalculate spline only when input data changes - if (previous_route_lanelets_ != route_lanelets) { - previous_route_lanelets_ = route_lanelets; - try { - spline_ = std::make_shared( - hdmap_utils_ptr_->getCenterPoints(route_lanelets)); - } catch (const common::scenario_simulator_exception::SemanticError & error) { - // reset the ptr when spline cannot be calculated - spline_.reset(); - } + + behavior_plugin_ptr_->setOtherEntityStatus(other_status_); + behavior_plugin_ptr_->setCanonicalizedEntityStatus(status_); + behavior_plugin_ptr_->setTargetSpeed(target_speed_); + auto route_lanelets = getRouteLanelets(); + behavior_plugin_ptr_->setRouteLanelets(route_lanelets); + + // recalculate spline only when input data changes + if (previous_route_lanelets_ != route_lanelets) { + previous_route_lanelets_ = route_lanelets; + try { + spline_ = std::make_shared( + hdmap_utils_ptr_->getCenterPoints(route_lanelets)); + } catch (const common::scenario_simulator_exception::SemanticError & error) { + // reset the ptr when spline cannot be calculated + spline_.reset(); } - behavior_plugin_ptr_->setReferenceTrajectory(spline_); - behavior_plugin_ptr_->update(current_time, step_time); - const auto status_updated = behavior_plugin_ptr_->getUpdatedStatus(); - if (const auto canonicalized_lanelet_pose = status_updated->getCanonicalizedLaneletPose()) { - if (isAtEndOfLanelets(canonicalized_lanelet_pose.value(), hdmap_utils_ptr_)) { - stopAtCurrentPosition(); - updateStandStillDuration(step_time); - updateTraveledDistance(step_time); - return; - } + } + behavior_plugin_ptr_->setReferenceTrajectory(spline_); + /// @note CanonicalizedEntityStatus is updated here, it is not skipped even if isAtEndOfLanelets return true + behavior_plugin_ptr_->update(current_time, step_time); + if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) { + if (pose::isAtEndOfLanelets(canonicalized_lanelet_pose.value(), hdmap_utils_ptr_)) { + stopAtCurrentPosition(); + updateStandStillDuration(step_time); + updateTraveledDistance(step_time); + return; } - setStatus(*status_updated); - updateStandStillDuration(step_time); - updateTraveledDistance(step_time); - } else { - updateEntityStatusTimestamp(current_time); } + updateStandStillDuration(step_time); + updateTraveledDistance(step_time); + EntityBase::onPostUpdate(current_time, step_time); } void VehicleEntity::requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) { behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); - if (status_.laneMatchingSucceed()) { + if (status_->laneMatchingSucceed()) { route_planner_.setWaypoints({lanelet_pose}); } behavior_plugin_ptr_->setGoalPoses({static_cast(lanelet_pose)}); @@ -191,8 +174,8 @@ void VehicleEntity::requestAcquirePosition(const geometry_msgs::msg::Pose & map_ { behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); if ( - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - map_pose, status_.getBoundingBox(), false, + const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + map_pose, status_->getBoundingBox(), false, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { requestAcquirePosition(canonicalized_lanelet_pose.value()); } else { @@ -219,8 +202,8 @@ void VehicleEntity::requestAssignRoute(const std::vector route; for (const auto & waypoint : waypoints) { if ( - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - waypoint, status_.getBoundingBox(), false, + const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + waypoint, status_->getBoundingBox(), false, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { route.emplace_back(canonicalized_lanelet_pose.value()); } else { @@ -238,8 +221,8 @@ auto VehicleEntity::requestFollowTrajectory( std::vector waypoints; for (const auto & vertex : parameter->shape.vertices) { if ( - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - vertex.position, status_.getBoundingBox(), false, + const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + vertex.position, status_->getBoundingBox(), false, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { waypoints.emplace_back(canonicalized_lanelet_pose.value()); } else { diff --git a/simulation/traffic_simulator/src/helper/helper.cpp b/simulation/traffic_simulator/src/helper/helper.cpp index db216173fdb..ccd672cdb94 100644 --- a/simulation/traffic_simulator/src/helper/helper.cpp +++ b/simulation/traffic_simulator/src/helper/helper.cpp @@ -55,7 +55,7 @@ auto constructCanonicalizedLaneletPose( const std::shared_ptr & hdmap_utils_ptr) -> CanonicalizedLaneletPose { if ( - auto canonicalized_lanelet_pose = canonicalize( + auto canonicalized_lanelet_pose = pose::canonicalize( traffic_simulator::helper::constructLaneletPose(lanelet_id, s, offset, roll, pitch, yaw), hdmap_utils_ptr)) { return canonicalized_lanelet_pose.value(); diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 7ebdb8710dd..cd8ab6d5a37 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -58,8 +58,8 @@ void TrafficController::autoSink() if (hdmap_utils_->getNextLaneletIds(lanelet_id).empty()) { LaneletPose lanelet_pose; lanelet_pose.lanelet_id = lanelet_id; - lanelet_pose.s = laneletLength(lanelet_id, hdmap_utils_); - const auto pose = toMapPose(lanelet_pose, hdmap_utils_); + lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_); + const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_); addModule( lanelet_id, 1, pose.position, get_entity_names_function, get_entity_pose_function, despawn_function); diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index d83391866a6..b6e8e8a9ff8 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -54,6 +54,7 @@ auto countLaneChanges( static_cast(from), static_cast(to), allow_lane_change); } +/// @sa https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/docs/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md auto longitudinalDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, bool include_adjacent_lanelet, bool include_opposite_direction, bool allow_lane_change, @@ -86,22 +87,19 @@ auto longitudinalDistance( return std::nullopt; } } else { - /// @todo here matching_distance should be passed - constexpr double matching_distance = 5.0; /** - * @brief hard coded parameter!! 5.0 is a matching distance of the toLaneletPoses function. - * A matching distance of about 1.5 lane widths is given as the matching distance to match the + * @brief A matching distance of about 1.5*lane widths is given as the matching distance to match the * Entity present on the adjacent Lanelet. + * The length of the horizontal bar must intersect with the adjacent lanelet, + * so it is always 10m regardless of the entity type. */ + constexpr double matching_distance = 5.0; + auto from_poses = hdmap_utils_ptr->toLaneletPoses( static_cast(from), static_cast(from).lanelet_id, matching_distance, include_opposite_direction); from_poses.emplace_back(from); - /** - * @brief hard coded parameter!! 5.0 is a matching distance of the toLaneletPoses function. - * A matching distance of about 1.5 lane widths is given as the matching distance to match the - * Entity present on the adjacent Lanelet. - */ + auto to_poses = hdmap_utils_ptr->toLaneletPoses( static_cast(to), static_cast(to).lanelet_id, matching_distance, include_opposite_direction); diff --git a/simulation/traffic_simulator/test/CMakeLists.txt b/simulation/traffic_simulator/test/CMakeLists.txt index 713c8b40678..32f62d79252 100644 --- a/simulation/traffic_simulator/test/CMakeLists.txt +++ b/simulation/traffic_simulator/test/CMakeLists.txt @@ -5,3 +5,4 @@ add_subdirectory(src/behavior) add_subdirectory(src/job) add_subdirectory(src/simulation_clock) add_subdirectory(src/hdmap_utils) +add_subdirectory(src/data_type) 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..6bbebabc879 --- /dev/null +++ b/simulation/traffic_simulator/test/src/data_type/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_lanelet_pose test_lanelet_pose.cpp) +target_link_libraries(test_lanelet_pose traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp b/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp new file mode 100644 index 00000000000..24d74f063ce --- /dev/null +++ b/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp @@ -0,0 +1,384 @@ +// 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 "../helper_functions.hpp" + +using CanonicalizedLaneletPose = traffic_simulator::lanelet_pose::CanonicalizedLaneletPose; + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +class CanonicalizedLaneletPoseTest : public testing::Test +{ +protected: + CanonicalizedLaneletPoseTest() : hdmap_utils(makeHdMapUtilsSharedPointer()) {} + + std::shared_ptr hdmap_utils; +}; + +/** + * @note Test constructor behavior with route_lanelets argument present when canonicalization function fails. + */ +TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withRoute_invalid) +{ + EXPECT_THROW( + CanonicalizedLaneletPose( + traffic_simulator::helper::constructLaneletPose(100000000000, 0.0, 0.0), lanelet::Ids{}, + hdmap_utils), + std::runtime_error); +} + +/** + * @note Test constructor behavior with route_lanelets argument present when canonicalization function succeeds + */ +TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withRoute) +{ + std::shared_ptr pose; + EXPECT_NO_THROW( + pose = std::make_shared( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), lanelet::Ids{120659}, + hdmap_utils)); + EXPECT_LANELET_POSE_EQ( + static_cast(*pose), + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); +} + +/** + * @note Test constructor behavior with route_lanelets argument absent when canonicalization function fails + */ +TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withoutRoute_invalid) +{ + EXPECT_THROW( + CanonicalizedLaneletPose( + traffic_simulator::helper::constructLaneletPose(100000000000, 0.0, 0.0), hdmap_utils), + std::runtime_error); +} + +/** + * @note Test constructor behavior with route_lanelets argument absent when canonicalization function succeeds + */ +TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withoutRoute) +{ + std::shared_ptr pose; + EXPECT_NO_THROW( + pose = std::make_shared( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils)); + EXPECT_LANELET_POSE_EQ( + static_cast(*pose), + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); +} + +/** + * @note Test copy constructor behavior + */ +TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_copyConstructor) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_copy(pose); + EXPECT_LANELET_POSE_EQ( + static_cast(pose), + static_cast(CanonicalizedLaneletPose(pose))); +} + +/** + * @note Test move constructor behavior + */ +TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_moveConstructor) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose2(pose); + const CanonicalizedLaneletPose pose_move = std::move(pose2); + EXPECT_LANELET_POSE_EQ( + static_cast(pose), + static_cast(pose_move)); +} + +/** + * @note Test copy assignment operator behavior + */ +TEST_F(CanonicalizedLaneletPoseTest, copyAssignment) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + CanonicalizedLaneletPose pose_assign( + traffic_simulator::helper::constructLaneletPose(34468, 0.0, 0.0), hdmap_utils); + + pose_assign = pose; + + EXPECT_LANELET_POSE_EQ( + static_cast(pose), + static_cast(pose_assign)); +} + +/** + * @note Test conversion operator behavior using static_cast + */ +TEST_F(CanonicalizedLaneletPoseTest, conversionLaneletPose) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const traffic_simulator::LaneletPose pose_casted = + static_cast(pose); + + EXPECT_EQ(pose_casted.lanelet_id, 120659); + EXPECT_DOUBLE_EQ(pose_casted.s, 0.0); + EXPECT_DOUBLE_EQ(pose_casted.offset, 0.0); + EXPECT_VECTOR3_EQ(pose_casted.rpy, geometry_msgs::msg::Vector3()); +} + +/** + * @note Test conversion operator behavior using static_cast + */ +TEST_F(CanonicalizedLaneletPoseTest, conversionPose) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + + const geometry_msgs::msg::Pose pose1 = + makePose(makePoint(3822.3815, 73784.9618, -1.761), makeQuaternionFromYaw(2.060578777273)); + + EXPECT_POSE_NEAR(static_cast(pose), pose1, 0.01); +} + +/** + * @note Test function behavior when alternative poses are present + */ +TEST_F(CanonicalizedLaneletPoseTest, hasAlternativeLaneletPose_true) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, -10.0, 0.0), hdmap_utils); + + EXPECT_TRUE(pose.hasAlternativeLaneletPose()); +} + +/** + * @note Test function behavior when alternative poses are absent + */ +TEST_F(CanonicalizedLaneletPoseTest, hasAlternativeLaneletPose_false) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 10.0, 0.0), hdmap_utils); + + EXPECT_FALSE(pose.hasAlternativeLaneletPose()); +} + +/** + * @note Test function behavior when there are no lanelet_poses + */ +TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRouteFrom_empty) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 20.0, 0.0), hdmap_utils); + const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); + const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); + + const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils); + const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils); + + ASSERT_TRUE(result1.has_value()); + ASSERT_TRUE(result2.has_value()); + + EXPECT_EQ(result1.value().lanelet_id, 120659); + EXPECT_EQ(result2.value().lanelet_id, 120659); +} + +/** + * @note Test function behavior when there is only one lanelet_pose + */ +TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRouteFrom_single) +{ + CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(34666, -20.0, 0.0), hdmap_utils); + const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); + const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); + + const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils); + const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils); + + ASSERT_TRUE(result1.has_value()); + ASSERT_TRUE(result2.has_value()); + + EXPECT_EQ(result1.value().lanelet_id, 34603); + EXPECT_EQ(result2.value().lanelet_id, 34603); +} + +/** + * @note Test function behavior when there are multiple lanelet_poses + */ +TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRouteFrom_multiple) +{ + CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, -20.0, 0.0), hdmap_utils); + const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); + const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); + + const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils); + const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils); + + ASSERT_TRUE(result1.has_value()); + ASSERT_TRUE(result2.has_value()); + + EXPECT_EQ(result1.value().lanelet_id, 34603); + EXPECT_EQ(result2.value().lanelet_id, 34579); +} + +/** + * @note Test accessor function + */ +TEST(CanonicalizedLaneletPose, getConsiderPoseByRoadSlope) +{ + EXPECT_EQ(CanonicalizedLaneletPose::getConsiderPoseByRoadSlope(), false); +} + +/** + * @note Test accessor function + */ +TEST(CanonicalizedLaneletPose, setConsiderPoseByRoadSlope) +{ + EXPECT_EQ(CanonicalizedLaneletPose::getConsiderPoseByRoadSlope(), false); + CanonicalizedLaneletPose::setConsiderPoseByRoadSlope(true); + EXPECT_EQ(CanonicalizedLaneletPose::getConsiderPoseByRoadSlope(), true); +} + +/** + * @note Test operator calculation correctness with CanonicalizedLaneletPose of lesser, equal and greater lanelet_id + */ +TEST_F(CanonicalizedLaneletPoseTest, operatorLessEqual) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_equal( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_less( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_greater( + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + + EXPECT_TRUE(pose_less <= pose); + EXPECT_TRUE(pose_equal <= pose); + EXPECT_FALSE(pose_greater <= pose); +} + +/** + * @note Test operator calculation correctness with CanonicalizedLaneletPose of lesser, equal and greater lanelet_id + */ +TEST_F(CanonicalizedLaneletPoseTest, operatorLess) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_equal( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_less( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_greater( + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + + EXPECT_TRUE(pose_less < pose); + EXPECT_FALSE(pose_equal < pose); + EXPECT_FALSE(pose_greater < pose); +} + +/** + * @note Test operator calculation correctness with CanonicalizedLaneletPose of lesser, equal and greater lanelet_id + */ +TEST_F(CanonicalizedLaneletPoseTest, operatorGreaterEqual) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_equal( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_less( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_greater( + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + + EXPECT_FALSE(pose_less >= pose); + EXPECT_TRUE(pose_equal >= pose); + EXPECT_TRUE(pose_greater >= pose); +} + +/** + * @note Test operator calculation correctness with CanonicalizedLaneletPose of lesser, equal and greater lanelet_id + */ +TEST_F(CanonicalizedLaneletPoseTest, operatorGreater) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_equal( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_less( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_greater( + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + + EXPECT_FALSE(pose_less > pose); + EXPECT_FALSE(pose_equal > pose); + EXPECT_TRUE(pose_greater > pose); +} + +/** + * @note Test function behavior when provided two poses occupying the same lanelet_id + */ +TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withPose_same) +{ + const CanonicalizedLaneletPose pose1( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose2( + traffic_simulator::helper::constructLaneletPose(120659, 1.0, 0.0), hdmap_utils); + + EXPECT_TRUE(traffic_simulator::isSameLaneletId(pose1, pose2)); +} + +/** + * @note Test function behavior when provided two poses occupying different lanelet_ids + */ +TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withPose_different) +{ + const CanonicalizedLaneletPose pose1( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + CanonicalizedLaneletPose pose2( + traffic_simulator::helper::constructLaneletPose(34606, 1.0, 0.0), hdmap_utils); + + EXPECT_FALSE(traffic_simulator::isSameLaneletId(pose1, pose2)); +} + +/** + * @note Test function behavior when provided with a pose having lanelet_id equal to the lanelet_id argument + */ +TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withLanelet_same) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + + EXPECT_TRUE(traffic_simulator::isSameLaneletId(pose, 120659)); +} + +/** + * @note Test function behavior when provided with a pose having lanelet_id different to the lanelet_id argument + */ +TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withLanelet_different) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + + EXPECT_FALSE(traffic_simulator::isSameLaneletId(pose, 34606)); +} 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 96bebd55008..4c0fc3d2b5f 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 @@ -77,11 +77,10 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, getCurrentAction_npcNotStarted) const auto blob = traffic_simulator::entity::MiscObjectEntity( entity_name, traffic_simulator::entity_status::CanonicalizedEntityStatus( - non_canonicalized_status, hdmap_utils_ptr), + non_canonicalized_status, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659)), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}); - EXPECT_FALSE(blob.isNpcLogicStarted()); - EXPECT_EQ(blob.getCurrentAction(), "waiting"); + EXPECT_EQ(blob.getCurrentAction(), "current_action_name"); } /** @@ -294,16 +293,6 @@ TEST_F(MiscObjectEntityTest_FullObject, get2DPolygon) EXPECT_POINT_EQ(polygon.at(4), ref_poly.at(4)); } -/** - * @note Test basic functionality; test whether the NPC logic is started correctly. - */ -TEST_F(MiscObjectEntityTest_FullObject, startNpcLogic) -{ - EXPECT_FALSE(misc_object.isNpcLogicStarted()); - misc_object.startNpcLogic(0.0); - EXPECT_TRUE(misc_object.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] @@ -454,23 +443,9 @@ TEST_F(MiscObjectEntityTest_FullObject, requestWalkStraight) * when NPC logic is started and velocity is greater than 0. */ TEST_F(MiscObjectEntityTest_FullObject, updateStandStillDuration_startedMoving) -{ - misc_object.startNpcLogic(0.0); - misc_object.setLinearVelocity(3.0); - - EXPECT_EQ(0.0, misc_object.updateStandStillDuration(0.1)); -} - -/** - * @note Test basic functionality; test updating stand still duration - * when NPC logic is not started. - */ -TEST_F(MiscObjectEntityTest_FullObject, updateStandStillDuration_notStarted) { misc_object.setLinearVelocity(3.0); - EXPECT_EQ(0.0, misc_object.updateStandStillDuration(0.1)); - misc_object.setLinearVelocity(0.0); EXPECT_EQ(0.0, misc_object.updateStandStillDuration(0.1)); } @@ -482,7 +457,6 @@ TEST_F(MiscObjectEntityTest_FullObject, updateTraveledDistance_startedMoving) { constexpr double velocity = 3.0; constexpr double step_time = 0.1; - misc_object.startNpcLogic(0.0); misc_object.setLinearVelocity(velocity); EXPECT_EQ(1.0 * step_time * velocity, misc_object.updateTraveledDistance(step_time)); @@ -491,19 +465,6 @@ TEST_F(MiscObjectEntityTest_FullObject, updateTraveledDistance_startedMoving) EXPECT_EQ(4.0 * step_time * velocity, misc_object.updateTraveledDistance(step_time)); } -/** - * @note Test basic functionality; test updating traveled distance correctness with NPC not started. - */ -TEST_F(MiscObjectEntityTest_FullObject, updateTraveledDistance_notStarted) -{ - constexpr double step_time = 0.1; - misc_object.setLinearVelocity(3.0); - EXPECT_EQ(0.0, misc_object.updateTraveledDistance(step_time)); - - misc_object.setLinearVelocity(0.0); - EXPECT_EQ(0.0, misc_object.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.). @@ -528,11 +489,9 @@ TEST_F( { EXPECT_FALSE(traffic_simulator::entity::MiscObjectEntity( entity_name, - traffic_simulator::CanonicalizedEntityStatus( - makeEntityStatus( - hdmap_utils_ptr, makePose(makePoint(3810.0, 73745.0)), makeBoundingBox(), 0.0, - entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), - hdmap_utils_ptr), + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makePose(makePoint(3810.0, 73745.0)), makeBoundingBox(), 1.0, + 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getCanonicalizedLaneletPose(5.0) .has_value()); @@ -548,13 +507,11 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, getCanonicalizedLaneletPose_onRoadAndCro EXPECT_TRUE( traffic_simulator::entity::MiscObjectEntity( entity_name, - traffic_simulator::CanonicalizedEntityStatus( - makeEntityStatus( - hdmap_utils_ptr, - makePose(makePoint(3766.1, 73738.2), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), - makeBoundingBox(), 0.0, entity_name, - traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), - hdmap_utils_ptr), + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, + makePose(makePoint(3766.1, 73738.2), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), + makeBoundingBox(), 1.0, 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getCanonicalizedLaneletPose(1.0) .has_value()); @@ -571,13 +528,11 @@ TEST_F( EXPECT_FALSE( traffic_simulator::entity::MiscObjectEntity( entity_name, - traffic_simulator::CanonicalizedEntityStatus( - makeEntityStatus( - hdmap_utils_ptr, - makePose(makePoint(3764.5, 73737.5), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), - makeBoundingBox(), 0.0, entity_name, - traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), - hdmap_utils_ptr), + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, + makePose(makePoint(3764.5, 73737.5), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), + makeBoundingBox(), 1.0, 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getCanonicalizedLaneletPose(1.0) .has_value()); diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index 4b68528b152..ba5f24b86c6 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -122,25 +122,31 @@ auto makeEntityStatus( auto makeCanonicalizedEntityStatus( std::shared_ptr hdmap_utils, - traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, + traffic_simulator::lanelet_pose::CanonicalizedLaneletPose canonicalized_lanelet_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); + makeEntityStatus(hdmap_utils, canonicalized_lanelet_pose, bbox, speed, name, type), + canonicalized_lanelet_pose); } 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", + traffic_simulator_msgs::msg::BoundingBox bbox, const double matching_distance = 1.0, + 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 { + const auto include_crosswalk = + (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == type || + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == type); + const auto canonicalized_lanelet_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( + pose, bbox, include_crosswalk, matching_distance, hdmap_utils); return traffic_simulator::entity_status::CanonicalizedEntityStatus( - makeEntityStatus(hdmap_utils, pose, bbox, speed, name, type), hdmap_utils); + makeEntityStatus(hdmap_utils, pose, bbox, speed, name, type), canonicalized_lanelet_pose); } #endif // TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 61a5fd96f0e..8110567b39d 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,115 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 2e37ee59a73..7e5ea839962 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 3.5.5 + 4.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 a913aa6c3e4..a2314170a07 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,125 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge pull request `#1358 `_ from tier4/RJD-1056-remove-npc-logic-started + Remove unused data members: npc_logic_started +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Trailing return type +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Forward isNpcLogicStarted to API and restore TestExecutor +* Remove npc_logic_started from API +* Contributors: DMoszynski, Dawid Moszynski, Masaya Kataoka, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge pull request `#1320 `_ from tier4/ref/RJD-1053-set-update-canonicalized-entity-status + ref(behavior_tree, traffic_simulator): move responsibility for canonicalization to traffic_simulator, simplify +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* feat(traffic_simulator): use CanonicalizedEntityStatus only with single constructor +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* tmp +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge branch 'master' into fix/distance-with-lane-change diff --git a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index 08314d0b5a9..a33b62fb37c 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -156,14 +156,14 @@ class TestExecutor }); } - void update() + auto update() -> void { executeWithErrorHandling([this]() { - if (!api_->isEgoSpawned() && !api_->isNpcLogicStarted()) { + if (not api_->isEgoSpawned() and not api_->isNpcLogicStarted()) { api_->startNpcLogic(); } if ( - api_->isEgoSpawned() && !api_->isNpcLogicStarted() && + api_->isEgoSpawned() and not api_->isNpcLogicStarted() and api_->asFieldOperatorApplication(api_->getEgoName()).engageable()) { api_->startNpcLogic(); } diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 05192a782a7..da86fefa1a6 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 3.5.5 + 4.2.2 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/random_test_runner/test/test_test_executor.cpp b/test_runner/random_test_runner/test/test_test_executor.cpp index 4088fff42e4..04fcacd9ac4 100644 --- a/test_runner/random_test_runner/test/test_test_executor.cpp +++ b/test_runner/random_test_runner/test/test_test_executor.cpp @@ -129,9 +129,8 @@ class MockTrafficSimulatorAPI traffic_simulator::CanonicalizedEntityStatus getEntityStatus(const std::string & name) { getEntityStatusMock(name); - entity_status_.lanelet_pose_valid = false; - std::shared_ptr hd_map_utils_nullptr = nullptr; - return traffic_simulator::CanonicalizedEntityStatus(entity_status_, hd_map_utils_nullptr); + /// @note set invalid LaneletPose so pass std::nullopt + return traffic_simulator::CanonicalizedEntityStatus(entity_status_, std::nullopt); } void setEntityStatusNecessaryValues( diff --git a/test_runner/random_test_runner/test/test_utils.hpp b/test_runner/random_test_runner/test/test_utils.hpp index deadfd0053b..a1c69241dd8 100644 --- a/test_runner/random_test_runner/test/test_utils.hpp +++ b/test_runner/random_test_runner/test/test_utils.hpp @@ -46,8 +46,8 @@ traffic_simulator::CanonicalizedEntityStatus getCanonicalizedEntityStatus( entity_status.pose.position.x = x; entity_status.pose.position.y = y; entity_status.pose.position.z = z; - entity_status.lanelet_pose_valid = false; - return traffic_simulator::CanonicalizedEntityStatus(entity_status, hdmap_utils_ptr); + /// @note set invalid LaneletPose so pass std::nullopt + return traffic_simulator::CanonicalizedEntityStatus(entity_status, std::nullopt); } inline traffic_simulator_msgs::msg::LaneletPose makeLaneletPose( diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 01a260b2f93..34bfae961dc 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,127 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ +* Merge pull request `#1362 `_ from tier4/feature/ros2-parameter-forwarding +* Move function `make_vehicle_parameters` into function `make_parameters` +* Move parameter `use_sim_time` into function `make_parameters` +* Simplify collection of `autoware.` prefixed parameters +* Remove data member `traffic_simulator::Configuration::rviz_config_path` +* Add feature to forward parameters prefixed with `autoware.` to Autoware +* Contributors: Kotaro Yoshimoto, yamacir-kit + +4.1.1 (2024-09-03) +------------------ +* Merge pull request `#1207 `_ from tier4/fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* feat(use_sim_time): set default as false +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Revert "feat(params): set use_sim_time default as True" + This reverts commit da85edf4956083563715daa3d60f0da1f94a423d. +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* feat(params): set use_sim_time default as True +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + 3.5.5 (2024-08-27) ------------------ * Merge pull request `#1348 `_ from tier4/fix/distance-with-lane-change 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 4d0b80e55f3..6ebccaa9c89 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 @@ -58,6 +58,10 @@ def default_autoware_launch_file_of(architecture_type): }[architecture_type] +def default_rviz_config_file(): + return Path(get_package_share_directory("traffic_simulator")) / "config/scenario_simulator_v2.rviz" + + def launch_setup(context, *args, **kwargs): # fmt: off architecture_type = LaunchConfiguration("architecture_type", default="awf/universe/20230906") @@ -77,7 +81,7 @@ def launch_setup(context, *args, **kwargs): port = LaunchConfiguration("port", default=5555) publish_empty_context = LaunchConfiguration("publish_empty_context", default=False) record = LaunchConfiguration("record", default=True) - rviz_config = LaunchConfiguration("rviz_config", default="") + rviz_config = LaunchConfiguration("rviz_config", default=default_rviz_config_file()) scenario = LaunchConfiguration("scenario", default=Path("/dev/null")) sensor_model = LaunchConfiguration("sensor_model", default="") sigterm_timeout = LaunchConfiguration("sigterm_timeout", default=8) @@ -129,22 +133,29 @@ def make_parameters(): {"rviz_config": rviz_config}, {"sensor_model": sensor_model}, {"sigterm_timeout": sigterm_timeout}, + {"use_sim_time": use_sim_time}, {"vehicle_model": vehicle_model}, ] - parameters += make_vehicle_parameters() - return parameters - def make_vehicle_parameters(): - parameters = [] + def collect_vehicle_parameters(): + if vehicle_model_name := vehicle_model.perform(context): + description = get_package_share_directory(vehicle_model_name + "_description") + return [ + description + "/config/vehicle_info.param.yaml", + description + "/config/simulator_model.param.yaml", + ] + else: + return [] + + if (it := collect_vehicle_parameters()) != []: + parameters += it - def description(): - return get_package_share_directory( - vehicle_model.perform(context) + "_description" - ) + def collect_prefixed_parameters(): + return [item[0][9:] + ':=' + item[1] for item in context.launch_configurations.items() if item[0][:9] == 'autoware.'] + + if (it := collect_prefixed_parameters()) != []: + parameters += [{"autoware.": it}] - if vehicle_model.perform(context): - parameters.append(description() + "/config/vehicle_info.param.yaml") - parameters.append(description() + "/config/simulator_model.param.yaml") return parameters return [ @@ -192,7 +203,7 @@ def description(): namespace="simulation", output="screen", on_exit=ShutdownOnce(), - parameters=make_parameters() + [{"use_sim_time": True}], + parameters=make_parameters(), condition=IfCondition(launch_simple_sensor_simulator), ), # The `name` keyword overrides the name for all created nodes, so duplicated nodes appear. @@ -205,7 +216,7 @@ def description(): executable="openscenario_interpreter_node", namespace="simulation", output="screen", - parameters=[{"use_sim_time": use_sim_time}]+make_parameters(), + parameters=make_parameters(), prefix=make_launch_prefix(), on_exit=ShutdownOnce(), ), @@ -229,13 +240,7 @@ def description(): name="rviz2", output={"stderr": "log", "stdout": "log"}, condition=IfCondition(launch_rviz), - arguments=[ - "-d", - str( - Path(get_package_share_directory("traffic_simulator")) - / "config/scenario_simulator_v2.rviz" - ), - ], + arguments=["-d", str(default_rviz_config_file())], ), ] diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 803393bef42..991162b345d 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 3.5.5 + 4.2.2 scenario test runner package Tatsuya Yamasaki Apache License 2.0