From 1abe1bda84869f381d0d3b6f5b64e8e7bd5636cc Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 21 May 2024 15:05:48 +0900 Subject: [PATCH 001/149] Add new struct `TimeToCollisionCondition` Signed-off-by: yamacir-kit --- .../syntax/time_to_collision_condition.hpp | 126 ++++++++++++++++++ .../src/syntax/entity_condition.cpp | 3 +- 2 files changed, 128 insertions(+), 1 deletion(-) create mode 100644 openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp new file mode 100644 index 00000000000..aff3deeec54 --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -0,0 +1,126 @@ +// 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 OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_HPP_ + +#include +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +/* + TimeToCollisionCondition + + The currently predicted time to collision of a triggering entity/entities + and either a reference entity or an explicit position is compared to a given + time value. Time to collision is calculated as the distance divided by the + relative speed. Acceleration of entities is not taken into consideration. + For the relative speed calculation the same coordinate system and relative + distance type applies as for the distance calculation. If the relative speed + is negative, which means the entity is moving away from the position / the + entities are moving away from each other, then the time to collision cannot + be predicted and the condition evaluates to false. The logical operator for + comparison is defined by the rule attribute. The property "alongRoute" is + deprecated. If "coordinateSystem" or "relativeDistanceType" are set, + "alongRoute" is ignored. + + XSD 1.3 Representation + + + + + + + + + deprecated + + + + + + + + + + +*/ +struct TimeToCollisionCondition : private SimulatorCore::ConditionEvaluation +{ + const Boolean freespace; + + const Rule rule; + + const Double value; + + const RelativeDistanceType relative_distance_type; + + const CoordinateSystem coordinate_system; + + const RoutingAlgorithm routing_algorithm; + + const TriggeringEntities triggering_entities; + + std::vector evaluations; + + explicit TimeToCollisionCondition( + const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) + : freespace(readAttribute("freespace", node, scope)), + rule(readAttribute("rule", node, scope)), + value(readAttribute("value", node, scope)), + relative_distance_type( + readAttribute("relativeDistanceType", node, scope)), + coordinate_system( + readAttribute("coordinateSystem", node, scope, CoordinateSystem::entity)), + routing_algorithm(readAttribute( + "routingAlgorithm", node, scope, RoutingAlgorithm::undefined)), + triggering_entities(triggering_entities), + evaluations(triggering_entities.entity_refs.size(), Double::nan()) + { + } + + auto description() const -> String + { + auto description = std::stringstream(); + + description << triggering_entities.description() << "'s time-to-collision to given entity " + << "TODO-RELATIVE-DISTANCE-TARGET" + << " = "; + + print_to(description, evaluations); + + description << " " << rule << " " << value << "?"; + + return description.str(); + } + + auto evaluate() + { + evaluations.clear(); + + return asBoolean(triggering_entities.apply([this](auto && triggering_entity) { + [[deprecated]] auto evaluateTimeToCollision = [](auto &&...) { return Double(); }; + evaluations.push_back(evaluateTimeToCollision(triggering_entity, "TODO")); + return std::invoke(rule, evaluations.back(), value); + })); + } +}; +} // namespace syntax +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_HPP_ diff --git a/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp index 9577b8f9453..a0b9aca1a00 100644 --- a/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace openscenario_interpreter { @@ -36,7 +37,7 @@ EntityCondition::EntityCondition( std::make_pair( "CollisionCondition", [&](const auto & node) { return make< CollisionCondition>(node, scope, triggering_entities); }), std::make_pair( "OffroadCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), std::make_pair( "TimeHeadwayCondition", [&](const auto & node) { return make< TimeHeadwayCondition>(node, scope, triggering_entities); }), - std::make_pair( "TimeToCollisionCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), + std::make_pair( "TimeToCollisionCondition", [&](const auto & node) { return make< TimeToCollisionCondition>(node, scope, triggering_entities); }), std::make_pair( "AccelerationCondition", [&](const auto & node) { return make< AccelerationCondition>(node, scope, triggering_entities); }), std::make_pair( "StandStillCondition", [&](const auto & node) { return make< StandStillCondition>(node, scope, triggering_entities); }), std::make_pair( "SpeedCondition", [&](const auto & node) { return make< SpeedCondition>(node, scope, triggering_entities); }), From 3a6c2b9a4981fb311fdd6314d38f7f723a4bdc6f Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 21 May 2024 15:26:31 +0900 Subject: [PATCH 002/149] Add new struct `TimeToCollisionConditionTarget` Signed-off-by: yamacir-kit --- .../syntax/time_to_collision_condition.hpp | 11 ++-- .../time_to_collision_condition_target.hpp | 54 +++++++++++++++++++ 2 files changed, 61 insertions(+), 4 deletions(-) create mode 100644 openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index aff3deeec54..6bb98167f53 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -17,6 +17,7 @@ #include #include +#include #include namespace openscenario_interpreter @@ -24,7 +25,7 @@ namespace openscenario_interpreter inline namespace syntax { /* - TimeToCollisionCondition + TimeToCollisionCondition 1.3 The currently predicted time to collision of a triggering entity/entities and either a reference entity or an explicit position is compared to a given @@ -39,8 +40,6 @@ inline namespace syntax deprecated. If "coordinateSystem" or "relativeDistanceType" are set, "alongRoute" is ignored. - XSD 1.3 Representation - @@ -62,6 +61,8 @@ inline namespace syntax */ struct TimeToCollisionCondition : private SimulatorCore::ConditionEvaluation { + const TimeToCollisionConditionTarget time_to_collision_condition_target; + const Boolean freespace; const Rule rule; @@ -80,7 +81,9 @@ struct TimeToCollisionCondition : private SimulatorCore::ConditionEvaluation explicit TimeToCollisionCondition( const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) - : freespace(readAttribute("freespace", node, scope)), + : time_to_collision_condition_target( + readElement("TimeToCollisionConditionTarget", node, scope)), + freespace(readAttribute("freespace", node, scope)), rule(readAttribute("rule", node, scope)), value(readAttribute("value", node, scope)), relative_distance_type( diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp new file mode 100644 index 00000000000..960a87e9660 --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp @@ -0,0 +1,54 @@ +// 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 OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_TARGET_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_TARGET_HPP_ + +#include +#include +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +/* + TimeToCollisionConditionTarget 1.3 + + Target position used in the TimeToCollisionCondition. Can be defined as + either an explicit position, or the position of a reference entity. + + + + + + + +*/ +struct TimeToCollisionConditionTarget : public ComplexType +{ + explicit TimeToCollisionConditionTarget(const pugi::xml_node & node, Scope & scope) + // clang-format off + : ComplexType(choice(node, + std::make_pair( "Position", [&](auto && node) { return make< Position>(std::forward(node), scope); }), + std::make_pair("EntityRef", [&](auto && node) { return make(std::forward(node), scope); }))) + // clang-format on + { + } +}; +} // namespace syntax +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_TARGET_HPP_ From 5447363cde632787022eb475ff1c4b8840357ce2 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 21 May 2024 16:16:53 +0900 Subject: [PATCH 003/149] Add new structs `RelativeSpeedCondition` and `DirectionalDimension` Signed-off-by: yamacir-kit --- .../syntax/directional_dimension.hpp | 108 ++++++++++++++++++ .../syntax/relative_speed_condition.hpp | 96 ++++++++++++++++ .../syntax/time_to_collision_condition.hpp | 6 +- .../src/syntax/entity_condition.cpp | 3 +- 4 files changed, 209 insertions(+), 4 deletions(-) create mode 100644 openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/directional_dimension.hpp create mode 100644 openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/directional_dimension.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/directional_dimension.hpp new file mode 100644 index 00000000000..6bafc12b920 --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/directional_dimension.hpp @@ -0,0 +1,108 @@ +// 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 OPENSCENARIO_INTERPRETER__SYNTAX__DIRECTIONAL_DIMENSION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__DIRECTIONAL_DIMENSION_HPP_ + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +/* + DirectionalDimension (OpenSCENARIO XML 1.3) + + Defines the directions in the entity coordinate system. + + + + + + + + + + + + + + + +*/ +struct DirectionalDimension +{ + enum value_type { + /* + Longitudinal direction (along the x-axis). + */ + longitudinal, // NOTE: DEFAULT VALUE + + /* + Lateral direction (along the y-axis). + */ + lateral, + + /* + Vertical direction (along the z-axis). + */ + vertical, + }; + + value_type value; + + DirectionalDimension() = default; + + constexpr DirectionalDimension(value_type value) : value(value) {} + + constexpr operator value_type() const noexcept { return value; } + + friend auto operator>>(std::istream & istream, DirectionalDimension & datum) -> std::istream & + { + if (auto token = std::string(); istream >> token) { + if (token == "longitudinal") { + datum.value = DirectionalDimension::longitudinal; + return istream; + } else if (token == "lateral") { + datum.value = DirectionalDimension::lateral; + return istream; + } else if (token == "vertical") { + datum.value = DirectionalDimension::vertical; + return istream; + } else { + throw UNEXPECTED_ENUMERATION_VALUE_SPECIFIED(DirectionalDimension, token); + } + } else { + datum.value = DirectionalDimension::value_type(); + return istream; + } + } + + friend auto operator<<(std::ostream & ostream, const DirectionalDimension & datum) + -> std::ostream & + { + switch (datum) { + case DirectionalDimension::longitudinal: + return ostream << "longitudinal"; + case DirectionalDimension::lateral: + return ostream << "lateral"; + case DirectionalDimension::vertical: + return ostream << "vertical"; + default: + throw UNEXPECTED_ENUMERATION_VALUE_ASSIGNED(DirectionalDimension, datum); + } + } +}; +} // namespace syntax +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__DIRECTIONAL_DIMENSION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp new file mode 100644 index 00000000000..96d6223e9db --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp @@ -0,0 +1,96 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_ + +#include +#include +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +/* + RelativeSpeedCondition 1.3 + + The current relative speed of a triggering entity/entities to a reference + entity is compared to a given value. The logical operator used for the + evaluation is defined by the rule attribute. If direction is used, only the + projection to that direction is used in the comparison, with the triggering + entity/entities as the reference. + + + + + + + +*/ +struct RelativeSpeedCondition : private SimulatorCore::ConditionEvaluation +{ + const EntityRef entity_ref; + + const Rule rule; + + const Double value; + + const DirectionalDimension direction; + + const TriggeringEntities triggering_entities; + + std::vector evaluations; + + explicit RelativeSpeedCondition( + const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) + : entity_ref(readAttribute("entityRef", node, scope)), + rule(readAttribute("rule", node, scope)), + value(readAttribute("value", node, scope)), + direction(readAttribute("direction", node, scope)), + triggering_entities(triggering_entities), + evaluations(triggering_entities.entity_refs.size(), Double::nan()) + { + } + + auto description() const + { + auto description = std::stringstream(); + + description << triggering_entities.description() << "'s relative speed to given entity " + << entity_ref << " = "; + + print_to(description, evaluations); + + description << " " << rule << " " << value << "?"; + + return description.str(); + } + + auto evaluate() + { + evaluations.clear(); + + return asBoolean(triggering_entities.apply([this](auto && triggering_entity) { + [[deprecated]] auto evaluateRelativeSpeed = [](auto &&...) { return Double(); }; + evaluations.push_back(evaluateRelativeSpeed(triggering_entity, entity_ref)); + return std::invoke(rule, evaluations.back(), value); + })); + } +}; +} // namespace syntax +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index 6bb98167f53..2e8eee42cb2 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -25,7 +25,7 @@ namespace openscenario_interpreter inline namespace syntax { /* - TimeToCollisionCondition 1.3 + TimeToCollisionCondition (OpenSCENARIO XML 1.3) The currently predicted time to collision of a triggering entity/entities and either a reference entity or an explicit position is compared to a given @@ -97,11 +97,11 @@ struct TimeToCollisionCondition : private SimulatorCore::ConditionEvaluation { } - auto description() const -> String + auto description() const { auto description = std::stringstream(); - description << triggering_entities.description() << "'s time-to-collision to given entity " + description << triggering_entities.description() << "'s time to collision to given entity " << "TODO-RELATIVE-DISTANCE-TARGET" << " = "; diff --git a/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp index a0b9aca1a00..bf25305d994 100644 --- a/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -41,7 +42,7 @@ EntityCondition::EntityCondition( std::make_pair( "AccelerationCondition", [&](const auto & node) { return make< AccelerationCondition>(node, scope, triggering_entities); }), std::make_pair( "StandStillCondition", [&](const auto & node) { return make< StandStillCondition>(node, scope, triggering_entities); }), std::make_pair( "SpeedCondition", [&](const auto & node) { return make< SpeedCondition>(node, scope, triggering_entities); }), - std::make_pair( "RelativeSpeedCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), + std::make_pair( "RelativeSpeedCondition", [&](const auto & node) { return make< RelativeSpeedCondition>(node, scope, triggering_entities); }), std::make_pair("TraveledDistanceCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), std::make_pair( "ReachPositionCondition", [&](const auto & node) { return make< ReachPositionCondition>(node, scope, triggering_entities); }), std::make_pair( "DistanceCondition", [&](const auto & node) { return make< DistanceCondition>(node, scope, triggering_entities); }), From b09ca1fc3ce9a855ef46c7309419fc5700573e90 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 21 May 2024 18:04:41 +0900 Subject: [PATCH 004/149] Add static member function `ConditionEvaluation::evaluateRelativeSpeed` Signed-off-by: yamacir-kit --- .../reader/attribute.hpp | 11 +++++ .../simulator_core.hpp | 20 ++++++--- .../syntax/relative_speed_condition.hpp | 45 ++++++++++++++++--- 3 files changed, 64 insertions(+), 12 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp index 06568802cbe..945e1a346a3 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -164,6 +165,16 @@ auto readAttribute(const std::string & name, const Node & node, const Scope & sc return value; } } + +template +auto readAttribute(const std::string & name, const Node & node, const Scope & scope, std::nullopt_t) +{ + if (node.attribute(name.c_str())) { + return std::make_optional(readAttribute(name, node, scope)); + } else { + return std::optional(); + } +} } // namespace reader } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index ed80b3e3879..79f5f7239d6 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -15,6 +15,7 @@ #ifndef OPENSCENARIO_INTERPRETER__SIMULATOR_CORE_HPP_ #define OPENSCENARIO_INTERPRETER__SIMULATOR_CORE_HPP_ +#include #include #include #include @@ -482,12 +483,6 @@ class SimulatorCore return core->getCurrentAccel(std::forward(xs)...).linear.x; } - template - static auto evaluateCollisionCondition(Ts &&... xs) -> bool - { - return core->checkCollision(std::forward(xs)...); - } - template static auto evaluateBoundingBoxEuclideanDistance(Ts &&... xs) // for RelativeDistanceCondition { @@ -500,6 +495,19 @@ class SimulatorCore } } + template + static auto evaluateCollisionCondition(Ts &&... xs) -> bool + { + return core->checkCollision(std::forward(xs)...); + } + + template + static auto evaluateRelativeSpeed(const T & x, const U & y) + { + using math::geometry::operator-; + return core->getCurrentTwist(x).linear - core->getCurrentTwist(y).linear; + } + template static auto evaluateSimulationTime(Ts &&... xs) -> double { diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp index 96d6223e9db..d535388c5dc 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp @@ -15,6 +15,7 @@ #ifndef OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_ #define OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_ +#include #include #include #include @@ -42,13 +43,26 @@ inline namespace syntax */ struct RelativeSpeedCondition : private SimulatorCore::ConditionEvaluation { + /* + Reference entity. + */ const EntityRef entity_ref; + /* + The operator (less, greater, equal). + */ const Rule rule; + /* + Relative speed value. Unit: [m/s]. Range: ]-inf..inf[. Relative speed is + defined as speed_rel = speed(triggering entity) - speed(reference entity) + */ const Double value; - const DirectionalDimension direction; + /* + Direction of the speed (if not given, the total speed is considered). + */ + const std::optional direction; const TriggeringEntities triggering_entities; @@ -59,7 +73,7 @@ struct RelativeSpeedCondition : private SimulatorCore::ConditionEvaluation : entity_ref(readAttribute("entityRef", node, scope)), rule(readAttribute("rule", node, scope)), value(readAttribute("value", node, scope)), - direction(readAttribute("direction", node, scope)), + direction(readAttribute("direction", node, scope, std::nullopt)), triggering_entities(triggering_entities), evaluations(triggering_entities.entity_refs.size(), Double::nan()) { @@ -69,8 +83,13 @@ struct RelativeSpeedCondition : private SimulatorCore::ConditionEvaluation { auto description = std::stringstream(); - description << triggering_entities.description() << "'s relative speed to given entity " - << entity_ref << " = "; + description << triggering_entities.description() << "'s relative "; + + if (direction) { + description << *direction << " "; + } + + description << "speed to given entity " << entity_ref << " = "; print_to(description, evaluations); @@ -84,8 +103,22 @@ struct RelativeSpeedCondition : private SimulatorCore::ConditionEvaluation evaluations.clear(); return asBoolean(triggering_entities.apply([this](auto && triggering_entity) { - [[deprecated]] auto evaluateRelativeSpeed = [](auto &&...) { return Double(); }; - evaluations.push_back(evaluateRelativeSpeed(triggering_entity, entity_ref)); + evaluations.push_back([this](auto && v) { + if (direction) { + switch (*direction) { + case DirectionalDimension::longitudinal: + return v.x; + case DirectionalDimension::lateral: + return v.y; + case DirectionalDimension::vertical: + return v.z; + default: + return math::geometry::norm(v); + } + } else { + return math::geometry::norm(v); + } + }(evaluateRelativeSpeed(triggering_entity, entity_ref))); return std::invoke(rule, evaluations.back(), value); })); } From d270cdcaf0d100b0949aff869756959029fa8c72 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 22 May 2024 16:58:51 +0900 Subject: [PATCH 005/149] Move entity existence check into `distance` from speceialized `distance` Signed-off-by: yamacir-kit --- .../syntax/relative_distance_condition.cpp | 385 +++++++----------- 1 file changed, 157 insertions(+), 228 deletions(-) diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index 6309d00bbb3..56d28b7388d 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -66,31 +66,20 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { - return std::abs(makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.x); - } else { - return Double::nan(); - } + return std::abs(makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.x); } -/** - * @note This implementation differs from the OpenSCENARIO standard. See the section "6.4. Distances" in the OpenSCENARIO User Guide. - */ template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { - return std::abs( - makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.x); - } else { - return Double::nan(); - } + /** + @note This implementation differs from the OpenSCENARIO standard. See the + section "6.4. Distances" in the OpenSCENARIO User Guide. + */ + return std::abs( + makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.x); } template <> @@ -98,35 +87,24 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { - return std::abs(makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y); - } else { - return Double::nan(); - } + return std::abs(makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y); } -/** - * @note This implementation differs from the OpenSCENARIO standard. See the section "6.4. Distances" in the OpenSCENARIO User Guide. - */ template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { - return std::abs( - makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y); - } else { - return Double::nan(); - } + /** + @note This implementation differs from the OpenSCENARIO standard. See the + section "6.4. Distances" in the OpenSCENARIO User Guide. + */ + return std::abs( + makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y); } -// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x,y,z) -static double hypot(const double x, const double y, const double z, const bool consider_z) +// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x, y, z) +auto hypot(double x, double y, double z, bool consider_z) { return consider_z ? std::hypot(x, y, z) : std::hypot(x, y); } @@ -136,17 +114,11 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, true>(const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { - return hypot( - makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.x, - makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y, - makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.z, - consider_z); - } else { - return Double::nan(); - } + return hypot( + makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.x, + makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y, + makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.z, + consider_z); } template <> @@ -154,16 +126,10 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, false>(const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { - return hypot( - makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.x, - makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y, - makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.z, consider_z); - } else { - return Double::nan(); - } + return hypot( + makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.x, + makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y, + makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.z, consider_z); } template <> @@ -171,27 +137,21 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(entity_ref).as().is_added and - global().entities->at(triggering_entity).as().is_added) { - /* - For historical reasons, signed distances are returned when - coordinateSystem == lane and relativeDistanceType == - longitudinal/lateral. The sign has been mainly used to determine the - front/back and left/right positional relationship (a negative value is - returned if the target entity is behind or to the right). - - This behavior violates the OpenSCENARIO standard. In the future, after - DistanceCondition and RelativeDistanceCondition of TIER IV's - OpenSCENARIO Interpreter support OpenSCENARIO 1.2 RoutingAlgorithm, this - behavior will be enabled only when routingAlgorithm == undefined. - */ - return static_cast( - makeNativeRelativeLanePosition(triggering_entity, entity_ref)) - .offset; - } else { - return Double::nan(); - } + /* + For historical reasons, signed distances are returned when + coordinateSystem == lane and relativeDistanceType == longitudinal/lateral. + The sign has been mainly used to determine the front/back and left/right + positional relationship (a negative value is returned if the target entity + is behind or to the right). + + This behavior violates the OpenSCENARIO standard. In the future, after + DistanceCondition and RelativeDistanceCondition of TIER IV's OpenSCENARIO + Interpreter support OpenSCENARIO 1.2 RoutingAlgorithm, this behavior will + be enabled only when routingAlgorithm == undefined. + */ + return static_cast( + makeNativeRelativeLanePosition(triggering_entity, entity_ref)) + .offset; } template <> @@ -199,27 +159,21 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(entity_ref).as().is_added and - global().entities->at(triggering_entity).as().is_added) { - /* - For historical reasons, signed distances are returned when - coordinateSystem == lane and relativeDistanceType == - longitudinal/lateral. The sign has been mainly used to determine the - front/back and left/right positional relationship (a negative value is - returned if the target entity is behind or to the right). - - This behavior violates the OpenSCENARIO standard. In the future, after - DistanceCondition and RelativeDistanceCondition of TIER IV's - OpenSCENARIO Interpreter support OpenSCENARIO 1.2 RoutingAlgorithm, this - behavior will be enabled only when routingAlgorithm == undefined. - */ - return static_cast( - makeNativeBoundingBoxRelativeLanePosition(triggering_entity, entity_ref)) - .offset; - } else { - return Double::nan(); - } + /* + For historical reasons, signed distances are returned when + coordinateSystem == lane and relativeDistanceType == longitudinal/lateral. + The sign has been mainly used to determine the front/back and left/right + positional relationship (a negative value is returned if the target entity + is behind or to the right). + + This behavior violates the OpenSCENARIO standard. In the future, after + DistanceCondition and RelativeDistanceCondition of TIER IV's OpenSCENARIO + Interpreter support OpenSCENARIO 1.2 RoutingAlgorithm, this behavior will + be enabled only when routingAlgorithm == undefined. + */ + return static_cast( + makeNativeBoundingBoxRelativeLanePosition(triggering_entity, entity_ref)) + .offset; } template <> @@ -227,27 +181,21 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { - /* - For historical reasons, signed distances are returned when - coordinateSystem == lane and relativeDistanceType == - longitudinal/lateral. The sign has been mainly used to determine the - front/back and left/right positional relationship (a negative value is - returned if the target entity is behind or to the right). - - This behavior violates the OpenSCENARIO standard. In the future, after - DistanceCondition and RelativeDistanceCondition of TIER IV's - OpenSCENARIO Interpreter support OpenSCENARIO 1.2 RoutingAlgorithm, this - behavior will be enabled only when routingAlgorithm == undefined. - */ - return static_cast( - makeNativeRelativeLanePosition(triggering_entity, entity_ref)) - .s; - } else { - return Double::nan(); - } + /* + For historical reasons, signed distances are returned when + coordinateSystem == lane and relativeDistanceType == longitudinal/lateral. + The sign has been mainly used to determine the front/back and left/right + positional relationship (a negative value is returned if the target entity + is behind or to the right). + + This behavior violates the OpenSCENARIO standard. In the future, after + DistanceCondition and RelativeDistanceCondition of TIER IV's OpenSCENARIO + Interpreter support OpenSCENARIO 1.2 RoutingAlgorithm, this behavior will + be enabled only when routingAlgorithm == undefined. + */ + return static_cast( + makeNativeRelativeLanePosition(triggering_entity, entity_ref)) + .s; } template <> @@ -255,27 +203,21 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { - /* - For historical reasons, signed distances are returned when - coordinateSystem == lane and relativeDistanceType == - longitudinal/lateral. The sign has been mainly used to determine the - front/back and left/right positional relationship (a negative value is - returned if the target entity is behind or to the right). - - This behavior violates the OpenSCENARIO standard. In the future, after - DistanceCondition and RelativeDistanceCondition of TIER IV's - OpenSCENARIO Interpreter support OpenSCENARIO 1.2 RoutingAlgorithm, this - behavior will be enabled only when routingAlgorithm == undefined. - */ - return static_cast( - makeNativeBoundingBoxRelativeLanePosition(triggering_entity, entity_ref)) - .s; - } else { - return Double::nan(); - } + /* + For historical reasons, signed distances are returned when + coordinateSystem == lane and relativeDistanceType == longitudinal/lateral. + The sign has been mainly used to determine the front/back and left/right + positional relationship (a negative value is returned if the target entity + is behind or to the right). + + This behavior violates the OpenSCENARIO standard. In the future, after + DistanceCondition and RelativeDistanceCondition of TIER IV's OpenSCENARIO + Interpreter support OpenSCENARIO 1.2 RoutingAlgorithm, this behavior will + be enabled only when routingAlgorithm == undefined. + */ + return static_cast( + makeNativeBoundingBoxRelativeLanePosition(triggering_entity, entity_ref)) + .s; } template <> @@ -283,16 +225,10 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, true>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(entity_ref).as().is_added and - global().entities->at(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, entity_ref, RoutingAlgorithm::shortest)) - .offset); - } else { - return Double::nan(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, entity_ref, RoutingAlgorithm::shortest)) + .offset); } template <> @@ -300,16 +236,10 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, false>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(entity_ref).as().is_added and - global().entities->at(triggering_entity).as().is_added) { - return std::abs( - static_cast( - makeNativeRelativeLanePosition(triggering_entity, entity_ref, RoutingAlgorithm::shortest)) - .offset); - } else { - return Double::nan(); - } + return std::abs( + static_cast( + makeNativeRelativeLanePosition(triggering_entity, entity_ref, RoutingAlgorithm::shortest)) + .offset); } template <> @@ -317,16 +247,10 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, true>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(entity_ref).as().is_added and - global().entities->at(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, entity_ref, RoutingAlgorithm::shortest)) - .s); - } else { - return Double::nan(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, entity_ref, RoutingAlgorithm::shortest)) + .s); } template <> @@ -334,64 +258,64 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, false>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(entity_ref).as().is_added and - global().entities->at(triggering_entity).as().is_added) { - return std::abs( - static_cast( - makeNativeRelativeLanePosition(triggering_entity, entity_ref, RoutingAlgorithm::shortest)) - .s); - } else { - return Double::nan(); - } + return std::abs( + static_cast( + makeNativeRelativeLanePosition(triggering_entity, entity_ref, RoutingAlgorithm::shortest)) + .s); } -#define SWITCH_COORDINATE_SYSTEM(FUNCTION, ...) \ - switch (coordinate_system) { \ - case CoordinateSystem::entity: \ - FUNCTION(__VA_ARGS__, CoordinateSystem::entity); \ - break; \ - case CoordinateSystem::lane: \ - FUNCTION(__VA_ARGS__, CoordinateSystem::lane); \ - break; \ - case CoordinateSystem::road: \ - FUNCTION(__VA_ARGS__, CoordinateSystem::road); \ - break; \ - case CoordinateSystem::trajectory: \ - FUNCTION(__VA_ARGS__, CoordinateSystem::trajectory); \ - break; \ +#define SWITCH_COORDINATE_SYSTEM(FUNCTION, ...) \ + switch (coordinate_system) { \ + case CoordinateSystem::entity: \ + FUNCTION(__VA_ARGS__, CoordinateSystem::entity); \ + break; \ + case CoordinateSystem::lane: \ + FUNCTION(__VA_ARGS__, CoordinateSystem::lane); \ + break; \ + case CoordinateSystem::road: \ + FUNCTION(__VA_ARGS__, CoordinateSystem::road); \ + break; \ + case CoordinateSystem::trajectory: \ + FUNCTION(__VA_ARGS__, CoordinateSystem::trajectory); \ + break; \ + default: \ + throw UNEXPECTED_ENUMERATION_VALUE_ASSIGNED(CoordinateSystem, coordinate_system); \ } -#define SWITCH_RELATIVE_DISTANCE_TYPE(FUNCTION, ...) \ - switch (relative_distance_type) { \ - case RelativeDistanceType::longitudinal: \ - FUNCTION(__VA_ARGS__, RelativeDistanceType::longitudinal); \ - break; \ - case RelativeDistanceType::lateral: \ - FUNCTION(__VA_ARGS__, RelativeDistanceType::lateral); \ - break; \ - case RelativeDistanceType::euclidianDistance: \ - FUNCTION(__VA_ARGS__, RelativeDistanceType::euclidianDistance); \ - break; \ +#define SWITCH_RELATIVE_DISTANCE_TYPE(FUNCTION, ...) \ + switch (relative_distance_type) { \ + case RelativeDistanceType::longitudinal: \ + FUNCTION(__VA_ARGS__, RelativeDistanceType::longitudinal); \ + break; \ + case RelativeDistanceType::lateral: \ + FUNCTION(__VA_ARGS__, RelativeDistanceType::lateral); \ + break; \ + case RelativeDistanceType::euclidianDistance: \ + FUNCTION(__VA_ARGS__, RelativeDistanceType::euclidianDistance); \ + break; \ + default: \ + throw UNEXPECTED_ENUMERATION_VALUE_ASSIGNED(RelativeDistanceType, relative_distance_type); \ } -#define SWITCH_ROUTING_ALGORITHM(FUNCTION, ...) \ - switch (routing_algorithm) { \ - case RoutingAlgorithm::assigned_route: \ - FUNCTION(__VA_ARGS__, RoutingAlgorithm::assigned_route); \ - break; \ - case RoutingAlgorithm::fastest: \ - FUNCTION(__VA_ARGS__, RoutingAlgorithm::fastest); \ - break; \ - case RoutingAlgorithm::least_intersections: \ - FUNCTION(__VA_ARGS__, RoutingAlgorithm::least_intersections); \ - break; \ - case RoutingAlgorithm::shortest: \ - FUNCTION(__VA_ARGS__, RoutingAlgorithm::shortest); \ - break; \ - case RoutingAlgorithm::undefined: \ - FUNCTION(__VA_ARGS__, RoutingAlgorithm::undefined); \ - break; \ +#define SWITCH_ROUTING_ALGORITHM(FUNCTION, ...) \ + switch (routing_algorithm) { \ + case RoutingAlgorithm::assigned_route: \ + FUNCTION(__VA_ARGS__, RoutingAlgorithm::assigned_route); \ + break; \ + case RoutingAlgorithm::fastest: \ + FUNCTION(__VA_ARGS__, RoutingAlgorithm::fastest); \ + break; \ + case RoutingAlgorithm::least_intersections: \ + FUNCTION(__VA_ARGS__, RoutingAlgorithm::least_intersections); \ + break; \ + case RoutingAlgorithm::shortest: \ + FUNCTION(__VA_ARGS__, RoutingAlgorithm::shortest); \ + break; \ + case RoutingAlgorithm::undefined: \ + FUNCTION(__VA_ARGS__, RoutingAlgorithm::undefined); \ + break; \ + default: \ + throw UNEXPECTED_ENUMERATION_VALUE_ASSIGNED(RoutingAlgorithm, routing_algorithm); \ } #define SWITCH_FREESPACE(FUNCTION, ...) \ @@ -401,9 +325,14 @@ auto RelativeDistanceCondition::distance< auto RelativeDistanceCondition::distance(const EntityRef & triggering_entity) -> double { - SWITCH_COORDINATE_SYSTEM( - SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); - return Double::nan(); + if ( + global().entities->at(triggering_entity).as().is_added and + global().entities->at(entity_ref).as().is_added) { + SWITCH_COORDINATE_SYSTEM( + SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); + } else { + return Double::nan(); + } } auto RelativeDistanceCondition::evaluate() -> Object From 3be8459641964857a2b9d89cc195957a2aed6023 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 22 May 2024 18:58:29 +0900 Subject: [PATCH 006/149] Update `RelativeDistanceCondition::distance` to static member function Signed-off-by: yamacir-kit --- .../syntax/relative_distance_condition.hpp | 36 +++++----- .../syntax/relative_distance_condition.cpp | 72 ++++++++++--------- 2 files changed, 55 insertions(+), 53 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp index 39926681b26..846eca5f40b 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp @@ -90,8 +90,6 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi std::vector results; // for description - const bool consider_z; - explicit RelativeDistanceCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); auto description() const -> String; @@ -99,12 +97,14 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi template < CoordinateSystem::value_type, RelativeDistanceType::value_type, RoutingAlgorithm::value_type, Boolean::value_type> - auto distance(const EntityRef &) -> double + static auto distance(const EntityRef &, const EntityRef &) -> double { throw SyntaxError(__FILE__, ":", __LINE__); } - auto distance(const EntityRef &) -> double; + static auto distance( + const EntityRef &, const EntityRef &, const Entities &, CoordinateSystem, RelativeDistanceType, + RoutingAlgorithm, bool) -> double; auto evaluate() -> Object; }; @@ -113,20 +113,20 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi // cspell: ignore euclidian // clang-format off -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; // clang-format on } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index 56d28b7388d..4fe56f1e425 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -38,12 +38,7 @@ RelativeDistanceCondition::RelativeDistanceCondition( rule(readAttribute("rule", node, scope)), value(readAttribute("value", node, scope)), triggering_entities(triggering_entities), - results(triggering_entities.entity_refs.size(), Double::nan()), - consider_z([]() { - rclcpp::Node node{"get_parameter", "simulation"}; - node.declare_parameter("consider_pose_by_road_slope", false); - return node.get_parameter("consider_pose_by_road_slope").as_bool(); - }()) + results(triggering_entities.entity_refs.size(), Double::nan()) { } @@ -64,7 +59,7 @@ auto RelativeDistanceCondition::description() const -> String template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { return std::abs(makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.x); } @@ -72,7 +67,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { /** @note This implementation differs from the OpenSCENARIO standard. See the @@ -85,7 +80,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { return std::abs(makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y); } @@ -93,7 +88,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { /** @note This implementation differs from the OpenSCENARIO standard. See the @@ -104,38 +99,40 @@ auto RelativeDistanceCondition::distance< } // @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x, y, z) -auto hypot(double x, double y, double z, bool consider_z) +auto hypot(double x, double y, double z) { + static auto consider_z = []() { + auto node = rclcpp::Node("get_parameter", "simulation"); + node.declare_parameter("consider_pose_by_road_slope", false); + return node.get_parameter("consider_pose_by_road_slope").as_bool(); + }(); + return consider_z ? std::hypot(x, y, z) : std::hypot(x, y); } template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, - true>(const EntityRef & triggering_entity) -> double + true>(const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { - return hypot( - makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.x, - makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y, - makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.z, - consider_z); + const auto relative_world = + makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref); + return hypot(relative_world.position.x, relative_world.position.y, relative_world.position.z); } template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, - false>(const EntityRef & triggering_entity) -> double + false>(const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { - return hypot( - makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.x, - makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y, - makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.z, consider_z); + const auto relative_world = makeNativeRelativeWorldPosition(triggering_entity, entity_ref); + return hypot(relative_world.position.x, relative_world.position.y, relative_world.position.z); } template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { /* For historical reasons, signed distances are returned when @@ -157,7 +154,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { /* For historical reasons, signed distances are returned when @@ -179,7 +176,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { /* For historical reasons, signed distances are returned when @@ -201,7 +198,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { /* For historical reasons, signed distances are returned when @@ -223,7 +220,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { return std::abs(static_cast( makeNativeBoundingBoxRelativeLanePosition( @@ -234,7 +231,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { return std::abs( static_cast( @@ -245,7 +242,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { return std::abs(static_cast( makeNativeBoundingBoxRelativeLanePosition( @@ -256,7 +253,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { return std::abs( static_cast( @@ -321,13 +318,16 @@ auto RelativeDistanceCondition::distance< #define SWITCH_FREESPACE(FUNCTION, ...) \ return freespace ? FUNCTION(__VA_ARGS__, true) : FUNCTION(__VA_ARGS__, false) -#define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity) +#define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity, entity_ref) -auto RelativeDistanceCondition::distance(const EntityRef & triggering_entity) -> double +auto RelativeDistanceCondition::distance( + const EntityRef & triggering_entity, const EntityRef & entity_ref, const Entities & entities, + CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, + RoutingAlgorithm routing_algorithm, bool freespace) -> double { if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { + entities.at(triggering_entity).as().is_added and + entities.at(entity_ref).as().is_added) { SWITCH_COORDINATE_SYSTEM( SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); } else { @@ -340,7 +340,9 @@ auto RelativeDistanceCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) { - results.push_back(distance(triggering_entity)); + results.push_back(distance( + triggering_entity, entity_ref, *global().entities, coordinate_system, relative_distance_type, + routing_algorithm, freespace)); return rule(static_cast(results.back()), value); })); } From 6535eebbe8d9a77a7aff6dc493b41ba994b5d0a1 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 3 Jun 2024 11:24:46 +0900 Subject: [PATCH 007/149] Add new static member function `RelativeSpeedCondition::evaluate` Signed-off-by: yamacir-kit --- .../include/geometry/vector3/norm.hpp | 1 + .../syntax/relative_distance_condition.hpp | 2 +- .../syntax/relative_speed_condition.hpp | 63 ++----------- .../syntax/time_to_collision_condition.hpp | 30 +++++- .../syntax/relative_distance_condition.cpp | 8 +- .../src/syntax/relative_speed_condition.cpp | 92 +++++++++++++++++++ 6 files changed, 133 insertions(+), 63 deletions(-) create mode 100644 openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp diff --git a/common/math/geometry/include/geometry/vector3/norm.hpp b/common/math/geometry/include/geometry/vector3/norm.hpp index 44fa57c9e62..6746c7ec7c6 100644 --- a/common/math/geometry/include/geometry/vector3/norm.hpp +++ b/common/math/geometry/include/geometry/vector3/norm.hpp @@ -15,6 +15,7 @@ #ifndef GEOMETRY__VECTOR3__NORM_HPP_ #define GEOMETRY__VECTOR3__NORM_HPP_ +#include #include namespace math diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp index 846eca5f40b..475f91ff739 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp @@ -103,7 +103,7 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi } static auto distance( - const EntityRef &, const EntityRef &, const Entities &, CoordinateSystem, RelativeDistanceType, + const EntityRef &, const EntityRef &, const Entities *, CoordinateSystem, RelativeDistanceType, RoutingAlgorithm, bool) -> double; auto evaluate() -> Object; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp index d535388c5dc..d42b50db0dd 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp @@ -15,11 +15,11 @@ #ifndef OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_ #define OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_ -#include #include #include #include -#include +#include +#include namespace openscenario_interpreter { @@ -41,7 +41,7 @@ inline namespace syntax */ -struct RelativeSpeedCondition : private SimulatorCore::ConditionEvaluation +struct RelativeSpeedCondition : private Scope, private SimulatorCore::ConditionEvaluation { /* Reference entity. @@ -68,60 +68,15 @@ struct RelativeSpeedCondition : private SimulatorCore::ConditionEvaluation std::vector evaluations; - explicit RelativeSpeedCondition( - const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) - : entity_ref(readAttribute("entityRef", node, scope)), - rule(readAttribute("rule", node, scope)), - value(readAttribute("value", node, scope)), - direction(readAttribute("direction", node, scope, std::nullopt)), - triggering_entities(triggering_entities), - evaluations(triggering_entities.entity_refs.size(), Double::nan()) - { - } + explicit RelativeSpeedCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); - auto description() const - { - auto description = std::stringstream(); + auto description() const -> String; - description << triggering_entities.description() << "'s relative "; + static auto evaluate( + const EntityRef &, const EntityRef &, const Entities *, + const std::optional &) -> double; - if (direction) { - description << *direction << " "; - } - - description << "speed to given entity " << entity_ref << " = "; - - print_to(description, evaluations); - - description << " " << rule << " " << value << "?"; - - return description.str(); - } - - auto evaluate() - { - evaluations.clear(); - - return asBoolean(triggering_entities.apply([this](auto && triggering_entity) { - evaluations.push_back([this](auto && v) { - if (direction) { - switch (*direction) { - case DirectionalDimension::longitudinal: - return v.x; - case DirectionalDimension::lateral: - return v.y; - case DirectionalDimension::vertical: - return v.z; - default: - return math::geometry::norm(v); - } - } else { - return math::geometry::norm(v); - } - }(evaluateRelativeSpeed(triggering_entity, entity_ref))); - return std::invoke(rule, evaluations.back(), value); - })); - } + auto evaluate() -> Object; }; } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index 2e8eee42cb2..39e7e776a1d 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -17,6 +17,8 @@ #include #include +#include +#include #include #include @@ -59,7 +61,7 @@ inline namespace syntax */ -struct TimeToCollisionCondition : private SimulatorCore::ConditionEvaluation +struct TimeToCollisionCondition : private Scope, private SimulatorCore::ConditionEvaluation { const TimeToCollisionConditionTarget time_to_collision_condition_target; @@ -81,7 +83,8 @@ struct TimeToCollisionCondition : private SimulatorCore::ConditionEvaluation explicit TimeToCollisionCondition( const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) - : time_to_collision_condition_target( + : Scope(scope), + time_to_collision_condition_target( readElement("TimeToCollisionConditionTarget", node, scope)), freespace(readAttribute("freespace", node, scope)), rule(readAttribute("rule", node, scope)), @@ -117,8 +120,27 @@ struct TimeToCollisionCondition : private SimulatorCore::ConditionEvaluation evaluations.clear(); return asBoolean(triggering_entities.apply([this](auto && triggering_entity) { - [[deprecated]] auto evaluateTimeToCollision = [](auto &&...) { return Double(); }; - evaluations.push_back(evaluateTimeToCollision(triggering_entity, "TODO")); + [[deprecated]] auto evaluateTimeToCollision = + [this]( + const EntityRef & triggering_entity, + const TimeToCollisionConditionTarget & time_to_collision_condition_target) { + if (time_to_collision_condition_target.is()) { + std::cerr << "RELATIVE DISTANCE = " + << RelativeDistanceCondition::distance( + triggering_entity, time_to_collision_condition_target.as(), + global().entities, coordinate_system, relative_distance_type, + routing_algorithm, freespace) + << ", " + << "RELATIVE SPEED = " + << RelativeSpeedCondition::evaluate( + triggering_entity, time_to_collision_condition_target.as(), + global().entities, std::nullopt) + << std::endl; + } + return Double(); + }; + evaluations.push_back( + evaluateTimeToCollision(triggering_entity, time_to_collision_condition_target)); return std::invoke(rule, evaluations.back(), value); })); } diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index 8740ef9761c..349ebae69ae 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -328,13 +328,13 @@ auto RelativeDistanceCondition::distance< #define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity, entity_ref) auto RelativeDistanceCondition::distance( - const EntityRef & triggering_entity, const EntityRef & entity_ref, const Entities & entities, + const EntityRef & triggering_entity, const EntityRef & entity_ref, const Entities * entities, CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, RoutingAlgorithm routing_algorithm, bool freespace) -> double { if ( - entities.at(triggering_entity).as().is_added and - entities.at(entity_ref).as().is_added) { + entities->at(triggering_entity).as().is_added and + entities->at(entity_ref).as().is_added) { SWITCH_COORDINATE_SYSTEM( SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); } else { @@ -348,7 +348,7 @@ auto RelativeDistanceCondition::evaluate() -> Object return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) { results.push_back(distance( - triggering_entity, entity_ref, *global().entities, coordinate_system, relative_distance_type, + triggering_entity, entity_ref, global().entities, coordinate_system, relative_distance_type, routing_algorithm, freespace)); return rule(static_cast(results.back()), value); })); diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp new file mode 100644 index 00000000000..0e7d30bd03f --- /dev/null +++ b/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp @@ -0,0 +1,92 @@ +// 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 + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +RelativeSpeedCondition::RelativeSpeedCondition( + const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) +: Scope(scope), + entity_ref(readAttribute("entityRef", node, scope)), + rule(readAttribute("rule", node, scope)), + value(readAttribute("value", node, scope)), + direction(readAttribute("direction", node, scope, std::nullopt)), + triggering_entities(triggering_entities), + evaluations(triggering_entities.entity_refs.size(), Double::nan()) +{ +} + +auto RelativeSpeedCondition::description() const -> String +{ + auto description = std::stringstream(); + + description << triggering_entities.description() << "'s relative "; + + if (direction) { + description << *direction << " "; + } + + description << "speed to given entity " << entity_ref << " = "; + + print_to(description, evaluations); + + description << " " << rule << " " << value << "?"; + + return description.str(); +} + +auto RelativeSpeedCondition::evaluate( + const EntityRef & triggering_entity, const EntityRef & entity_ref, const Entities * entities, + const std::optional & direction) -> double +{ + if ( + entities->at(triggering_entity).as().is_added and + entities->at(entity_ref).as().is_added) { + if (const auto v = evaluateRelativeSpeed(triggering_entity, entity_ref); direction) { + switch (*direction) { + case DirectionalDimension::longitudinal: + return v.x; + case DirectionalDimension::lateral: + return v.y; + case DirectionalDimension::vertical: + return v.z; + default: + return math::geometry::norm(v); + } + } else { + return math::geometry::norm(v); + } + } else { + return Double::nan(); + } +} + +auto RelativeSpeedCondition::evaluate() -> Object +{ + evaluations.clear(); + + return asBoolean(triggering_entities.apply([this](auto && triggering_entity) { + evaluations.push_back(evaluate(triggering_entity, entity_ref, global().entities, direction)); + return std::invoke(rule, evaluations.back(), value); + })); +} +} // namespace syntax +} // namespace openscenario_interpreter From be16d09e3ed0e6865a8ac8cd42a638edd84c5188 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 26 Jun 2024 16:44:01 +0900 Subject: [PATCH 008/149] Remove data member `DistanceCondition::consider_z` Signed-off-by: yamacir-kit --- .../syntax/distance_condition.hpp | 36 +++++----- .../src/syntax/distance_condition.cpp | 69 +++++++++---------- 2 files changed, 49 insertions(+), 56 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp index 61ea2db6345..f64e250326e 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp @@ -107,18 +107,18 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua std::vector results; // for description - const bool consider_z; - explicit DistanceCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); auto description() const -> std::string; - auto distance(const EntityRef &) const -> double; + // TODO STATIC + auto distance(const EntityRef &) -> double; template < CoordinateSystem::value_type, RelativeDistanceType::value_type, RoutingAlgorithm::value_type, Boolean::value_type> - auto distance(const EntityRef &) const -> double + // TODO STATIC + auto distance(const EntityRef &) -> double { throw SyntaxError(__FILE__, ":", __LINE__); } @@ -130,20 +130,20 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua // cspell: ignore euclidian // clang-format off -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; // clang-format on } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp index 4e817bb7286..f759f485c79 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp @@ -46,12 +46,7 @@ DistanceCondition::DistanceCondition( value(readAttribute("value", node, scope)), position(readElement("Position", node, scope)), triggering_entities(triggering_entities), - results(triggering_entities.entity_refs.size(), Double::nan()), - consider_z([]() { - rclcpp::Node node{"get_parameter", "simulation"}; - node.declare_parameter("consider_pose_by_road_slope", false); - return node.get_parameter("consider_pose_by_road_slope").as_bool(); - }()) + results(triggering_entities.entity_refs.size(), Double::nan()) { std::set supported = { RoutingAlgorithm::value_type::shortest, RoutingAlgorithm::value_type::undefined}; @@ -127,7 +122,7 @@ auto DistanceCondition::description() const -> std::string #define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity) -auto DistanceCondition::distance(const EntityRef & triggering_entity) const -> double +auto DistanceCondition::distance(const EntityRef & triggering_entity) -> double { SWITCH_COORDINATE_SYSTEM( SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); @@ -135,15 +130,21 @@ auto DistanceCondition::distance(const EntityRef & triggering_entity) const -> d } // @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x,y,z) -static double hypot(const double x, const double y, const double z, const bool consider_z) +static auto hypot(const double x, const double y, const double z) { + static auto consider_z = []() { + auto node = rclcpp::Node("get_parameter", "simulation"); + node.declare_parameter("consider_pose_by_road_slope", false); + return node.get_parameter("consider_pose_by_road_slope").as_bool(); + }(); + return consider_z ? std::hypot(x, y, z) : std::hypot(x, y); } template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, - false>(const EntityRef & triggering_entity) const -> double + false>(const EntityRef & triggering_entity) -> double { return apply( overload( @@ -151,29 +152,25 @@ auto DistanceCondition::distance< const auto relative_world = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); return hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z, - consider_z); + relative_world.position.x, relative_world.position.y, relative_world.position.z); }, [&](const RelativeWorldPosition & position) { const auto relative_world = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); return hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z, - consider_z); + relative_world.position.x, relative_world.position.y, relative_world.position.z); }, [&](const RelativeObjectPosition & position) { const auto relative_world = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); return hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z, - consider_z); + relative_world.position.x, relative_world.position.y, relative_world.position.z); }, [&](const LanePosition & position) { const auto relative_world = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); return hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z, - consider_z); + relative_world.position.x, relative_world.position.y, relative_world.position.z); }), position); } @@ -181,7 +178,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, - true>(const EntityRef & triggering_entity) const -> double + true>(const EntityRef & triggering_entity) -> double { return apply( overload( @@ -189,29 +186,25 @@ auto DistanceCondition::distance< const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition( triggering_entity, static_cast(position)); return hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z, - consider_z); + relative_world.position.x, relative_world.position.y, relative_world.position.z); }, [&](const RelativeWorldPosition & position) { const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition( triggering_entity, static_cast(position)); return hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z, - consider_z); + relative_world.position.x, relative_world.position.y, relative_world.position.z); }, [&](const RelativeObjectPosition & position) { const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition( triggering_entity, static_cast(position)); return hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z, - consider_z); + relative_world.position.x, relative_world.position.y, relative_world.position.z); }, [&](const LanePosition & position) { const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition( triggering_entity, static_cast(position)); return hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z, - consider_z); + relative_world.position.x, relative_world.position.y, relative_world.position.z); }), position); } @@ -219,7 +212,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -249,7 +242,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -279,7 +272,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -309,7 +302,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -339,7 +332,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -388,7 +381,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -437,7 +430,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -486,7 +479,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -535,7 +528,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -588,7 +581,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -641,7 +634,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -694,7 +687,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( From 77a1b31e289734ae59baeff140fc62b3c55d4acc Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 26 Jun 2024 16:57:36 +0900 Subject: [PATCH 009/149] Add `const Position &` to the argument of `DistanceCondition::distance` Signed-off-by: yamacir-kit --- .../syntax/distance_condition.hpp | 33 +++++++++-------- .../src/syntax/distance_condition.cpp | 35 ++++++++++--------- 2 files changed, 34 insertions(+), 34 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp index f64e250326e..6ace25ce376 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp @@ -112,13 +112,12 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua auto description() const -> std::string; // TODO STATIC - auto distance(const EntityRef &) -> double; + auto distance(const EntityRef &, const Position &) -> double; template < CoordinateSystem::value_type, RelativeDistanceType::value_type, RoutingAlgorithm::value_type, Boolean::value_type> - // TODO STATIC - auto distance(const EntityRef &) -> double + auto distance(const EntityRef &, const Position &) -> double { throw SyntaxError(__FILE__, ":", __LINE__); } @@ -130,20 +129,20 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua // cspell: ignore euclidian // clang-format off -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; // clang-format on } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp index f759f485c79..e59f6cb38d7 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp @@ -120,9 +120,10 @@ auto DistanceCondition::description() const -> std::string #define SWITCH_FREESPACE(FUNCTION, ...) \ return freespace ? FUNCTION(__VA_ARGS__, true) : FUNCTION(__VA_ARGS__, false) -#define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity) +#define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity, position) -auto DistanceCondition::distance(const EntityRef & triggering_entity) -> double +auto DistanceCondition::distance(const EntityRef & triggering_entity, const Position & position) + -> double { SWITCH_COORDINATE_SYSTEM( SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); @@ -144,7 +145,7 @@ static auto hypot(const double x, const double y, const double z) template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, - false>(const EntityRef & triggering_entity) -> double + false>(const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -178,7 +179,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, - true>(const EntityRef & triggering_entity) -> double + true>(const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -212,7 +213,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -242,7 +243,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -272,7 +273,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -302,7 +303,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -332,7 +333,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -381,7 +382,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -430,7 +431,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -479,7 +480,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -528,7 +529,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -581,7 +582,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -634,7 +635,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -687,7 +688,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -742,7 +743,7 @@ auto DistanceCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - results.push_back(distance(triggering_entity)); + results.push_back(distance(triggering_entity, position)); return rule(static_cast(results.back()), value); })); } From 9b13ccd31853c522ce6bf87b80aeceb97f110820 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 26 Jun 2024 17:20:03 +0900 Subject: [PATCH 010/149] Update member function `CoordinateSystem::distance` to be static member Signed-off-by: yamacir-kit --- .../syntax/distance_condition.hpp | 2 +- .../src/syntax/distance_condition.cpp | 409 ++++++------------ 2 files changed, 143 insertions(+), 268 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp index 6ace25ce376..4068c498995 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp @@ -117,7 +117,7 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua template < CoordinateSystem::value_type, RelativeDistanceType::value_type, RoutingAlgorithm::value_type, Boolean::value_type> - auto distance(const EntityRef &, const Position &) -> double + static auto distance(const EntityRef &, const Position &) -> double { throw SyntaxError(__FILE__, ":", __LINE__); } diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp index e59f6cb38d7..5a73c536e7f 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp @@ -125,9 +125,12 @@ auto DistanceCondition::description() const -> std::string auto DistanceCondition::distance(const EntityRef & triggering_entity, const Position & position) -> double { - SWITCH_COORDINATE_SYSTEM( - SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); - return Double::nan(); + if (global().entities->ref(triggering_entity).as().is_added) { + SWITCH_COORDINATE_SYSTEM( + SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); + } else { + return Double::nan(); + } } // @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x,y,z) @@ -338,43 +341,27 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position))) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position))) + .offset; }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position))) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position))) + .offset; }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return makeNativeRelativeLanePosition( - triggering_entity, static_cast(position)) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return makeNativeRelativeLanePosition( + triggering_entity, static_cast(position)) + .offset; }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position))) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position))) + .offset; }), position); } @@ -387,43 +374,27 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position))) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position))) + .offset; }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position))) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position))) + .offset; }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position)) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position)) + .offset; }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position))) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position))) + .offset; }), position); } @@ -436,43 +407,27 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position))) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position))) + .s; }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position))) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position))) + .s; }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return makeNativeRelativeLanePosition( - triggering_entity, static_cast(position)) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return makeNativeRelativeLanePosition( + triggering_entity, static_cast(position)) + .s; }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).template as().is_added) { - return static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position))) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position))) + .s; }), position); } @@ -485,43 +440,27 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position))) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position))) + .s; }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position))) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position))) + .s; }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position)) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position)) + .s; }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).template as().is_added) { - return static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position))) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position))) + .s; }), position); } @@ -534,47 +473,31 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .offset); }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .offset); }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest) + .offset); }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .offset); }), position); } @@ -587,47 +510,31 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .offset); }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .offset); }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest) + .offset); }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .offset); }), position); } @@ -640,47 +547,31 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .s); }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .s); }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest) + .s); }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).template as().is_added) { - return std::abs(static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .s); }), position); } @@ -693,47 +584,31 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .s); }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .s); }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest) + .s); }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).template as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .s); }), position); } From f95eccb3711f4a036b9f67d8de25a901c3aa5b0f Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 27 Jun 2024 14:33:59 +0900 Subject: [PATCH 011/149] Rename `(Relative)?DistanceCondition::distance` to `evaluate` Signed-off-by: yamacir-kit --- .../syntax/distance_condition.hpp | 7 ++++--- .../syntax/relative_distance_condition.hpp | 2 +- .../syntax/speed_condition.hpp | 4 +++- .../syntax/time_to_collision_condition.hpp | 16 +++++++++++++--- .../src/syntax/distance_condition.cpp | 12 ++++++++---- .../src/syntax/relative_distance_condition.cpp | 4 ++-- .../src/syntax/speed_condition.cpp | 18 ++++++++++++++++-- 7 files changed, 47 insertions(+), 16 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp index 4068c498995..453a31fed35 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp @@ -111,9 +111,6 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua auto description() const -> std::string; - // TODO STATIC - auto distance(const EntityRef &, const Position &) -> double; - template < CoordinateSystem::value_type, RelativeDistanceType::value_type, RoutingAlgorithm::value_type, Boolean::value_type> @@ -122,6 +119,10 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua throw SyntaxError(__FILE__, ":", __LINE__); } + static auto evaluate( + const EntityRef &, const Position &, const Entities *, CoordinateSystem, RelativeDistanceType, + RoutingAlgorithm, bool) -> double; + auto evaluate() -> Object; }; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp index 475f91ff739..bf09beca6d2 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp @@ -102,7 +102,7 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi throw SyntaxError(__FILE__, ":", __LINE__); } - static auto distance( + static auto evaluate( const EntityRef &, const EntityRef &, const Entities *, CoordinateSystem, RelativeDistanceType, RoutingAlgorithm, bool) -> double; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp index 29d0be6eb99..9e3398a4ecb 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp @@ -37,7 +37,7 @@ inline namespace syntax * * * -------------------------------------------------------------------------- */ -struct SpeedCondition : private SimulatorCore::ConditionEvaluation +struct SpeedCondition : private Scope, private SimulatorCore::ConditionEvaluation { const Double value; @@ -51,6 +51,8 @@ struct SpeedCondition : private SimulatorCore::ConditionEvaluation auto description() const -> String; + static auto evaluate(const EntityRef &, const Entities *) -> double; + auto evaluate() -> Object; }; } // namespace syntax diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index 39e7e776a1d..23140651a67 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -17,8 +17,10 @@ #include #include +#include #include #include +#include #include #include @@ -126,16 +128,24 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio const TimeToCollisionConditionTarget & time_to_collision_condition_target) { if (time_to_collision_condition_target.is()) { std::cerr << "RELATIVE DISTANCE = " - << RelativeDistanceCondition::distance( + << RelativeDistanceCondition::evaluate( triggering_entity, time_to_collision_condition_target.as(), global().entities, coordinate_system, relative_distance_type, routing_algorithm, freespace) - << ", " - << "RELATIVE SPEED = " + << ", RELATIVE SPEED = " << RelativeSpeedCondition::evaluate( triggering_entity, time_to_collision_condition_target.as(), global().entities, std::nullopt) << std::endl; + } else { + std::cerr << "DISTANCE = " + << DistanceCondition::evaluate( + triggering_entity, time_to_collision_condition_target.as(), + global().entities, coordinate_system, relative_distance_type, + routing_algorithm, freespace) + << ", RELATIVE SPEED = " + << SpeedCondition::evaluate(triggering_entity, global().entities) + << std::endl; } return Double(); }; diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp index 5a73c536e7f..4621497b6f0 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp @@ -122,10 +122,12 @@ auto DistanceCondition::description() const -> std::string #define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity, position) -auto DistanceCondition::distance(const EntityRef & triggering_entity, const Position & position) - -> double +auto DistanceCondition::evaluate( + const EntityRef & triggering_entity, const Position & position, const Entities * entities, + CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, + RoutingAlgorithm routing_algorithm, bool freespace) -> double { - if (global().entities->ref(triggering_entity).as().is_added) { + if (entities->ref(triggering_entity).as().is_added) { SWITCH_COORDINATE_SYSTEM( SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); } else { @@ -618,7 +620,9 @@ auto DistanceCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - results.push_back(distance(triggering_entity, position)); + results.push_back(evaluate( + triggering_entity, position, global().entities, coordinate_system, relative_distance_type, + routing_algorithm, freespace)); return rule(static_cast(results.back()), value); })); } diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index 349ebae69ae..36203a6479b 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -327,7 +327,7 @@ auto RelativeDistanceCondition::distance< #define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity, entity_ref) -auto RelativeDistanceCondition::distance( +auto RelativeDistanceCondition::evaluate( const EntityRef & triggering_entity, const EntityRef & entity_ref, const Entities * entities, CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, RoutingAlgorithm routing_algorithm, bool freespace) -> double @@ -347,7 +347,7 @@ auto RelativeDistanceCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) { - results.push_back(distance( + results.push_back(evaluate( triggering_entity, entity_ref, global().entities, coordinate_system, relative_distance_type, routing_algorithm, freespace)); return rule(static_cast(results.back()), value); diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp index 95f4f9b2497..4c4c34e131f 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp @@ -13,7 +13,10 @@ // limitations under the License. #include +#include #include +#include +#include #include #include @@ -23,7 +26,8 @@ inline namespace syntax { SpeedCondition::SpeedCondition( const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) -: value(readAttribute("value", node, scope)), +: Scope(scope), + value(readAttribute("value", node, scope)), compare(readAttribute("rule", node, scope)), triggering_entities(triggering_entities), results(triggering_entities.entity_refs.size(), Double::nan()) @@ -43,12 +47,22 @@ auto SpeedCondition::description() const -> String return description.str(); } +auto SpeedCondition::evaluate(const EntityRef & triggering_entity, const Entities * entities) + -> double +{ + if (entities->ref(triggering_entity).as().is_added) { + return evaluateSpeed(triggering_entity); + } else { + return Double::nan(); + } +} + auto SpeedCondition::evaluate() -> Object { results.clear(); return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - results.push_back(evaluateSpeed(triggering_entity)); + results.push_back(evaluate(triggering_entity, global().entities)); return compare(results.back(), value); })); } From 26e5a413f1ee5d8751abc3a281b06ab470b3703e Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 5 Jul 2024 16:14:32 +0900 Subject: [PATCH 012/149] Add support for `DirectionalDimension` to `SpeedCondition` Signed-off-by: yamacir-kit --- .../simulator_core.hpp | 2 +- .../syntax/speed_action.hpp | 3 +- .../syntax/speed_condition.hpp | 46 +++++++++++++------ .../syntax/speed_profile_action.hpp | 3 +- .../src/syntax/speed_action.cpp | 14 ++++-- .../src/syntax/speed_condition.cpp | 38 ++++++++++----- .../src/syntax/speed_profile_action.cpp | 7 ++- 7 files changed, 76 insertions(+), 37 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index ee5ee67895f..f6417f82168 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -507,7 +507,7 @@ class SimulatorCore template static auto evaluateSpeed(Ts &&... xs) { - return core->getCurrentTwist(std::forward(xs)...).linear.x; + return core->getCurrentTwist(std::forward(xs)...).linear; } template diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_action.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_action.hpp index 54f7eded7eb..0eeb69dff4d 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_action.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_action.hpp @@ -38,8 +38,7 @@ inline namespace syntax * * -------------------------------------------------------------------------- */ struct SpeedAction : private Scope, // NOTE: Required for access to actors - private SimulatorCore::ActionApplication, - private SimulatorCore::ConditionEvaluation + private SimulatorCore::ActionApplication { const TransitionDynamics speed_action_dynamics; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp index 9e3398a4ecb..7a776f2c0c2 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -26,32 +27,49 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- SpeedCondition --------------------------------------------------------- - * - * Compares a triggering entity's/entities' speed to a target speed. The - * logical operator for the comparison is given by the rule attribute. - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + SpeedCondition (OpenSCENARIO XML 1.3) + + Compares a triggering entity's/entities' speed to a target speed. + The logical operator for the comparison is given by the rule + attribute. If direction is used, only the projection to that + direction is used in the comparison. + + + + + + +*/ struct SpeedCondition : private Scope, private SimulatorCore::ConditionEvaluation { + /* + The operator (less, greater, equal). + */ + const Rule rule; + + /* + Speed value of the speed condition. Unit: [m/s]. + */ const Double value; - const Rule compare; + /* + Direction of the speed (if not given, the total speed is + considered). + */ + const std::optional direction; const TriggeringEntities triggering_entities; - std::vector results; // for description + std::vector evaluations; // for description explicit SpeedCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); auto description() const -> String; - static auto evaluate(const EntityRef &, const Entities *) -> double; + static auto evaluate( + const EntityRef &, const Entities *, const std::optional & = std::nullopt) + -> double; auto evaluate() -> Object; }; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_profile_action.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_profile_action.hpp index 2fea3279574..b6ce60f977b 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_profile_action.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_profile_action.hpp @@ -40,8 +40,7 @@ inline namespace syntax * * -------------------------------------------------------------------------- */ struct SpeedProfileAction : private Scope, // NOTE: Required for access to actors - private SimulatorCore::ActionApplication, - private SimulatorCore::ConditionEvaluation + private SimulatorCore::ActionApplication { /* Reference entity. If set, the speed values will be interpreted as relative diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp index e72523eb949..c3a10522d76 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp @@ -16,6 +16,7 @@ #include #include #include +#include namespace openscenario_interpreter { @@ -45,19 +46,22 @@ auto SpeedAction::accomplished() -> bool auto check = [this](auto && actor) { if (speed_action_target.is()) { return equal_to()( - speed_action_target.as().value, evaluateSpeed(actor)); + speed_action_target.as().value, + SpeedCondition::evaluate(actor, global().entities)); } else { switch (speed_action_target.as().speed_target_value_type) { case SpeedTargetValueType::delta: return equal_to()( - evaluateSpeed(speed_action_target.as().entity_ref) + + SpeedCondition::evaluate( + speed_action_target.as().entity_ref, global().entities) + speed_action_target.as().value, - evaluateSpeed(actor)); + SpeedCondition::evaluate(actor, global().entities)); case SpeedTargetValueType::factor: return equal_to()( - evaluateSpeed(speed_action_target.as().entity_ref) * + SpeedCondition::evaluate( + speed_action_target.as().entity_ref, global().entities) * speed_action_target.as().value, - evaluateSpeed(actor)); + SpeedCondition::evaluate(actor, global().entities)); default: return false; } diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp index 4c4c34e131f..1e8ebcdf228 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -27,10 +28,11 @@ inline namespace syntax SpeedCondition::SpeedCondition( const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) : Scope(scope), + rule(readAttribute("rule", node, scope)), value(readAttribute("value", node, scope)), - compare(readAttribute("rule", node, scope)), + direction(readAttribute("direction", node, scope, std::nullopt)), triggering_entities(triggering_entities), - results(triggering_entities.entity_refs.size(), Double::nan()) + evaluations(triggering_entities.entity_refs.size(), Double::nan()) { } @@ -40,18 +42,32 @@ auto SpeedCondition::description() const -> String description << triggering_entities.description() << "'s speed = "; - print_to(description, results); + print_to(description, evaluations); - description << " " << compare << " " << value << "?"; + description << " " << rule << " " << value << "?"; return description.str(); } -auto SpeedCondition::evaluate(const EntityRef & triggering_entity, const Entities * entities) - -> double +auto SpeedCondition::evaluate( + const EntityRef & triggering_entity, const Entities * entities, + const std::optional & direction) -> double { - if (entities->ref(triggering_entity).as().is_added) { - return evaluateSpeed(triggering_entity); + if (entities->isAdded(triggering_entity)) { + if (const auto v = evaluateSpeed(triggering_entity); direction) { + switch (*direction) { + case DirectionalDimension::longitudinal: + return v.x; + case DirectionalDimension::lateral: + return v.y; + case DirectionalDimension::vertical: + return v.z; + default: + return math::geometry::norm(v); + } + } else { + return math::geometry::norm(v); + } } else { return Double::nan(); } @@ -59,11 +75,11 @@ auto SpeedCondition::evaluate(const EntityRef & triggering_entity, const Entitie auto SpeedCondition::evaluate() -> Object { - results.clear(); + evaluations.clear(); return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - results.push_back(evaluate(triggering_entity, global().entities)); - return compare(results.back(), value); + evaluations.push_back(evaluate(triggering_entity, global().entities)); + return std::invoke(rule, evaluations.back(), value); })); } } // namespace syntax diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp index accb647abf7..214e274fcbb 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include namespace openscenario_interpreter @@ -100,10 +101,12 @@ auto SpeedProfileAction::run() -> void for (auto && [actor, iter] : accomplishments) { auto accomplished = [this](const auto & actor, const auto & speed_profile_entry) { if (entity_ref.empty()) { - return equal_to()(evaluateSpeed(actor), speed_profile_entry.speed); + return equal_to()( + SpeedCondition::evaluate(actor, global().entities), speed_profile_entry.speed); } else { return equal_to()( - evaluateSpeed(actor), speed_profile_entry.speed + evaluateSpeed(entity_ref)); + SpeedCondition::evaluate(actor, global().entities), + speed_profile_entry.speed + SpeedCondition::evaluate(entity_ref, global().entities)); } }; From cddc3a52c475717a36249c6b837f29ccbbe2bf73 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 5 Jul 2024 17:11:22 +0900 Subject: [PATCH 013/149] Move function `hypot` into new header `cmath/hypot.hpp` Signed-off-by: yamacir-kit --- .../openscenario_interpreter/cmath/hypot.hpp | 44 +++++++++++++++++++ .../syntax/reach_position_condition.hpp | 2 - .../syntax/relative_speed_condition.hpp | 2 +- .../syntax/time_to_collision_condition.hpp | 2 +- .../src/syntax/distance_condition.cpp | 15 +------ .../src/syntax/reach_position_condition.cpp | 22 +++------- .../syntax/relative_distance_condition.cpp | 13 +----- 7 files changed, 55 insertions(+), 45 deletions(-) create mode 100644 openscenario/openscenario_interpreter/include/openscenario_interpreter/cmath/hypot.hpp diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/cmath/hypot.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/cmath/hypot.hpp new file mode 100644 index 00000000000..23a19b9e0c9 --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/cmath/hypot.hpp @@ -0,0 +1,44 @@ +// 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 OPENSCENARIO_INTERPRETER__CMATH__HYPOT_HPP_ +#define OPENSCENARIO_INTERPRETER__CMATH__HYPOT_HPP_ + +#include +#include + +namespace openscenario_interpreter +{ +inline namespace cmath +{ +/* + @todo: after checking all the scenario work well with + consider_pose_by_road_slope = true, remove this function and use + std::hypot(x, y, z) +*/ +template +auto hypot(T x, T y, T z) +{ + static const auto consider_pose_by_road_slope = []() { + auto node = rclcpp::Node("get_parameter", "simulation"); + node.declare_parameter("consider_pose_by_road_slope", false); + return node.get_parameter("consider_pose_by_road_slope").as_bool(); + }(); + + return consider_pose_by_road_slope ? std::hypot(x, y, z) : std::hypot(x, y); +} +} // namespace cmath +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__CMATH__HYPOT_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp index 527614ad500..35e98272ce5 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp @@ -49,8 +49,6 @@ struct ReachPositionCondition : private SimulatorCore::CoordinateSystemConversio std::vector results; // for description - const bool consider_z; - explicit ReachPositionCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); auto description() const -> String; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp index d42b50db0dd..a485b3e315b 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp @@ -74,7 +74,7 @@ struct RelativeSpeedCondition : private Scope, private SimulatorCore::ConditionE static auto evaluate( const EntityRef &, const EntityRef &, const Entities *, - const std::optional &) -> double; + const std::optional & = std::nullopt) -> double; auto evaluate() -> Object; }; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index 23140651a67..c6f70c43ac3 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -135,7 +135,7 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio << ", RELATIVE SPEED = " << RelativeSpeedCondition::evaluate( triggering_entity, time_to_collision_condition_target.as(), - global().entities, std::nullopt) + global().entities) << std::endl; } else { std::cerr << "DISTANCE = " diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp index 4621497b6f0..3c6479e12ff 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include @@ -130,23 +130,12 @@ auto DistanceCondition::evaluate( if (entities->ref(triggering_entity).as().is_added) { SWITCH_COORDINATE_SYSTEM( SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); + return Double::nan(); } else { return Double::nan(); } } -// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x,y,z) -static auto hypot(const double x, const double y, const double z) -{ - static auto consider_z = []() { - auto node = rclcpp::Node("get_parameter", "simulation"); - node.declare_parameter("consider_pose_by_road_slope", false); - return node.get_parameter("consider_pose_by_road_slope").as_bool(); - }(); - - return consider_z ? std::hypot(x, y, z) : std::hypot(x, y); -} - template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, diff --git a/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp index 07dc4e81d06..08d7000c86e 100644 --- a/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -31,12 +32,7 @@ ReachPositionCondition::ReachPositionCondition( position(readElement("Position", node, scope)), compare(Rule::lessThan), triggering_entities(triggering_entities), - results(triggering_entities.entity_refs.size(), Double::nan()), - consider_z([]() { - rclcpp::Node node{"get_parameter", "simulation"}; - node.declare_parameter("consider_pose_by_road_slope", false); - return node.get_parameter("consider_pose_by_road_slope").as_bool(); - }()) + results(triggering_entities.entity_refs.size(), Double::nan()) { } @@ -53,12 +49,6 @@ auto ReachPositionCondition::description() const -> String return description.str(); } -// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x,y,z) -static double hypot(const double x, const double y, const double z, const bool consider_z) -{ - return consider_z ? std::hypot(x, y, z) : std::hypot(x, y); -} - auto ReachPositionCondition::evaluate() -> Object { // TODO USE DistanceCondition::distance @@ -66,22 +56,22 @@ auto ReachPositionCondition::evaluate() -> Object [&](const WorldPosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z); + return hypot(pose.position.x, pose.position.y, pose.position.z); }, [&](const RelativeWorldPosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z); + return hypot(pose.position.x, pose.position.y, pose.position.z); }, [&](const RelativeObjectPosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z); + return hypot(pose.position.x, pose.position.y, pose.position.z); }, [&](const LanePosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z); + return hypot(pose.position.x, pose.position.y, pose.position.z); }); results.clear(); diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index 36203a6479b..b9b3b535a0a 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include // TEMPORARY (TODO REMOVE THIS LINE) @@ -105,18 +106,6 @@ auto RelativeDistanceCondition::distance< makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y); } -// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x, y, z) -auto hypot(double x, double y, double z) -{ - static auto consider_z = []() { - auto node = rclcpp::Node("get_parameter", "simulation"); - node.declare_parameter("consider_pose_by_road_slope", false); - return node.get_parameter("consider_pose_by_road_slope").as_bool(); - }(); - - return consider_z ? std::hypot(x, y, z) : std::hypot(x, y); -} - template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, From 5018106f2b92152ff2d49b0e038c252665af488c Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 5 Jul 2024 18:11:04 +0900 Subject: [PATCH 014/149] Update `unordered_map` of the `Entities` base class to private Signed-off-by: yamacir-kit --- .../syntax/entities.hpp | 28 +++++++++++-------- .../src/syntax/add_entity_action.cpp | 2 +- .../syntax/relative_distance_condition.cpp | 4 +-- .../src/syntax/relative_speed_condition.cpp | 4 +-- .../src/syntax/teleport_action.cpp | 2 +- 5 files changed, 21 insertions(+), 19 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entities.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entities.hpp index a8abbd1dc91..27a65121a24 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entities.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entities.hpp @@ -23,18 +23,24 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- Entities --------------------------------------------------------------- - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ -struct Entities : public std::unordered_map // TODO to be data member +/* + Entities (OpenSCENARIO XML 1.3) + + Definition of entities (scenario objects or entity selections) in + a scenario. + + + + + + + +*/ +struct Entities : private std::unordered_map { + using std::unordered_map::begin; + using std::unordered_map::end; + explicit Entities(const pugi::xml_node &, Scope &); auto isAdded(const EntityRef &) const -> bool; diff --git a/openscenario/openscenario_interpreter/src/syntax/add_entity_action.cpp b/openscenario/openscenario_interpreter/src/syntax/add_entity_action.cpp index d0408597bf5..ae11d05f50c 100644 --- a/openscenario/openscenario_interpreter/src/syntax/add_entity_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/add_entity_action.cpp @@ -47,7 +47,7 @@ auto AddEntityAction::endsImmediately() noexcept -> bool // auto AddEntityAction::operator()(const EntityRef & entity_ref) const -> void try { - const auto entity = global().entities->at(entity_ref); + const auto entity = global().entities->ref(entity_ref); const auto add_entity = overload( [&](const Vehicle & vehicle) { diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index b9b3b535a0a..558affe0730 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -321,9 +321,7 @@ auto RelativeDistanceCondition::evaluate( CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, RoutingAlgorithm routing_algorithm, bool freespace) -> double { - if ( - entities->at(triggering_entity).as().is_added and - entities->at(entity_ref).as().is_added) { + if (entities->isAdded(triggering_entity) and entities->isAdded(entity_ref)) { SWITCH_COORDINATE_SYSTEM( SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); } else { diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp index 0e7d30bd03f..748b5ba179c 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp @@ -57,9 +57,7 @@ auto RelativeSpeedCondition::evaluate( const EntityRef & triggering_entity, const EntityRef & entity_ref, const Entities * entities, const std::optional & direction) -> double { - if ( - entities->at(triggering_entity).as().is_added and - entities->at(entity_ref).as().is_added) { + if (entities->isAdded(triggering_entity) and entities->isAdded(entity_ref)) { if (const auto v = evaluateRelativeSpeed(triggering_entity, entity_ref); direction) { switch (*direction) { case DirectionalDimension::longitudinal: diff --git a/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp b/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp index 4e59e603519..8d150d0c919 100644 --- a/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp @@ -44,7 +44,7 @@ auto TeleportAction::run() noexcept -> void {} auto TeleportAction::start() const -> void { for (const auto & actor : actors) { - if (not global().entities->at(actor).as().is_added) { + if (not global().entities->isAdded(actor)) { AddEntityAction(local(), position)(actor); // NOTE: TIER IV extension } else { return teleport(actor, position); From fdae8b4dee1109e33c22ec71ac14ef189e84ffae Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 8 Jul 2024 12:41:28 +0900 Subject: [PATCH 015/149] Add new static member function `TimeToCollisionCondition::evaluate` Signed-off-by: yamacir-kit --- .../syntax/distance_condition.hpp | 4 +- .../syntax/relative_distance_condition.hpp | 4 +- .../syntax/relative_speed_condition.hpp | 2 +- .../syntax/speed_condition.hpp | 2 +- .../syntax/time_to_collision_condition.hpp | 75 ++++++++++++------- .../src/syntax/distance_condition.cpp | 6 +- .../syntax/relative_distance_condition.cpp | 6 +- .../src/syntax/relative_speed_condition.cpp | 4 +- .../src/syntax/speed_action.cpp | 10 +-- .../src/syntax/speed_condition.cpp | 4 +- .../src/syntax/speed_profile_action.cpp | 6 +- 11 files changed, 70 insertions(+), 53 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp index 453a31fed35..1c72887b355 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp @@ -120,8 +120,8 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua } static auto evaluate( - const EntityRef &, const Position &, const Entities *, CoordinateSystem, RelativeDistanceType, - RoutingAlgorithm, bool) -> double; + const Entities *, const EntityRef &, const Position &, CoordinateSystem, RelativeDistanceType, + RoutingAlgorithm, Boolean) -> double; auto evaluate() -> Object; }; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp index bf09beca6d2..8dea6e12dad 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp @@ -103,8 +103,8 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi } static auto evaluate( - const EntityRef &, const EntityRef &, const Entities *, CoordinateSystem, RelativeDistanceType, - RoutingAlgorithm, bool) -> double; + const Entities *, const EntityRef &, const EntityRef &, CoordinateSystem, RelativeDistanceType, + RoutingAlgorithm, Boolean) -> double; auto evaluate() -> Object; }; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp index a485b3e315b..1fb90aaef54 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp @@ -73,7 +73,7 @@ struct RelativeSpeedCondition : private Scope, private SimulatorCore::ConditionE auto description() const -> String; static auto evaluate( - const EntityRef &, const EntityRef &, const Entities *, + const Entities *, const EntityRef &, const EntityRef &, const std::optional & = std::nullopt) -> double; auto evaluate() -> Object; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp index 7a776f2c0c2..874951d4c5a 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp @@ -68,7 +68,7 @@ struct SpeedCondition : private Scope, private SimulatorCore::ConditionEvaluatio auto description() const -> String; static auto evaluate( - const EntityRef &, const Entities *, const std::optional & = std::nullopt) + const Entities *, const EntityRef &, const std::optional & = std::nullopt) -> double; auto evaluate() -> Object; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index c6f70c43ac3..e460d8d6fb7 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -117,40 +117,57 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio return description.str(); } + static auto evaluate( + const Entities * entities, const EntityRef & triggering_entity, + const TimeToCollisionConditionTarget & time_to_collision_condition_target, + CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, + RoutingAlgorithm routing_algorithm, Boolean freespace) + { + if (time_to_collision_condition_target.is()) { + const auto relative_distance = RelativeDistanceCondition::evaluate( + entities, // + triggering_entity, // + time_to_collision_condition_target.as(), // + coordinate_system, // + relative_distance_type, // + routing_algorithm, // + freespace); + + const auto relative_speed = RelativeSpeedCondition::evaluate( + entities, // + triggering_entity, // + time_to_collision_condition_target.as()); + + std::cerr << "RELATIVE DISTANCE = " << relative_distance + << ", RELATIVE SPEED = " << relative_speed << std::endl; + + return relative_distance / relative_speed; + } else { + const auto distance = DistanceCondition::evaluate( + entities, // + triggering_entity, // + time_to_collision_condition_target.as(), // + coordinate_system, // + relative_distance_type, // + routing_algorithm, // + freespace); + + const auto speed = SpeedCondition::evaluate(entities, triggering_entity); + + std::cerr << "DISTANCE = " << distance << ", RELATIVE SPEED = " << speed << std::endl; + + return distance / speed; + } + } + auto evaluate() { evaluations.clear(); return asBoolean(triggering_entities.apply([this](auto && triggering_entity) { - [[deprecated]] auto evaluateTimeToCollision = - [this]( - const EntityRef & triggering_entity, - const TimeToCollisionConditionTarget & time_to_collision_condition_target) { - if (time_to_collision_condition_target.is()) { - std::cerr << "RELATIVE DISTANCE = " - << RelativeDistanceCondition::evaluate( - triggering_entity, time_to_collision_condition_target.as(), - global().entities, coordinate_system, relative_distance_type, - routing_algorithm, freespace) - << ", RELATIVE SPEED = " - << RelativeSpeedCondition::evaluate( - triggering_entity, time_to_collision_condition_target.as(), - global().entities) - << std::endl; - } else { - std::cerr << "DISTANCE = " - << DistanceCondition::evaluate( - triggering_entity, time_to_collision_condition_target.as(), - global().entities, coordinate_system, relative_distance_type, - routing_algorithm, freespace) - << ", RELATIVE SPEED = " - << SpeedCondition::evaluate(triggering_entity, global().entities) - << std::endl; - } - return Double(); - }; - evaluations.push_back( - evaluateTimeToCollision(triggering_entity, time_to_collision_condition_target)); + evaluations.push_back(evaluate( + global().entities, triggering_entity, time_to_collision_condition_target, coordinate_system, + relative_distance_type, routing_algorithm, freespace)); return std::invoke(rule, evaluations.back(), value); })); } diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp index 3c6479e12ff..6df2d9c64f6 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp @@ -123,9 +123,9 @@ auto DistanceCondition::description() const -> std::string #define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity, position) auto DistanceCondition::evaluate( - const EntityRef & triggering_entity, const Position & position, const Entities * entities, + const Entities * entities, const EntityRef & triggering_entity, const Position & position, CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, - RoutingAlgorithm routing_algorithm, bool freespace) -> double + RoutingAlgorithm routing_algorithm, Boolean freespace) -> double { if (entities->ref(triggering_entity).as().is_added) { SWITCH_COORDINATE_SYSTEM( @@ -610,7 +610,7 @@ auto DistanceCondition::evaluate() -> Object return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { results.push_back(evaluate( - triggering_entity, position, global().entities, coordinate_system, relative_distance_type, + global().entities, triggering_entity, position, coordinate_system, relative_distance_type, routing_algorithm, freespace)); return rule(static_cast(results.back()), value); })); diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index 558affe0730..dea1883ad25 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -317,9 +317,9 @@ auto RelativeDistanceCondition::distance< #define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity, entity_ref) auto RelativeDistanceCondition::evaluate( - const EntityRef & triggering_entity, const EntityRef & entity_ref, const Entities * entities, + const Entities * entities, const EntityRef & triggering_entity, const EntityRef & entity_ref, CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, - RoutingAlgorithm routing_algorithm, bool freespace) -> double + RoutingAlgorithm routing_algorithm, Boolean freespace) -> double { if (entities->isAdded(triggering_entity) and entities->isAdded(entity_ref)) { SWITCH_COORDINATE_SYSTEM( @@ -335,7 +335,7 @@ auto RelativeDistanceCondition::evaluate() -> Object return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) { results.push_back(evaluate( - triggering_entity, entity_ref, global().entities, coordinate_system, relative_distance_type, + global().entities, triggering_entity, entity_ref, coordinate_system, relative_distance_type, routing_algorithm, freespace)); return rule(static_cast(results.back()), value); })); diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp index 748b5ba179c..32b3781dbe5 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp @@ -54,7 +54,7 @@ auto RelativeSpeedCondition::description() const -> String } auto RelativeSpeedCondition::evaluate( - const EntityRef & triggering_entity, const EntityRef & entity_ref, const Entities * entities, + const Entities * entities, const EntityRef & triggering_entity, const EntityRef & entity_ref, const std::optional & direction) -> double { if (entities->isAdded(triggering_entity) and entities->isAdded(entity_ref)) { @@ -82,7 +82,7 @@ auto RelativeSpeedCondition::evaluate() -> Object evaluations.clear(); return asBoolean(triggering_entities.apply([this](auto && triggering_entity) { - evaluations.push_back(evaluate(triggering_entity, entity_ref, global().entities, direction)); + evaluations.push_back(evaluate(global().entities, triggering_entity, entity_ref, direction)); return std::invoke(rule, evaluations.back(), value); })); } diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp index c3a10522d76..90133eaa5d9 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp @@ -47,21 +47,21 @@ auto SpeedAction::accomplished() -> bool if (speed_action_target.is()) { return equal_to()( speed_action_target.as().value, - SpeedCondition::evaluate(actor, global().entities)); + SpeedCondition::evaluate(global().entities, actor)); } else { switch (speed_action_target.as().speed_target_value_type) { case SpeedTargetValueType::delta: return equal_to()( SpeedCondition::evaluate( - speed_action_target.as().entity_ref, global().entities) + + global().entities, speed_action_target.as().entity_ref) + speed_action_target.as().value, - SpeedCondition::evaluate(actor, global().entities)); + SpeedCondition::evaluate(global().entities, actor)); case SpeedTargetValueType::factor: return equal_to()( SpeedCondition::evaluate( - speed_action_target.as().entity_ref, global().entities) * + global().entities, speed_action_target.as().entity_ref) * speed_action_target.as().value, - SpeedCondition::evaluate(actor, global().entities)); + SpeedCondition::evaluate(global().entities, actor)); default: return false; } diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp index 1e8ebcdf228..505504398f9 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp @@ -50,7 +50,7 @@ auto SpeedCondition::description() const -> String } auto SpeedCondition::evaluate( - const EntityRef & triggering_entity, const Entities * entities, + const Entities * entities, const EntityRef & triggering_entity, const std::optional & direction) -> double { if (entities->isAdded(triggering_entity)) { @@ -78,7 +78,7 @@ auto SpeedCondition::evaluate() -> Object evaluations.clear(); return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - evaluations.push_back(evaluate(triggering_entity, global().entities)); + evaluations.push_back(evaluate(global().entities, triggering_entity)); return std::invoke(rule, evaluations.back(), value); })); } diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp index 214e274fcbb..0e0ed7dd509 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp @@ -102,11 +102,11 @@ auto SpeedProfileAction::run() -> void auto accomplished = [this](const auto & actor, const auto & speed_profile_entry) { if (entity_ref.empty()) { return equal_to()( - SpeedCondition::evaluate(actor, global().entities), speed_profile_entry.speed); + SpeedCondition::evaluate(global().entities, actor), speed_profile_entry.speed); } else { return equal_to()( - SpeedCondition::evaluate(actor, global().entities), - speed_profile_entry.speed + SpeedCondition::evaluate(entity_ref, global().entities)); + SpeedCondition::evaluate(global().entities, actor), + speed_profile_entry.speed + SpeedCondition::evaluate(global().entities, entity_ref)); } }; From 09de4c5df200b333133ffc293cd4c82880f7b169 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 12:12:16 +0900 Subject: [PATCH 016/149] rename argument and member variables Signed-off-by: Masaya Kataoka --- .../traffic/traffic_sink.hpp | 14 ++++++------- .../src/traffic/traffic_sink.cpp | 20 +++++++++---------- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 0754ace3f92..4409aef8d10 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -42,10 +42,10 @@ class TrafficSink : public TrafficModuleBase { public: explicit TrafficSink( - lanelet::Id lanelet_id, double radius, const geometry_msgs::msg::Point & position, - const std::function(void)> & get_entity_names_function, - const std::function & get_entity_pose_function, - const std::function & despawn_function); + const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, + const std::function(void)> & get_entity_names, + const std::function & get_entity_pose, + const std::function & despawn); const lanelet::Id lanelet_id; const double radius; const geometry_msgs::msg::Point position; @@ -54,9 +54,9 @@ class TrafficSink : public TrafficModuleBase -> void override; private: - const std::function(void)> get_entity_names_function; - const std::function get_entity_pose_function; - const std::function despawn_function; + const std::function(void)> get_entity_names; + const std::function get_entity_pose; + const std::function despawn; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index b85663f17d6..990a439b50b 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -38,28 +38,28 @@ namespace traffic_simulator namespace traffic { TrafficSink::TrafficSink( - lanelet::Id lanelet_id, double radius, const geometry_msgs::msg::Point & position, - const std::function(void)> & get_entity_names_function, - const std::function & get_entity_pose_function, - const std::function & despawn_function) + const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, + const std::function(void)> & get_entity_names, + const std::function & get_entity_pose, + const std::function & despawn) : TrafficModuleBase(), lanelet_id(lanelet_id), radius(radius), position(position), - get_entity_names_function(get_entity_names_function), - get_entity_pose_function(get_entity_pose_function), - despawn_function(despawn_function) + get_entity_names(get_entity_names), + get_entity_pose(get_entity_pose), + despawn(despawn) { } void TrafficSink::execute( [[maybe_unused]] const double current_time, [[maybe_unused]] const double step_time) { - const auto names = get_entity_names_function(); + const auto names = get_entity_names(); for (const auto & name : names) { - const auto pose = get_entity_pose_function(name); + const auto pose = get_entity_pose(name); if (math::geometry::getDistance(position, pose) <= radius) { - despawn_function(name); + despawn(name); } } } From fddf3db4c395b3158b39ac3753be601cc7dcf33d Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 12:16:47 +0900 Subject: [PATCH 017/149] rename variable in TrafficController class Signed-off-by: Masaya Kataoka --- .../traffic/traffic_controller.hpp | 12 ++++++------ .../src/traffic/traffic_controller.cpp | 15 +++++++-------- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 75c2a53ea83..f5696f42d90 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -43,9 +43,9 @@ class TrafficController public: explicit TrafficController( std::shared_ptr hdmap_utils, - const std::function(void)> & get_entity_names_function, - const std::function & get_entity_pose_function, - const std::function & despawn_function, bool auto_sink = false); + const std::function(void)> & get_entity_names, + const std::function & get_entity_pose, + const std::function & despawn, bool auto_sink = false); template void addModule(Ts &&... xs) @@ -60,9 +60,9 @@ class TrafficController void autoSink(); const std::shared_ptr hdmap_utils_; std::vector> modules_; - const std::function(void)> get_entity_names_function; - const std::function get_entity_pose_function; - const std::function despawn_function; + const std::function(void)> get_entity_names; + const std::function get_entity_pose; + const std::function despawn; public: const bool auto_sink; diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index cd8ab6d5a37..cc11ab58661 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -38,13 +38,13 @@ namespace traffic { TrafficController::TrafficController( std::shared_ptr hdmap_utils, - const std::function(void)> & get_entity_names_function, - const std::function & get_entity_pose_function, - const std::function & despawn_function, bool auto_sink) + const std::function(void)> & get_entity_names, + const std::function & get_entity_pose, + const std::function & despawn, bool auto_sink) : hdmap_utils_(hdmap_utils), - get_entity_names_function(get_entity_names_function), - get_entity_pose_function(get_entity_pose_function), - despawn_function(despawn_function), + get_entity_names(get_entity_names), + get_entity_pose(get_entity_pose), + despawn(despawn), auto_sink(auto_sink) { if (auto_sink) { @@ -61,8 +61,7 @@ void TrafficController::autoSink() 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); + lanelet_id, 1, pose.position, get_entity_names, get_entity_pose, despawn); } } } From 1d622794d2d0e86b8d8fa5321efcd17e0307c943 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 12:36:22 +0900 Subject: [PATCH 018/149] add get_entity_type function Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/api/api.hpp | 7 +++++++ .../traffic_simulator/traffic/traffic_controller.hpp | 2 ++ .../include/traffic_simulator/traffic/traffic_sink.hpp | 3 +++ .../traffic_simulator/src/traffic/traffic_controller.cpp | 5 ++++- simulation/traffic_simulator/src/traffic/traffic_sink.cpp | 2 ++ 5 files changed, 18 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 1d5f44795fa..60daaa9498e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -75,6 +75,13 @@ class API getROS2Parameter("architecture_type", "awf/universe"))), traffic_controller_ptr_(std::make_shared( entity_manager_ptr_->getHdmapUtils(), [this]() { return API::getEntityNames(); }, + [this](const auto & entity_name) { + if (const auto entity = getEntity(entity_name)) { + return entity->getEntityType(); + } else { + THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); + } + }, [this](const auto & entity_name) { if (const auto entity = getEntity(entity_name)) { return entity->getMapPose(); diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index f5696f42d90..0f5832e32ee 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -44,6 +44,7 @@ class TrafficController explicit TrafficController( std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, + const std::function & get_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink = false); @@ -61,6 +62,7 @@ class TrafficController const std::shared_ptr hdmap_utils_; std::vector> modules_; const std::function(void)> get_entity_names; + const std::function get_entity_type; const std::function get_entity_pose; const std::function despawn; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 4409aef8d10..3d51deeb299 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include #include @@ -44,6 +45,7 @@ class TrafficSink : public TrafficModuleBase explicit TrafficSink( const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, + const std::function & get_entity_type, const std::function & get_entity_pose, const std::function & despawn); const lanelet::Id lanelet_id; @@ -55,6 +57,7 @@ class TrafficSink : public TrafficModuleBase private: const std::function(void)> get_entity_names; + const std::function get_entity_type; const std::function get_entity_pose; const std::function despawn; }; diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index cc11ab58661..8f29625ec8f 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -39,10 +40,12 @@ namespace traffic TrafficController::TrafficController( std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, + const std::function & get_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink) : hdmap_utils_(hdmap_utils), get_entity_names(get_entity_names), + get_entity_type(get_entity_type), get_entity_pose(get_entity_pose), despawn(despawn), auto_sink(auto_sink) @@ -61,7 +64,7 @@ void TrafficController::autoSink() 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, get_entity_pose, despawn); + lanelet_id, 1, pose.position, get_entity_names, get_entity_type, get_entity_pose, despawn); } } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 990a439b50b..9a4745217ee 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -40,6 +40,7 @@ namespace traffic TrafficSink::TrafficSink( const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, + const std::function & get_entity_type, const std::function & get_entity_pose, const std::function & despawn) : TrafficModuleBase(), @@ -47,6 +48,7 @@ TrafficSink::TrafficSink( radius(radius), position(position), get_entity_names(get_entity_names), + get_entity_type(get_entity_type), get_entity_pose(get_entity_pose), despawn(despawn) { From 4824083a04afa8203467c9beb9da6679585dc211 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 12:45:46 +0900 Subject: [PATCH 019/149] add sinkable_entity_type variable Signed-off-by: Masaya Kataoka --- .../traffic_simulator/include/traffic_simulator/api/api.hpp | 2 ++ .../include/traffic_simulator/traffic/traffic_controller.hpp | 3 +++ .../include/traffic_simulator/traffic/traffic_sink.hpp | 2 ++ .../traffic_simulator/src/traffic/traffic_controller.cpp | 5 ++++- simulation/traffic_simulator/src/traffic/traffic_sink.cpp | 3 +++ 5 files changed, 14 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 60daaa9498e..e2ef5ed517b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -82,6 +83,7 @@ class API THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); } }, + std::set({}), [this](const auto & entity_name) { if (const auto entity = getEntity(entity_name)) { return entity->getMapPose(); diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 0f5832e32ee..3d2f86a0c44 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -27,6 +27,7 @@ #define TRAFFIC_SIMULATOR__TRAFFIC__TRAFFIC_CONTROLLER_HPP_ #include +#include #include #include #include @@ -45,6 +46,7 @@ class TrafficController std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, const std::function & get_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink = false); @@ -63,6 +65,7 @@ class TrafficController std::vector> modules_; const std::function(void)> get_entity_names; const std::function get_entity_type; + const std::set sinkable_entity_type; const std::function get_entity_pose; const std::function despawn; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 3d51deeb299..0b1a9246252 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -46,6 +46,7 @@ class TrafficSink : public TrafficModuleBase const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, const std::function & get_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn); const lanelet::Id lanelet_id; @@ -58,6 +59,7 @@ class TrafficSink : public TrafficModuleBase private: const std::function(void)> get_entity_names; const std::function get_entity_type; + const std::set sinkable_entity_type; const std::function get_entity_pose; const std::function despawn; }; diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 8f29625ec8f..b06c0670828 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -41,11 +41,13 @@ TrafficController::TrafficController( std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, const std::function & get_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink) : hdmap_utils_(hdmap_utils), get_entity_names(get_entity_names), get_entity_type(get_entity_type), + sinkable_entity_type(sinkable_entity_type), get_entity_pose(get_entity_pose), despawn(despawn), auto_sink(auto_sink) @@ -64,7 +66,8 @@ void TrafficController::autoSink() 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, get_entity_type, get_entity_pose, despawn); + lanelet_id, 1, pose.position, get_entity_names, get_entity_type, sinkable_entity_type, + get_entity_pose, despawn); } } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 9a4745217ee..4cb1fa7ba78 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,7 @@ TrafficSink::TrafficSink( const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, const std::function & get_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn) : TrafficModuleBase(), @@ -49,6 +51,7 @@ TrafficSink::TrafficSink( position(position), get_entity_names(get_entity_names), get_entity_type(get_entity_type), + sinkable_entity_type(sinkable_entity_type), get_entity_pose(get_entity_pose), despawn(despawn) { From 61577fc51ea4dc019e531da312c5c2f685726e68 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 12:51:45 +0900 Subject: [PATCH 020/149] add sinkable_entity_type to the configuration class Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/api/configuration.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index 155fab255b9..adf967debf0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -21,7 +21,9 @@ #include #include #include +#include #include +#include namespace traffic_simulator { @@ -33,6 +35,8 @@ struct Configuration bool auto_sink = true; + const std::set sinkable_entity_type = {}; + bool verbose = false; bool standalone_mode = false; From 25b59a5dac02e3a48311c328ebcd8c37a8b76dc6 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 13:14:55 +0900 Subject: [PATCH 021/149] add doxygen comment Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/api/api.hpp | 2 +- .../traffic_simulator/traffic/traffic_sink.hpp | 12 ++++++++++++ .../traffic_simulator/src/traffic/traffic_sink.cpp | 3 +++ 3 files changed, 16 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index e2ef5ed517b..a30855dc2cc 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -83,7 +83,7 @@ class API THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); } }, - std::set({}), + configuration.sinkable_entity_type, [this](const auto & entity_name) { if (const auto entity = getEntity(entity_name)) { return entity->getMapPose(); diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 0b1a9246252..679e33b53dd 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -42,6 +42,18 @@ namespace traffic class TrafficSink : public TrafficModuleBase { public: + /** + * @brief Construct a new Traffic Sink object + * @param lanelet_id Lanelet ID for visualization + * @todo lanelet_id value is only used for visualization and its very confusing. So it should be refactor. + * @param radius Entity is despawned when the distance between the entity's coordinates in the Map coordinate system and the TrafficSink's coordinates is less than this value. + * @param position Position of the traffic sink. + * @param get_entity_names Function to get the name of entity + * @param get_entity_type Function to get the type of entity + * @param sinkable_entity_type If this type is applicable, the entity is dewpanned only when it approaches radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. + * @param get_entity_pose Function to get the pose of entity. + * @param despawn Function to despawn entity. + */ explicit TrafficSink( const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 4cb1fa7ba78..0c115120247 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -62,6 +62,9 @@ void TrafficSink::execute( { const auto names = get_entity_names(); for (const auto & name : names) { + const auto is_sinkable_entity = [](const auto) { + + }; const auto pose = get_entity_pose(name); if (math::geometry::getDistance(position, pose) <= radius) { despawn(name); From f7a2b9280da556d55a35bdc8e0fbbdb0a9141bfd Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 13:54:07 +0900 Subject: [PATCH 022/149] enable compare entity type Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/api/configuration.hpp | 2 +- .../traffic_simulator/data_type/entity_status.hpp | 8 ++++++++ .../traffic_simulator/traffic/traffic_controller.hpp | 4 ++-- .../traffic_simulator/traffic/traffic_sink.hpp | 4 ++-- .../src/traffic/traffic_controller.cpp | 2 +- .../traffic_simulator/src/traffic/traffic_sink.cpp | 11 +++++++---- 6 files changed, 21 insertions(+), 10 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index adf967debf0..bebbdc5d049 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -35,7 +35,7 @@ struct Configuration bool auto_sink = true; - const std::set sinkable_entity_type = {}; + const std::set sinkable_entity_type = {}; bool verbose = false; 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 68a781808a3..94d13834eba 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 @@ -26,6 +26,14 @@ using EntityStatus = traffic_simulator_msgs::msg::EntityStatus; using EntityType = traffic_simulator_msgs::msg::EntityType; using EntitySubtype = traffic_simulator_msgs::msg::EntitySubtype; +struct EntityTypeComparator +{ + bool operator()(const EntityType & lhs, const EntityType & rhs) const + { + return lhs.type < rhs.type; + } +}; + inline namespace entity_status { class CanonicalizedEntityStatus diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 3d2f86a0c44..0b38cb03352 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -46,7 +46,7 @@ class TrafficController std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink = false); @@ -65,7 +65,7 @@ class TrafficController std::vector> modules_; const std::function(void)> get_entity_names; const std::function get_entity_type; - const std::set sinkable_entity_type; + const std::set sinkable_entity_type; const std::function get_entity_pose; const std::function despawn; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 679e33b53dd..32e85825cc5 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -58,7 +58,7 @@ class TrafficSink : public TrafficModuleBase const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn); const lanelet::Id lanelet_id; @@ -71,7 +71,7 @@ class TrafficSink : public TrafficModuleBase private: const std::function(void)> get_entity_names; const std::function get_entity_type; - const std::set sinkable_entity_type; + const std::set sinkable_entity_type; const std::function get_entity_pose; const std::function despawn; }; diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index b06c0670828..67efe0189cf 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -41,7 +41,7 @@ TrafficController::TrafficController( std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink) : hdmap_utils_(hdmap_utils), diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 0c115120247..d5a38c38bd6 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -42,7 +42,7 @@ TrafficSink::TrafficSink( const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn) : TrafficModuleBase(), @@ -62,11 +62,14 @@ void TrafficSink::execute( { const auto names = get_entity_names(); for (const auto & name : names) { - const auto is_sinkable_entity = [](const auto) { - + const auto is_sinkable_entity = [this](const auto & entity_name) { + return sinkable_entity_type.empty() ? true : [this](const auto & entity_name) { + return sinkable_entity_type.find(get_entity_type(entity_name)) != + sinkable_entity_type.end(); + }(entity_name); }; const auto pose = get_entity_pose(name); - if (math::geometry::getDistance(position, pose) <= radius) { + if (is_sinkable_entity(name) and math::geometry::getDistance(position, pose) <= radius) { despawn(name); } } From 243a1d17362dee41fde1ca57c10c11ed2fe984a7 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 14:09:25 +0900 Subject: [PATCH 023/149] enable set traffic sink in cpp scenario Signed-off-by: Masaya Kataoka --- mock/cpp_mock_scenarios/CMakeLists.txt | 3 +- .../cpp_mock_scenarios/cpp_scenario_node.hpp | 6 +- .../src/traffic_sink/CMakeLists.txt | 14 ++++ .../src/traffic_sink/auto_sink_vehicle.cpp | 71 +++++++++++++++++++ .../traffic_simulator/api/configuration.hpp | 2 +- 5 files changed, 93 insertions(+), 3 deletions(-) create mode 100644 mock/cpp_mock_scenarios/src/traffic_sink/CMakeLists.txt create mode 100644 mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp diff --git a/mock/cpp_mock_scenarios/CMakeLists.txt b/mock/cpp_mock_scenarios/CMakeLists.txt index 669465e6482..883da343c49 100644 --- a/mock/cpp_mock_scenarios/CMakeLists.txt +++ b/mock/cpp_mock_scenarios/CMakeLists.txt @@ -55,8 +55,9 @@ if(BUILD_CPP_MOCK_SCENARIOS) add_subdirectory(src/random_scenario) add_subdirectory(src/spawn) add_subdirectory(src/speed_planning) - add_subdirectory(src/traffic_source) add_subdirectory(src/synchronized_action) + add_subdirectory(src/traffic_sink) + add_subdirectory(src/traffic_source) # add_subdirectory(src/respawn_ego) endif() 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 4d1141c8fc7..925c2661170 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 @@ -71,7 +71,9 @@ class CppScenarioNode : public rclcpp::Node int timeout_; auto configure( const std::string & map_path, const std::string & lanelet2_map_file, - const std::string & scenario_filename, const bool verbose) -> traffic_simulator::Configuration + const std::string & scenario_filename, const bool verbose, const bool auto_sink = false, + const std::set + sinkable_entity_type = {}) -> traffic_simulator::Configuration { auto configuration = traffic_simulator::Configuration(map_path); { @@ -79,6 +81,8 @@ class CppScenarioNode : public rclcpp::Node // configuration.lanelet2_map_file = "lanelet2_map_with_private_road_and_walkway_ele_fix.osm"; configuration.scenario_path = scenario_filename; configuration.verbose = verbose; + configuration.auto_sink = auto_sink; + configuration.sinkable_entity_type = sinkable_entity_type; } checkConfiguration(configuration); return configuration; diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/CMakeLists.txt b/mock/cpp_mock_scenarios/src/traffic_sink/CMakeLists.txt new file mode 100644 index 00000000000..e1966600cb5 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/traffic_sink/CMakeLists.txt @@ -0,0 +1,14 @@ +ament_auto_add_executable(auto_sink_vehicle + auto_sink_vehicle.cpp +) +target_link_libraries(auto_sink_vehicle cpp_scenario_node) + +install(TARGETS + auto_sink_vehicle + DESTINATION lib/cpp_mock_scenarios +) + +if(BUILD_TESTING) + include(../../cmake/add_cpp_mock_scenario_test.cmake) + add_cpp_mock_scenario_test(${PROJECT_NAME} "auto_sink_vehicle" "5.0") +endif() diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp new file mode 100644 index 00000000000..f5c767829e1 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -0,0 +1,71 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cpp_mock_scenarios +{ +class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit AutoSinkVehicleScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "auto_sink_vehicle", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", + "lanelet2_map.osm", __FILE__, false, option) + { + start(); + } + +private: + void onUpdate() override + { + const auto map_pose = traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 120545, 0.0, 0.0, api_.getHdmapUtils())); + if (api_.reachPosition("ego", map_pose, 0.1)) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } else { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + api_.spawn( + "ego", + traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 120545, 0.0, 0.0, api_.getHdmapUtils())), + getVehicleParameters()); + } +}; +} // namespace cpp_mock_scenarios + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index bebbdc5d049..3cfca6510ab 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -35,7 +35,7 @@ struct Configuration bool auto_sink = true; - const std::set sinkable_entity_type = {}; + std::set sinkable_entity_type = {}; bool verbose = false; From 22c447028dd896bdcab18b1c0cee13569dbf70b9 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 14:19:17 +0900 Subject: [PATCH 024/149] enable sink vehicle Signed-off-by: Masaya Kataoka --- .../include/cpp_mock_scenarios/cpp_scenario_node.hpp | 4 +++- mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp | 8 ++++++-- .../src/traffic_sink/auto_sink_vehicle.cpp | 4 +++- 3 files changed, 12 insertions(+), 4 deletions(-) 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 925c2661170..92246fd7ae8 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 @@ -33,7 +33,9 @@ class CppScenarioNode : public rclcpp::Node explicit CppScenarioNode( const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, - const bool verbose, const rclcpp::NodeOptions & option); + const bool verbose, const rclcpp::NodeOptions & option, const bool auto_sink = false, + const std::set + sinkable_entity_type = {}); void start(); void stop(Result result, const std::string & description = ""); void expectThrow() { exception_expect_ = true; } diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index 7864980bdab..bd2d4ba1b2a 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -20,10 +20,14 @@ namespace cpp_mock_scenarios CppScenarioNode::CppScenarioNode( const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, - const rclcpp::NodeOptions & option) + const rclcpp::NodeOptions & option, const bool auto_sink, + const std::set + sinkable_entity_type) : Node(node_name, option), api_( - this, configure(map_path, lanelet2_map_file, scenario_filename, verbose), + this, + configure( + map_path, lanelet2_map_file, scenario_filename, verbose, auto_sink, sinkable_entity_type), declare_parameter("global_real_time_factor", 1.0), declare_parameter("global_frame_rate", 20.0)), scenario_filename_(scenario_filename), diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index f5c767829e1..cbaa9ca9278 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -30,7 +30,9 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode explicit AutoSinkVehicleScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "auto_sink_vehicle", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option) + "lanelet2_map.osm", __FILE__, false, option, true, + {traffic_simulator_msgs::build().type( + traffic_simulator::EntityType::VEHICLE)}) { start(); } From ff273988855e1929596d9464a71813a0dfc33c68 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 14:29:29 +0900 Subject: [PATCH 025/149] add testcase for autosink Signed-off-by: Masaya Kataoka --- .../src/traffic_sink/auto_sink_vehicle.cpp | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index cbaa9ca9278..4f4a55a7a72 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -40,13 +40,12 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - const auto map_pose = traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils())); - if (api_.reachPosition("ego", map_pose, 0.1)) { - stop(cpp_mock_scenarios::Result::SUCCESS); - } else { - stop(cpp_mock_scenarios::Result::FAILURE); + if (api_.getCurrentTime() >= 0.1) { + if (api_.entityExists("ego") && !api_.entityExists("ego")) { + stop(cpp_mock_scenarios::Result::FAILURE); + } else { + stop(cpp_mock_scenarios::Result::SUCCESS); + } } } @@ -56,8 +55,14 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode "ego", traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils())), + 34774, 11.0, 0.0, api_.getHdmapUtils())), getVehicleParameters()); + api_.spawn( + "bob", + traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34774, 11.0, 0.0, api_.getHdmapUtils())), + getPedestrianParameters()); } }; } // namespace cpp_mock_scenarios From c15b04eedfb474f220f0a349d4ec8a7bbbf72346 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 14:32:26 +0900 Subject: [PATCH 026/149] fix typo Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/traffic/traffic_sink.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 32e85825cc5..c40086dfcf3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -50,7 +50,7 @@ class TrafficSink : public TrafficModuleBase * @param position Position of the traffic sink. * @param get_entity_names Function to get the name of entity * @param get_entity_type Function to get the type of entity - * @param sinkable_entity_type If this type is applicable, the entity is dewpanned only when it approaches radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. + * @param sinkable_entity_type If this type is applicable, the entity is despawned only when it approaches radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. * @param get_entity_pose Function to get the pose of entity. * @param despawn Function to despawn entity. */ From 9d3a6269c67a1e709ed069a45e9aeb16b05fa803 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 14:38:29 +0900 Subject: [PATCH 027/149] fix typo Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/traffic/traffic_sink.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index c40086dfcf3..55e40f6bc70 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -46,11 +46,11 @@ class TrafficSink : public TrafficModuleBase * @brief Construct a new Traffic Sink object * @param lanelet_id Lanelet ID for visualization * @todo lanelet_id value is only used for visualization and its very confusing. So it should be refactor. - * @param radius Entity is despawned when the distance between the entity's coordinates in the Map coordinate system and the TrafficSink's coordinates is less than this value. + * @param radius The entity despawns when the distance between the entity's coordinates in the Map coordinate system and the TrafficSink's coordinates is less than this value. * @param position Position of the traffic sink. * @param get_entity_names Function to get the name of entity * @param get_entity_type Function to get the type of entity - * @param sinkable_entity_type If this type is applicable, the entity is despawned only when it approaches radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. + * @param sinkable_entity_type If this type is applicable, the entity despawn only when it approaches radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. * @param get_entity_pose Function to get the pose of entity. * @param despawn Function to despawn entity. */ From 8dc26c460cdd772a3b460bf72bd555848a07e506 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 14:45:29 +0900 Subject: [PATCH 028/149] fix typo Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/traffic/traffic_sink.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 55e40f6bc70..752f9288a1a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -46,7 +46,7 @@ class TrafficSink : public TrafficModuleBase * @brief Construct a new Traffic Sink object * @param lanelet_id Lanelet ID for visualization * @todo lanelet_id value is only used for visualization and its very confusing. So it should be refactor. - * @param radius The entity despawns when the distance between the entity's coordinates in the Map coordinate system and the TrafficSink's coordinates is less than this value. + * @param radius The entity despawn when the distance between the entity's coordinates in the Map coordinate system and the TrafficSink's coordinates is less than this value. * @param position Position of the traffic sink. * @param get_entity_names Function to get the name of entity * @param get_entity_type Function to get the type of entity From db654279660be5a9385d7b40db2b350918184822 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Mon, 11 Nov 2024 12:26:52 +0900 Subject: [PATCH 029/149] modify launch file Signed-off-by: Masaya Kataoka --- .../launch/mock_test.launch.py | 76 ++++++++++++++----- 1 file changed, 56 insertions(+), 20 deletions(-) diff --git a/mock/cpp_mock_scenarios/launch/mock_test.launch.py b/mock/cpp_mock_scenarios/launch/mock_test.launch.py index 0289e874095..0aa7d1ef1a7 100644 --- a/mock/cpp_mock_scenarios/launch/mock_test.launch.py +++ b/mock/cpp_mock_scenarios/launch/mock_test.launch.py @@ -30,7 +30,13 @@ from launch.events import Shutdown from launch.event_handlers import OnProcessExit, OnProcessIO -from launch.actions import EmitEvent, RegisterEventHandler, LogInfo, TimerAction, OpaqueFunction +from launch.actions import ( + EmitEvent, + RegisterEventHandler, + LogInfo, + TimerAction, + OpaqueFunction, +) from launch.actions.declare_launch_argument import DeclareLaunchArgument from launch.substitutions.launch_configuration import LaunchConfiguration @@ -40,6 +46,7 @@ from pathlib import Path + class Color: BLACK = "\033[30m" RED = "\033[31m" @@ -69,6 +76,7 @@ def on_stdout_output(event: launch.Event) -> None: if lines[0] == "cpp_scenario:success": print(Color.GREEN + "Scenario Succeed" + Color.END) + def architecture_types(): return ["awf/universe", "awf/universe/20230906"] @@ -94,8 +102,13 @@ def default_autoware_launch_file_of(architecture_type): "awf/universe/20230906": "planning_simulator.launch.xml", }[architecture_type] + def default_rviz_config_file(): - return Path(get_package_share_directory("traffic_simulator")) / "config/scenario_simulator_v2.rviz" + return ( + Path(get_package_share_directory("traffic_simulator")) + / "config/scenario_simulator_v2.rviz" + ) + def launch_setup(context, *args, **kwargs): # fmt: off @@ -125,20 +138,38 @@ def launch_setup(context, *args, **kwargs): junit_path = LaunchConfiguration("junit_path", default="/tmp/output.xunit.xml") # fmt: on - print(f"architecture_type := {architecture_type.perform(context)}") - print(f"autoware_launch_file := {autoware_launch_file.perform(context)}") - print(f"autoware_launch_package := {autoware_launch_package.perform(context)}") - print(f"consider_acceleration_by_road_slope := {consider_acceleration_by_road_slope.perform(context)}") - print(f"consider_pose_by_road_slope := {consider_pose_by_road_slope.perform(context)}") - print(f"global_frame_rate := {global_frame_rate.perform(context)}") - print(f"global_real_time_factor := {global_real_time_factor.perform(context)}") + print( + f"architecture_type := {architecture_type.perform(context)}" + ) + print( + f"autoware_launch_file := {autoware_launch_file.perform(context)}" + ) + print( + f"autoware_launch_package := {autoware_launch_package.perform(context)}" + ) + print( + f"consider_acceleration_by_road_slope := {consider_acceleration_by_road_slope.perform(context)}" + ) + print( + f"consider_pose_by_road_slope := {consider_pose_by_road_slope.perform(context)}" + ) + print( + f"global_frame_rate := {global_frame_rate.perform(context)}" + ) + print( + f"global_real_time_factor := {global_real_time_factor.perform(context)}" + ) print(f"global_timeout := {global_timeout.perform(context)}") - print(f"initialize_duration := {initialize_duration.perform(context)}") + print( + f"initialize_duration := {initialize_duration.perform(context)}" + ) print(f"launch_autoware := {launch_autoware.perform(context)}") print(f"launch_rviz := {launch_rviz.perform(context)}") print(f"output_directory := {output_directory.perform(context)}") print(f"port := {port.perform(context)}") - print(f"publish_empty_context := {publish_empty_context.perform(context)}") + print( + f"publish_empty_context := {publish_empty_context.perform(context)}" + ) print(f"record := {record.perform(context)}") print(f"rviz_config := {rviz_config.perform(context)}") print(f"scenario := {scenario.perform(context)}") @@ -154,12 +185,14 @@ def make_parameters(): {"architecture_type": architecture_type}, {"autoware_launch_file": autoware_launch_file}, {"autoware_launch_package": autoware_launch_package}, - {"consider_acceleration_by_road_slope": consider_acceleration_by_road_slope}, + { + "consider_acceleration_by_road_slope": consider_acceleration_by_road_slope + }, {"consider_pose_by_road_slope": consider_pose_by_road_slope}, {"initialize_duration": initialize_duration}, {"launch_autoware": launch_autoware}, {"port": port}, - {"publish_empty_context" : publish_empty_context}, + {"publish_empty_context": publish_empty_context}, {"record": record}, {"rviz_config": rviz_config}, {"sensor_model": sensor_model}, @@ -187,13 +220,13 @@ def description(): return parameters cpp_scenario_node = Node( - package=scenario_package, - executable=scenario, - name="scenario_node", - output="screen", - arguments=[("__log_level:=info")], - parameters=make_parameters() + [{"use_sim_time": use_sim_time}], - ) + package=scenario_package, + executable=scenario, + name="scenario_node", + output="screen", + arguments=[("__log_level:=info")], + parameters=make_parameters() + [{"use_sim_time": use_sim_time}], + ) io_handler = OnProcessIO( target_action=cpp_scenario_node, on_stderr=on_stderr_output, @@ -241,6 +274,7 @@ def description(): namespace="simulation", name="visualizer", output="screen", + remappings=[("/simulation/entity/status", "/entity/status")], ), Node( package="rviz2", @@ -249,10 +283,12 @@ def description(): output={"stderr": "log", "stdout": "log"}, condition=IfCondition(launch_rviz), arguments=["-d", str(default_rviz_config_file())], + remappings=[("/simulation/lanelet/marker", "/lanelet/marker")], ), RegisterEventHandler(event_handler=io_handler), RegisterEventHandler(event_handler=shutdown_handler), ] + def generate_launch_description(): return LaunchDescription([OpaqueFunction(function=launch_setup)]) From 91b19d35e3ac19763738911807104f1a37554f3a Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Mon, 11 Nov 2024 12:43:55 +0900 Subject: [PATCH 030/149] remap debug marker Signed-off-by: Masaya Kataoka --- mock/cpp_mock_scenarios/launch/mock_test.launch.py | 5 ++++- mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/mock/cpp_mock_scenarios/launch/mock_test.launch.py b/mock/cpp_mock_scenarios/launch/mock_test.launch.py index 0aa7d1ef1a7..b618c0e56ca 100644 --- a/mock/cpp_mock_scenarios/launch/mock_test.launch.py +++ b/mock/cpp_mock_scenarios/launch/mock_test.launch.py @@ -283,7 +283,10 @@ def description(): output={"stderr": "log", "stdout": "log"}, condition=IfCondition(launch_rviz), arguments=["-d", str(default_rviz_config_file())], - remappings=[("/simulation/lanelet/marker", "/lanelet/marker")], + remappings=[ + ("/simulation/lanelet/marker", "/lanelet/marker"), + ("/simulation/debug_marker", "/debug_marker"), + ], ), RegisterEventHandler(event_handler=io_handler), RegisterEventHandler(event_handler=shutdown_handler), diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index 74dbf25598b..c0d70ef03e7 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -30,7 +30,7 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode explicit StopAtCrosswalkScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "stop_at_crosswalk", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option) + "lanelet2_map.osm", __FILE__, false, option, true) { start(); } From b67579782898863834858c6a1acc26ceb5e98928 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 11 Nov 2024 12:16:28 +0900 Subject: [PATCH 031/149] Add launch argument `simulate_localization` to `scenario_test_runner` Signed-off-by: yamacir-kit --- .../include/concealer/autoware_universe.hpp | 3 +- external/concealer/src/autoware_universe.cpp | 31 +++++++++++++++++-- .../ego_entity_simulation.cpp | 3 +- .../src/entity/ego_entity.cpp | 1 + .../launch/scenario_test_runner.launch.py | 4 +++ 5 files changed, 37 insertions(+), 5 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 39d4b3f576b..01067a3f7f3 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -44,6 +44,7 @@ class AutowareUniverse : public Autoware PublisherWrapper setAcceleration; PublisherWrapper setOdometry; + PublisherWrapper setPose; PublisherWrapper setSteeringReport; PublisherWrapper setGearReport; PublisherWrapper setControlModeReport; @@ -69,7 +70,7 @@ class AutowareUniverse : public Autoware auto stopAndJoin() -> void; public: - CONCEALER_PUBLIC explicit AutowareUniverse(); + CONCEALER_PUBLIC explicit AutowareUniverse(bool); ~AutowareUniverse(); diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index b22f2c68c9b..4fc18db8ff8 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -16,15 +16,25 @@ namespace concealer { -AutowareUniverse::AutowareUniverse() +AutowareUniverse::AutowareUniverse(bool simulate_localization) : getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), getGearCommandImpl("/control/command/gear_cmd", rclcpp::QoS(1), *this), getTurnIndicatorsCommand("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), getPathWithLaneId( "/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), *this), - setAcceleration("/localization/acceleration", *this), - setOdometry("/localization/kinematic_state", *this), + setAcceleration( + simulate_localization ? "/localization/acceleration" + : "/simulation/debug/localization/acceleration", + *this), + setOdometry( + simulate_localization ? "/localization/kinematic_state" + : "/simulation/debug/localization/kinematic_state", + *this), + setPose( + simulate_localization ? "/simulation/debug/localization/pose_estimator/pose_with_covariance" + : "/localization/pose_estimator/pose_with_covariance", + *this), setSteeringReport("/vehicle/status/steering_status", *this), setGearReport("/vehicle/status/gear_status", *this), setControlModeReport("/vehicle/status/control_mode", *this), @@ -102,6 +112,21 @@ auto AutowareUniverse::updateLocalization() -> void return message; }()); + setPose([this]() { + // See https://github.com/tier4/autoware.universe/blob/45ab20af979c5663e5a8d4dda787b1dea8d6e55b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L785-L803 + geometry_msgs::msg::PoseWithCovarianceStamped message; + message.header.stamp = get_clock()->now(); + message.header.frame_id = "map"; + message.pose.pose = current_pose.load(); + message.pose.covariance.at(6 * 0 + 0) = 0.0225; // XYZRPY_COV_IDX::X_X + message.pose.covariance.at(6 * 1 + 1) = 0.0225; // XYZRPY_COV_IDX::Y_Y + message.pose.covariance.at(6 * 2 + 2) = 0.0225; // XYZRPY_COV_IDX::Z_Z + message.pose.covariance.at(6 * 3 + 3) = 0.000625; // XYZRPY_COV_IDX::ROLL_ROLL + message.pose.covariance.at(6 * 4 + 4) = 0.000625; // XYZRPY_COV_IDX::PITCH_PITCH + message.pose.covariance.at(6 * 5 + 5) = 0.000625; // XYZRPY_COV_IDX::YAW_YAW + return message; + }()); + setTransform(current_pose.load()); } 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 23ac33eeb3f..7ce88f75de3 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 @@ -41,7 +41,8 @@ EgoEntitySimulation::EgoEntitySimulation( const traffic_simulator_msgs::msg::VehicleParameters & parameters, double step_time, const std::shared_ptr & hdmap_utils, const rclcpp::Parameter & use_sim_time, const bool consider_acceleration_by_road_slope) -: autoware(std::make_unique()), +: autoware( + std::make_unique(getParameter("simulate_localization"))), vehicle_model_type_(getVehicleModelType()), vehicle_model_ptr_(makeSimulationModel(vehicle_model_type_, step_time, parameters)), status_(initial_status, std::nullopt), diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 72d29a96de8..332b26723f5 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -54,6 +54,7 @@ auto EgoEntity::makeFieldOperatorApplication( 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")); + parameters.push_back("localization_sim_mode:=" + std::string(getParameter(node_parameters, "simulate_localization") ? "api" : "pose_twist_estimator")); // clang-format on return getParameter(node_parameters, "launch_autoware", true) 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 b6cb6b9d66a..28daf6722c1 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 @@ -85,6 +85,7 @@ def launch_setup(context, *args, **kwargs): scenario = LaunchConfiguration("scenario", default=Path("/dev/null")) sensor_model = LaunchConfiguration("sensor_model", default="") sigterm_timeout = LaunchConfiguration("sigterm_timeout", default=8) + simulate_localization = LaunchConfiguration("simulate_localization", default=True) use_sim_time = LaunchConfiguration("use_sim_time", default=False) vehicle_model = LaunchConfiguration("vehicle_model", default="") # fmt: on @@ -109,6 +110,7 @@ def launch_setup(context, *args, **kwargs): print(f"scenario := {scenario.perform(context)}") print(f"sensor_model := {sensor_model.perform(context)}") print(f"sigterm_timeout := {sigterm_timeout.perform(context)}") + print(f"simulate_localization := {simulate_localization.perform(context)}") print(f"use_sim_time := {use_sim_time.perform(context)}") print(f"vehicle_model := {vehicle_model.perform(context)}") @@ -133,6 +135,7 @@ def make_parameters(): {"rviz_config": rviz_config}, {"sensor_model": sensor_model}, {"sigterm_timeout": sigterm_timeout}, + {"simulate_localization": simulate_localization}, {"use_sim_time": use_sim_time}, {"vehicle_model": vehicle_model}, ] @@ -177,6 +180,7 @@ def collect_prefixed_parameters(): DeclareLaunchArgument("scenario", default_value=scenario ), DeclareLaunchArgument("sensor_model", default_value=sensor_model ), DeclareLaunchArgument("sigterm_timeout", default_value=sigterm_timeout ), + DeclareLaunchArgument("simulate_localization", default_value=simulate_localization ), DeclareLaunchArgument("use_sim_time", default_value=use_sim_time ), DeclareLaunchArgument("vehicle_model", default_value=vehicle_model ), # fmt: on From ba75cc2570933a4d36803d8b66e519b19a930617 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Tue, 12 Nov 2024 15:16:01 +0900 Subject: [PATCH 032/149] use uint8_t instead of traffic_simulator_msgs::msg::EntityType Signed-off-by: Masaya Kataoka --- .../cpp_mock_scenarios/cpp_scenario_node.hpp | 6 ++---- mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp | 3 +-- .../src/traffic_sink/auto_sink_vehicle.cpp | 4 +--- .../include/simulation_interface/conversions.hpp | 15 ++++++--------- .../traffic_simulator/api/configuration.hpp | 2 +- .../behavior/behavior_plugin_base.hpp | 14 +++++++------- .../traffic_simulator/data_type/entity_status.hpp | 8 -------- .../traffic/traffic_controller.hpp | 4 ++-- .../traffic_simulator/traffic/traffic_sink.hpp | 4 ++-- .../src/data_type/entity_status.cpp | 1 + .../src/traffic/traffic_controller.cpp | 2 +- .../src/traffic/traffic_sink.cpp | 4 ++-- 12 files changed, 26 insertions(+), 41 deletions(-) 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 92246fd7ae8..680e4e128ef 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 @@ -34,8 +34,7 @@ class CppScenarioNode : public rclcpp::Node const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, const rclcpp::NodeOptions & option, const bool auto_sink = false, - const std::set - sinkable_entity_type = {}); + const std::set sinkable_entity_type = {}); void start(); void stop(Result result, const std::string & description = ""); void expectThrow() { exception_expect_ = true; } @@ -74,8 +73,7 @@ class CppScenarioNode : public rclcpp::Node auto configure( const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, const bool auto_sink = false, - const std::set - sinkable_entity_type = {}) -> traffic_simulator::Configuration + const std::set sinkable_entity_type = {}) -> traffic_simulator::Configuration { auto configuration = traffic_simulator::Configuration(map_path); { diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index bd2d4ba1b2a..d49605402be 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -21,8 +21,7 @@ CppScenarioNode::CppScenarioNode( const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, const rclcpp::NodeOptions & option, const bool auto_sink, - const std::set - sinkable_entity_type) + const std::set sinkable_entity_type) : Node(node_name, option), api_( this, diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index 4f4a55a7a72..479860c3382 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -30,9 +30,7 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode explicit AutoSinkVehicleScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "auto_sink_vehicle", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option, true, - {traffic_simulator_msgs::build().type( - traffic_simulator::EntityType::VEHICLE)}) + "lanelet2_map.osm", __FILE__, false, option, true, {traffic_simulator::EntityType::VEHICLE}) { start(); } diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 4ec8320616b..aca2245de55 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -168,9 +168,9 @@ void toMsg(const rosgraph_msgs::Clock & proto, rosgraph_msgs::msg::Clock & time) void toProto(const std_msgs::msg::Header & header, std_msgs::Header & proto); void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header); -#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ - auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \ - auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void +#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ + auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &) -> void; \ + auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &) -> void DEFINE_CONVERSION(autoware_auto_control_msgs, AckermannLateralCommand); DEFINE_CONVERSION(autoware_auto_control_msgs, LongitudinalCommand); @@ -192,8 +192,7 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr - { + auto convert_color = [](auto color) constexpr { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -208,8 +207,7 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr - { + auto convert_shape = [](auto shape) constexpr { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -237,8 +235,7 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr - { + auto convert_status = [](auto status) constexpr { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index 3cfca6510ab..c2b17c99d5e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -35,7 +35,7 @@ struct Configuration bool auto_sink = true; - std::set sinkable_entity_type = {}; + std::set sinkable_entity_type = {}; bool verbose = false; 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 f4162f3e9b0..c4dac740c60 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 @@ -44,13 +44,13 @@ class BehaviorPluginBase 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) \ - virtual TYPE get##NAME() = 0; \ - virtual void set##NAME(const TYPE & value) = 0; \ - auto get##NAME##Key() const->const std::string & \ - { \ - static const std::string key = KEY; \ - return key; \ +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + virtual TYPE get##NAME() = 0; \ + virtual void set##NAME(const TYPE & value) = 0; \ + auto get##NAME##Key() const -> const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ } // clang-format off 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 94d13834eba..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 @@ -26,14 +26,6 @@ using EntityStatus = traffic_simulator_msgs::msg::EntityStatus; using EntityType = traffic_simulator_msgs::msg::EntityType; using EntitySubtype = traffic_simulator_msgs::msg::EntitySubtype; -struct EntityTypeComparator -{ - bool operator()(const EntityType & lhs, const EntityType & rhs) const - { - return lhs.type < rhs.type; - } -}; - inline namespace entity_status { class CanonicalizedEntityStatus diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 0b38cb03352..1dac416f036 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -46,7 +46,7 @@ class TrafficController std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink = false); @@ -65,7 +65,7 @@ class TrafficController std::vector> modules_; const std::function(void)> get_entity_names; const std::function get_entity_type; - const std::set sinkable_entity_type; + const std::set sinkable_entity_type; const std::function get_entity_pose; const std::function despawn; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 752f9288a1a..81fbc8f5ad8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -58,7 +58,7 @@ class TrafficSink : public TrafficModuleBase const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn); const lanelet::Id lanelet_id; @@ -71,7 +71,7 @@ class TrafficSink : public TrafficModuleBase private: const std::function(void)> get_entity_names; const std::function get_entity_type; - const std::set sinkable_entity_type; + const std::set sinkable_entity_type; const std::function get_entity_pose; const std::function despawn; }; diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index b2bfb78432e..6420e77aec3 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -17,6 +17,7 @@ namespace traffic_simulator { + inline namespace entity_status { diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 67efe0189cf..1510323a4f8 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -41,7 +41,7 @@ TrafficController::TrafficController( std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink) : hdmap_utils_(hdmap_utils), diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index d5a38c38bd6..9636a4d54d3 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -42,7 +42,7 @@ TrafficSink::TrafficSink( const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn) : TrafficModuleBase(), @@ -64,7 +64,7 @@ void TrafficSink::execute( for (const auto & name : names) { const auto is_sinkable_entity = [this](const auto & entity_name) { return sinkable_entity_type.empty() ? true : [this](const auto & entity_name) { - return sinkable_entity_type.find(get_entity_type(entity_name)) != + return sinkable_entity_type.find(get_entity_type(entity_name).type) != sinkable_entity_type.end(); }(entity_name); }; From 83409529a20d62f223b761e9cfde956fbe07044c Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Tue, 12 Nov 2024 21:40:16 +0900 Subject: [PATCH 033/149] apply reformat Signed-off-by: Masaya Kataoka --- .../include/simulation_interface/conversions.hpp | 15 +++++++++------ .../behavior/behavior_plugin_base.hpp | 14 +++++++------- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index aca2245de55..4ec8320616b 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -168,9 +168,9 @@ void toMsg(const rosgraph_msgs::Clock & proto, rosgraph_msgs::msg::Clock & time) void toProto(const std_msgs::msg::Header & header, std_msgs::Header & proto); void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header); -#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ - auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &) -> void; \ - auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &) -> void +#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ + auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \ + auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void DEFINE_CONVERSION(autoware_auto_control_msgs, AckermannLateralCommand); DEFINE_CONVERSION(autoware_auto_control_msgs, LongitudinalCommand); @@ -192,7 +192,8 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr { + auto convert_color = [](auto color) constexpr + { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -207,7 +208,8 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr { + auto convert_shape = [](auto shape) constexpr + { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -235,7 +237,8 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr { + auto convert_status = [](auto status) constexpr + { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index c4dac740c60..f4162f3e9b0 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 @@ -44,13 +44,13 @@ class BehaviorPluginBase 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) \ - virtual TYPE get##NAME() = 0; \ - virtual void set##NAME(const TYPE & value) = 0; \ - auto get##NAME##Key() const -> const std::string & \ - { \ - static const std::string key = KEY; \ - return key; \ +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + virtual TYPE get##NAME() = 0; \ + virtual void set##NAME(const TYPE & value) = 0; \ + auto get##NAME##Key() const->const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ } // clang-format off From 6d3b4bf18ca8ed54afbaf497e46b66af3f9b766a Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 13 Nov 2024 10:17:30 +0900 Subject: [PATCH 034/149] remove sonarcloud warning Signed-off-by: Masaya Kataoka --- simulation/traffic_simulator/src/traffic/traffic_sink.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 9636a4d54d3..d9f46d2097a 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -63,9 +63,8 @@ void TrafficSink::execute( const auto names = get_entity_names(); for (const auto & name : names) { const auto is_sinkable_entity = [this](const auto & entity_name) { - return sinkable_entity_type.empty() ? true : [this](const auto & entity_name) { - return sinkable_entity_type.find(get_entity_type(entity_name).type) != - sinkable_entity_type.end(); + return sinkable_entity_type.empty() ? true : [this](const auto & name) { + return sinkable_entity_type.find(get_entity_type(name).type) != sinkable_entity_type.end(); }(entity_name); }; const auto pose = get_entity_pose(name); From 56f31ccac4e5d54050dc0d2585696e304666d494 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 13 Nov 2024 12:50:28 +0900 Subject: [PATCH 035/149] remove warning Signed-off-by: Masaya Kataoka --- simulation/traffic_simulator/src/traffic/traffic_sink.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index d9f46d2097a..82096d5bb02 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -63,9 +63,10 @@ void TrafficSink::execute( const auto names = get_entity_names(); for (const auto & name : names) { const auto is_sinkable_entity = [this](const auto & entity_name) { - return sinkable_entity_type.empty() ? true : [this](const auto & name) { - return sinkable_entity_type.find(get_entity_type(name).type) != sinkable_entity_type.end(); - }(entity_name); + return sinkable_entity_type.empty() + ? true + : sinkable_entity_type.find(get_entity_type(entity_name).type) != + sinkable_entity_type.end(); }; const auto pose = get_entity_pose(name); if (is_sinkable_entity(name) and math::geometry::getDistance(position, pose) <= radius) { From 66ea1250eb711798a8d13c94f0167ff888344a16 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Thu, 14 Nov 2024 13:46:42 +0900 Subject: [PATCH 036/149] fix check condition Signed-off-by: Masaya Kataoka --- mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index 479860c3382..fe1b2110b33 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -39,7 +39,7 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { if (api_.getCurrentTime() >= 0.1) { - if (api_.entityExists("ego") && !api_.entityExists("ego")) { + if (api_.entityExists("bob") && !api_.entityExists("ego")) { stop(cpp_mock_scenarios::Result::FAILURE); } else { stop(cpp_mock_scenarios::Result::SUCCESS); From b11ca9df169194650873861d534c0416ab96be36 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 20 Nov 2024 14:30:53 +0900 Subject: [PATCH 037/149] Add the missing semicolon Signed-off-by: yamacir-kit --- external/concealer/include/concealer/autoware_universe.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 227fa89ff40..0692c7717c0 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -45,7 +45,7 @@ class AutowareUniverse : public Autoware using GearCommand = autoware_vehicle_msgs::msg::GearCommand; using GearReport = autoware_vehicle_msgs::msg::GearReport; using PathWithLaneId = tier4_planning_msgs::msg::PathWithLaneId; - using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; using SteeringReport = autoware_vehicle_msgs::msg::SteeringReport; using TurnIndicatorsCommand = autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using TurnIndicatorsReport = autoware_vehicle_msgs::msg::TurnIndicatorsReport; From 2656a5a38a964181b9605374fefceb3431f2b131 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 22 Nov 2024 10:35:28 +0900 Subject: [PATCH 038/149] Split `(Relative)?DistanceCondition::evaluate` into two overloads Signed-off-by: yamacir-kit --- .../syntax/relative_speed_condition.hpp | 7 +- .../syntax/speed_condition.hpp | 5 +- .../syntax/time_to_collision_condition.hpp | 68 +++++++++---------- .../src/syntax/relative_speed_condition.cpp | 41 ++++++----- .../src/syntax/speed_action.cpp | 11 +-- .../src/syntax/speed_condition.cpp | 40 ++++++----- .../src/syntax/speed_profile_action.cpp | 7 +- 7 files changed, 102 insertions(+), 77 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp index 5920e0ed3b7..a7e118bbb3b 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp @@ -72,9 +72,12 @@ struct RelativeSpeedCondition : private Scope, private SimulatorCore::ConditionE auto description() const -> String; + static auto evaluate(const Entities *, const Entity &, const Entity &) + -> geometry_msgs::msg::Vector3; + static auto evaluate( - const Entities *, const Entity &, const Entity &, - const std::optional & = std::nullopt) -> double; + const Entities *, const Entity &, const Entity &, const std::optional &) + -> double; auto evaluate() -> Object; }; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp index 05a87962d34..d674955a105 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp @@ -68,9 +68,10 @@ struct SpeedCondition : private Scope, private SimulatorCore::ConditionEvaluatio auto description() const -> String; + static auto evaluate(const Entities *, const Entity &) -> geometry_msgs::msg::Vector3; + static auto evaluate( - const Entities *, const Entity &, const std::optional & = std::nullopt) - -> double; + const Entities *, const Entity &, const std::optional &) -> double; auto evaluate() -> Object; }; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index 61a1b0a76b5..e6e67c8a815 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -123,40 +123,40 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, RoutingAlgorithm routing_algorithm, Boolean freespace) { - if (time_to_collision_condition_target.is()) { - const auto relative_distance = RelativeDistanceCondition::evaluate( - entities, // - triggering_entity, // - time_to_collision_condition_target.as(), // - coordinate_system, // - relative_distance_type, // - routing_algorithm, // - freespace); - - const auto relative_speed = RelativeSpeedCondition::evaluate( - entities, // - triggering_entity, // - time_to_collision_condition_target.as()); - - // std::cerr << "RELATIVE DISTANCE = " << relative_distance - // << ", RELATIVE SPEED = " << relative_speed << std::endl; - - return relative_distance / relative_speed; - } else { - const auto distance = DistanceCondition::evaluate( - entities, // - triggering_entity, // - time_to_collision_condition_target.as(), // - coordinate_system, // - relative_distance_type, // - routing_algorithm, // - freespace); - - const auto speed = SpeedCondition::evaluate(entities, triggering_entity); - - // std::cerr << "DISTANCE = " << distance << ", RELATIVE SPEED = " << speed << std::endl; - - return distance / speed; + auto distance = [&]() { + if (time_to_collision_condition_target.is()) { + return RelativeDistanceCondition::evaluate( + entities, triggering_entity, time_to_collision_condition_target.as(), + coordinate_system, relative_distance_type, routing_algorithm, freespace); + } else { + return DistanceCondition::evaluate( + entities, triggering_entity, time_to_collision_condition_target.as(), + coordinate_system, relative_distance_type, routing_algorithm, freespace); + } + }; + + auto speed = [&](auto &&... xs) { + if (time_to_collision_condition_target.is()) { + return RelativeSpeedCondition::evaluate( + entities, // + triggering_entity, // + time_to_collision_condition_target.as(), std::forward(xs)...); + } else { + return SpeedCondition::evaluate( + entities, triggering_entity, std::forward(xs)...); + } + }; + + switch (relative_distance_type) { + case RelativeDistanceType::longitudinal: + return distance() / speed(DirectionalDimension::longitudinal); + + case RelativeDistanceType::lateral: + return distance() / speed(DirectionalDimension::lateral); + + default: + case RelativeDistanceType::euclidianDistance: + return distance() / speed(std::nullopt); } } diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp index a9c6d49be05..e61c643ccf2 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp @@ -58,28 +58,37 @@ auto RelativeSpeedCondition::description() const -> String } auto RelativeSpeedCondition::evaluate( - const Entities * entities, const Entity & triggering_entity, const Entity & entity_ref, - const std::optional & direction) -> double + const Entities * entities, const Entity & triggering_entity, const Entity & entity_ref) + -> geometry_msgs::msg::Vector3 { if ( triggering_entity.apply([&](const auto & each) { return entities->isAdded(each); }).min() and entities->isAdded(entity_ref)) { - if (const auto v = evaluateRelativeSpeed(triggering_entity, entity_ref); direction) { - switch (*direction) { - case DirectionalDimension::longitudinal: - return v.x; - case DirectionalDimension::lateral: - return v.y; - case DirectionalDimension::vertical: - return v.z; - default: - return math::geometry::norm(v); - } - } else { - return math::geometry::norm(v); + return evaluateRelativeSpeed(triggering_entity, entity_ref); + } else { + return geometry_msgs::build() + .x(Double::nan()) + .y(Double::nan()) + .z(Double::nan()); + } +} + +auto RelativeSpeedCondition::evaluate( + const Entities * entities, const Entity & triggering_entity, const Entity & entity_ref, + const std::optional & direction) -> double +{ + if (const auto v = evaluate(entities, triggering_entity, entity_ref); direction) { + switch (*direction) { + default: + case DirectionalDimension::longitudinal: + return v.x; + case DirectionalDimension::lateral: + return v.y; + case DirectionalDimension::vertical: + return v.z; } } else { - return Double::nan(); + return math::geometry::norm(v); } } diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp index a0811838241..777fde3e70a 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp @@ -56,8 +56,9 @@ auto SpeedAction::accomplished() -> bool }; auto check = [this](auto && actor) { - auto evaluation = actor.apply( - [this](const auto & actor) { return SpeedCondition::evaluate(global().entities, actor); }); + auto evaluation = actor.apply([this](const auto & actor) { + return SpeedCondition::evaluate(global().entities, actor, std::nullopt); + }); if (speed_action_target.is()) { return not evaluation.size() or equal_to>()( @@ -69,7 +70,8 @@ auto SpeedAction::accomplished() -> bool return not evaluation.size() or equal_to>()( SpeedCondition::evaluate( - global().entities, speed_action_target.as().entity_ref) + + global().entities, speed_action_target.as().entity_ref, + std::nullopt) + speed_action_target.as().value, evaluation) .min(); @@ -77,7 +79,8 @@ auto SpeedAction::accomplished() -> bool return not evaluation.size() or equal_to>()( SpeedCondition::evaluate( - global().entities, speed_action_target.as().entity_ref) * + global().entities, speed_action_target.as().entity_ref, + std::nullopt) * speed_action_target.as().value, evaluation) .min(); diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp index dd4c858269b..a392505abb5 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp @@ -49,27 +49,35 @@ auto SpeedCondition::description() const -> String return description.str(); } +auto SpeedCondition::evaluate(const Entities * entities, const Entity & triggering_entity) + -> geometry_msgs::msg::Vector3 +{ + if (entities->isAdded(triggering_entity)) { + return evaluateSpeed(triggering_entity); + } else { + return geometry_msgs::build() + .x(Double::nan()) + .y(Double::nan()) + .z(Double::nan()); + } +} + auto SpeedCondition::evaluate( const Entities * entities, const Entity & triggering_entity, const std::optional & direction) -> double { - if (entities->isAdded(triggering_entity)) { - if (const auto v = evaluateSpeed(triggering_entity); direction) { - switch (*direction) { - case DirectionalDimension::longitudinal: - return v.x; - case DirectionalDimension::lateral: - return v.y; - case DirectionalDimension::vertical: - return v.z; - default: - return math::geometry::norm(v); - } - } else { - return math::geometry::norm(v); + if (const auto v = evaluate(entities, triggering_entity); direction) { + switch (*direction) { + default: + case DirectionalDimension::longitudinal: + return v.x; + case DirectionalDimension::lateral: + return v.y; + case DirectionalDimension::vertical: + return v.z; } } else { - return Double::nan(); + return math::geometry::norm(v); } } @@ -79,7 +87,7 @@ auto SpeedCondition::evaluate() -> Object return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { results.push_back(triggering_entity.apply([&](const auto & triggering_entity) { - return evaluate(global().entities, triggering_entity); + return evaluate(global().entities, triggering_entity, direction); })); return not results.back().size() or std::invoke(rule, results.back(), value).min(); })); diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp index 4e241addd8b..eff38064c90 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp @@ -121,14 +121,15 @@ auto SpeedProfileAction::run() -> void { for (auto && [actor, iter] : accomplishments) { auto accomplished = [this](const auto & actor, const auto & speed_profile_entry) { - auto speeds = actor.apply( - [&](const auto & object) { return SpeedCondition::evaluate(global().entities, object); }); + auto speeds = actor.apply([&](const auto & object) { + return SpeedCondition::evaluate(global().entities, object, std::nullopt); + }); if (not speeds.size()) { return true; } else if (entity_ref) { return equal_to>()( speeds, speed_profile_entry.speed + - SpeedCondition::evaluate(global().entities, entity_ref)) + SpeedCondition::evaluate(global().entities, entity_ref, std::nullopt)) .min(); } else { return equal_to>()(speeds, speed_profile_entry.speed).min(); From b8a9e63dd96d3ccb8cf32812f57b779ee1fdf4d5 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 25 Nov 2024 18:18:15 +0900 Subject: [PATCH 039/149] Add new member function `evaluateCartesianTimeToCollisionCondition` Signed-off-by: yamacir-kit --- .../simulator_core.hpp | 125 +++++++++++++++++- .../syntax/time_to_collision_condition.hpp | 29 +++- .../src/syntax/relative_speed_condition.cpp | 8 +- .../scenario_test_runner/config/workflow.txt | 1 + 4 files changed, 157 insertions(+), 6 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 6fd055886fc..628c7a9c6a4 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -15,11 +15,14 @@ #ifndef OPENSCENARIO_INTERPRETER__SIMULATOR_CORE_HPP_ #define OPENSCENARIO_INTERPRETER__SIMULATOR_CORE_HPP_ +#include #include +#include #include #include #include #include +#include #include #include #include @@ -528,6 +531,90 @@ class SimulatorCore return core->getCurrentAccel(std::forward(xs)...).linear.x; } + static auto evaluateCartesianTimeToCollisionCondition(const Entity & from, const Entity & to) + { + if (const auto entity_a = core->getEntity(from.name())) { + if (const auto entity_b = core->getEntity(to.name())) { + struct OrientedBoundingBox + { + Eigen::Vector3d position; + + Eigen::Matrix3d rotation; + + Eigen::Vector3d size; // [width, height, depth] + + explicit OrientedBoundingBox( + const geometry_msgs::msg::Pose & pose, + const traffic_simulator_msgs::msg::BoundingBox & box) + : position( + pose.position.x + box.center.x, pose.position.y + box.center.y, + pose.position.z + box.center.z), + rotation(math::geometry::getRotationMatrix(pose.orientation)), + size(box.dimensions.y / 2, box.dimensions.z / 2, box.dimensions.x / 2) + { + } + + auto axis(std::size_t index) const -> Eigen::Vector3d { return rotation.col(index); } + }; + + const Eigen::Vector3d v = [&]() { + const auto v = evaluateRelativeSpeed(from, to); + return Eigen::Vector3d(v.x, v.y, v.z); + }(); + + const auto a = OrientedBoundingBox(entity_a->getMapPose(), entity_a->getBoundingBox()); + const auto b = OrientedBoundingBox(entity_b->getMapPose(), entity_b->getBoundingBox()); + + const auto axes = std::array{ + a.axis(0), + b.axis(0), + + a.axis(1), + b.axis(1), + + a.axis(2), + b.axis(2), + + a.axis(0).cross(b.axis(0)), + a.axis(0).cross(b.axis(1)), + a.axis(0).cross(b.axis(2)), + + a.axis(1).cross(b.axis(0)), + a.axis(1).cross(b.axis(1)), + a.axis(1).cross(b.axis(2)), + + a.axis(2).cross(b.axis(0)), + a.axis(2).cross(b.axis(1)), + a.axis(2).cross(b.axis(2)), + }; + + auto t_min = std::numeric_limits::infinity(); + + Eigen::Vector3d d = b.position - a.position; + + for (const auto & axis : axes) { + auto project = [&](const auto & obb) { + return std::abs(axis.dot(obb.axis(0)) * obb.size(0)) + + std::abs(axis.dot(obb.axis(1)) * obb.size(1)) + + std::abs(axis.dot(obb.axis(2)) * obb.size(2)); + }; + + if (const auto relative_speed = v.dot(axis); relative_speed < 0) { + const auto separation = std::abs(d.dot(axis)) - project(a) - project(b); + + if (const auto t = separation / -relative_speed; 0 < t) { + t_min = std::min(t_min, t); + } + } + } + + return t_min; + } + } + + return std::numeric_limits::quiet_NaN(); + } + template static auto evaluateCollisionCondition(Ts &&... xs) -> bool { @@ -551,11 +638,41 @@ class SimulatorCore return std::numeric_limits::quiet_NaN(); } - template - static auto evaluateRelativeSpeed(const T & x, const U & y) + static auto evaluateRelativeSpeed(const Entity & from, const Entity & to) + -> geometry_msgs::msg::Vector3 { - using math::geometry::operator-; - return core->getCurrentTwist(x).linear - core->getCurrentTwist(y).linear; + auto direction = [](auto orientation) { + const auto euler_angle = math::geometry::convertQuaternionToEulerAngle(orientation); + + // clang-format off + const auto roll = euler_angle.x; + const auto pitch = euler_angle.y; + const auto yaw = euler_angle.z; + // clang-format on + + auto v = decltype(euler_angle)(); + + v.x = std::cos(yaw) * std::cos(pitch); + v.y = std::sin(yaw) * std::cos(pitch); + v.z = std::sin(pitch); + + return v; + }; + + if (const auto a = core->getEntity(from.name())) { + if (const auto b = core->getEntity(to.name())) { + using math::geometry::operator*; + using math::geometry::operator-; + + return direction(b->getMapPose().orientation) * b->getCurrentTwist().linear.x - + direction(a->getMapPose().orientation) * a->getCurrentTwist().linear.x; + } + } + + return geometry_msgs::build() + .x(Double::nan()) + .y(Double::nan()) + .z(Double::nan()); } template diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index e6e67c8a815..087822fd67d 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -149,14 +149,41 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio switch (relative_distance_type) { case RelativeDistanceType::longitudinal: + /* + There is no need to treat the cases where the speed is zero, NaN, or + infinity specially. When zero, NaN, or infinity appear in the + denominator, the Time-To-Collision will be infinity, NaN, or zero, + respectively, which are the desired return values to distinguish + between "no collision after infinite time", "undefined", and + "already a collision". + */ return distance() / speed(DirectionalDimension::longitudinal); case RelativeDistanceType::lateral: + /* + There is no need to treat the cases where the speed is zero, NaN, or + infinity specially. When zero, NaN, or infinity appear in the + denominator, the Time-To-Collision will be infinity, NaN, or zero, + respectively, which are the desired return values to distinguish + between "no collision after infinite time", "undefined", and + "already a collision". + */ return distance() / speed(DirectionalDimension::lateral); default: case RelativeDistanceType::euclidianDistance: - return distance() / speed(std::nullopt); + if (time_to_collision_condition_target.is()) { + if (freespace) { + return evaluateCartesianTimeToCollisionCondition( + triggering_entity, time_to_collision_condition_target.as()); + } else { + // TODO + return std::numeric_limits::quiet_NaN(); + } + } else { + // TODO + return distance() / speed(std::nullopt); + } } } diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp index e61c643ccf2..a8fe32a054e 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp @@ -64,7 +64,13 @@ auto RelativeSpeedCondition::evaluate( if ( triggering_entity.apply([&](const auto & each) { return entities->isAdded(each); }).min() and entities->isAdded(entity_ref)) { - return evaluateRelativeSpeed(triggering_entity, entity_ref); + /* + Relative speed is defined as speed_rel = speed(triggering entity) - + speed(reference entity) + + See: https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/RelativeSpeedCondition.html + */ + return evaluateRelativeSpeed(entity_ref, triggering_entity); } else { return geometry_msgs::build() .x(Double::nan()) diff --git a/test_runner/scenario_test_runner/config/workflow.txt b/test_runner/scenario_test_runner/config/workflow.txt index 5f7aba83593..58b17f8c17a 100644 --- a/test_runner/scenario_test_runner/config/workflow.txt +++ b/test_runner/scenario_test_runner/config/workflow.txt @@ -6,6 +6,7 @@ $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityConditio $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeDistanceCondition.yaml $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition.yaml $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition-back.yaml +$(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition-simple.yaml $(find-pkg-share scenario_test_runner)/scenario/ControllerAction.AssignControllerAction.yaml $(find-pkg-share scenario_test_runner)/scenario/LongitudinalAction.SpeedAction.yaml $(find-pkg-share scenario_test_runner)/scenario/Property.isBlind.yaml From 6bc50fea435ab716a9fdfc377ba48ab5c3d9629a Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 26 Nov 2024 10:31:23 +0900 Subject: [PATCH 040/149] Add new test scenario `...EntityCondition.TimeToCollisionCondition.yaml` Signed-off-by: yamacir-kit --- .../scenario_test_runner/config/workflow.txt | 2 +- ...ityCondition.TimeToCollisionCondition.yaml | 166 ++++++++++++++++++ 2 files changed, 167 insertions(+), 1 deletion(-) create mode 100644 test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml diff --git a/test_runner/scenario_test_runner/config/workflow.txt b/test_runner/scenario_test_runner/config/workflow.txt index 58b17f8c17a..da8d9cd0923 100644 --- a/test_runner/scenario_test_runner/config/workflow.txt +++ b/test_runner/scenario_test_runner/config/workflow.txt @@ -6,7 +6,7 @@ $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityConditio $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeDistanceCondition.yaml $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition.yaml $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition-back.yaml -$(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition-simple.yaml +$(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml $(find-pkg-share scenario_test_runner)/scenario/ControllerAction.AssignControllerAction.yaml $(find-pkg-share scenario_test_runner)/scenario/LongitudinalAction.SpeedAction.yaml $(find-pkg-share scenario_test_runner)/scenario/Property.isBlind.yaml diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml new file mode 100644 index 00000000000..da9326acb0f --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml @@ -0,0 +1,166 @@ +ScenarioModifiers: + ScenarioModifier: [] +OpenSCENARIO: + FileHeader: { revMajor: 1, revMinor: 1, date: '2024-11-25T04:18:18.703Z', description: '', author: Tatsuya Yamasaki } + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + VehicleCatalog: + Directory: { path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle } + RoadNetwork: + LogicFile: { filepath: $(ros2 pkg prefix --share kashiwanoha_map)/map } + Entities: + ScenarioObject: + - name: ego + CatalogReference: { catalogName: sample_vehicle, entryName: sample_vehicle } + ObjectController: + Controller: + name: '' + Properties: + Property: [] + - name: vehicle_01 + CatalogReference: { catalogName: sample_vehicle, entryName: sample_vehicle } + ObjectController: + Controller: + name: '' + Properties: + Property: [] + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 5 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - entityRef: vehicle_01 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34510' + s: 10 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - ControllerAction: + AssignControllerAction: + Controller: + name: '' + Properties: + Property: + - name: maxSpeed + value: '1' + Story: + - name: '' + Act: + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 20 + rule: greaterThan + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 180 + rule: greaterThan + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: [ entityRef: ego ] + EntityCondition: + TimeToCollisionCondition: + freespace: true + rule: lessThan + value: 0 + relativeDistanceType: longitudinal + coordinateSystem: lane + routingAlgorithm: undefined + TimeToCollisionConditionTarget: + EntityRef: + entityRef: vehicle_01 + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: [ entityRef: ego ] + EntityCondition: + TimeToCollisionCondition: + freespace: false + rule: lessThan + value: 0 + relativeDistanceType: cartesianDistance + coordinateSystem: entity + routingAlgorithm: undefined + TimeToCollisionConditionTarget: + EntityRef: + entityRef: vehicle_01 + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitFailure + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + StopTrigger: + ConditionGroup: [] From ec0bea7bf55eca3ec550ae55e133b959cbcb40d3 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 26 Nov 2024 14:33:32 +0900 Subject: [PATCH 041/149] Cleanup static member function `TimeToCollisionCondition::evaluate` Signed-off-by: yamacir-kit --- .../simulator_core.hpp | 169 +++++++++--------- .../syntax/time_to_collision_condition.hpp | 75 ++++---- ...ityCondition.TimeToCollisionCondition.yaml | 79 +++++++- 3 files changed, 190 insertions(+), 133 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 628c7a9c6a4..0b440c48199 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -531,90 +531,6 @@ class SimulatorCore return core->getCurrentAccel(std::forward(xs)...).linear.x; } - static auto evaluateCartesianTimeToCollisionCondition(const Entity & from, const Entity & to) - { - if (const auto entity_a = core->getEntity(from.name())) { - if (const auto entity_b = core->getEntity(to.name())) { - struct OrientedBoundingBox - { - Eigen::Vector3d position; - - Eigen::Matrix3d rotation; - - Eigen::Vector3d size; // [width, height, depth] - - explicit OrientedBoundingBox( - const geometry_msgs::msg::Pose & pose, - const traffic_simulator_msgs::msg::BoundingBox & box) - : position( - pose.position.x + box.center.x, pose.position.y + box.center.y, - pose.position.z + box.center.z), - rotation(math::geometry::getRotationMatrix(pose.orientation)), - size(box.dimensions.y / 2, box.dimensions.z / 2, box.dimensions.x / 2) - { - } - - auto axis(std::size_t index) const -> Eigen::Vector3d { return rotation.col(index); } - }; - - const Eigen::Vector3d v = [&]() { - const auto v = evaluateRelativeSpeed(from, to); - return Eigen::Vector3d(v.x, v.y, v.z); - }(); - - const auto a = OrientedBoundingBox(entity_a->getMapPose(), entity_a->getBoundingBox()); - const auto b = OrientedBoundingBox(entity_b->getMapPose(), entity_b->getBoundingBox()); - - const auto axes = std::array{ - a.axis(0), - b.axis(0), - - a.axis(1), - b.axis(1), - - a.axis(2), - b.axis(2), - - a.axis(0).cross(b.axis(0)), - a.axis(0).cross(b.axis(1)), - a.axis(0).cross(b.axis(2)), - - a.axis(1).cross(b.axis(0)), - a.axis(1).cross(b.axis(1)), - a.axis(1).cross(b.axis(2)), - - a.axis(2).cross(b.axis(0)), - a.axis(2).cross(b.axis(1)), - a.axis(2).cross(b.axis(2)), - }; - - auto t_min = std::numeric_limits::infinity(); - - Eigen::Vector3d d = b.position - a.position; - - for (const auto & axis : axes) { - auto project = [&](const auto & obb) { - return std::abs(axis.dot(obb.axis(0)) * obb.size(0)) + - std::abs(axis.dot(obb.axis(1)) * obb.size(1)) + - std::abs(axis.dot(obb.axis(2)) * obb.size(2)); - }; - - if (const auto relative_speed = v.dot(axis); relative_speed < 0) { - const auto separation = std::abs(d.dot(axis)) - project(a) - project(b); - - if (const auto t = separation / -relative_speed; 0 < t) { - t_min = std::min(t_min, t); - } - } - } - - return t_min; - } - } - - return std::numeric_limits::quiet_NaN(); - } - template static auto evaluateCollisionCondition(Ts &&... xs) -> bool { @@ -639,7 +555,6 @@ class SimulatorCore } static auto evaluateRelativeSpeed(const Entity & from, const Entity & to) - -> geometry_msgs::msg::Vector3 { auto direction = [](auto orientation) { const auto euler_angle = math::geometry::convertQuaternionToEulerAngle(orientation); @@ -707,6 +622,90 @@ class SimulatorCore return std::numeric_limits::quiet_NaN(); } } + + static auto evaluateTimeToCollisionCondition(const Entity & from, const Entity & to) + { + if (const auto entity_a = core->getEntity(from.name())) { + if (const auto entity_b = core->getEntity(to.name())) { + struct OrientedBoundingBox + { + Eigen::Vector3d position; + + Eigen::Matrix3d rotation; + + Eigen::Vector3d size; // [width, height, depth] + + explicit OrientedBoundingBox( + const geometry_msgs::msg::Pose & pose, + const traffic_simulator_msgs::msg::BoundingBox & box) + : position( + pose.position.x + box.center.x, pose.position.y + box.center.y, + pose.position.z + box.center.z), + rotation(math::geometry::getRotationMatrix(pose.orientation)), + size(box.dimensions.y / 2, box.dimensions.z / 2, box.dimensions.x / 2) + { + } + + auto axis(std::size_t index) const -> Eigen::Vector3d { return rotation.col(index); } + }; + + const Eigen::Vector3d v = [&]() { + const auto v = evaluateRelativeSpeed(from, to); + return Eigen::Vector3d(v.x, v.y, v.z); + }(); + + const auto a = OrientedBoundingBox(entity_a->getMapPose(), entity_a->getBoundingBox()); + const auto b = OrientedBoundingBox(entity_b->getMapPose(), entity_b->getBoundingBox()); + + const auto axes = std::array{ + a.axis(0), + b.axis(0), + + a.axis(1), + b.axis(1), + + a.axis(2), + b.axis(2), + + a.axis(0).cross(b.axis(0)), + a.axis(0).cross(b.axis(1)), + a.axis(0).cross(b.axis(2)), + + a.axis(1).cross(b.axis(0)), + a.axis(1).cross(b.axis(1)), + a.axis(1).cross(b.axis(2)), + + a.axis(2).cross(b.axis(0)), + a.axis(2).cross(b.axis(1)), + a.axis(2).cross(b.axis(2)), + }; + + auto t_min = std::numeric_limits::infinity(); + + Eigen::Vector3d d = b.position - a.position; + + for (const auto & axis : axes) { + auto project = [&](const auto & obb) { + return std::abs(axis.dot(obb.axis(0)) * obb.size(0)) + + std::abs(axis.dot(obb.axis(1)) * obb.size(1)) + + std::abs(axis.dot(obb.axis(2)) * obb.size(2)); + }; + + if (const auto relative_speed = v.dot(axis); relative_speed < 0) { + const auto separation = std::abs(d.dot(axis)) - project(a) - project(b); + + if (const auto t = separation / -relative_speed; 0 < t) { + t_min = std::min(t_min, t); + } + } + } + + return t_min; + } + } + + return std::numeric_limits::quiet_NaN(); + } }; class NonStandardOperation : private CoordinateSystemConversion diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index 087822fd67d..dd606281e7d 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -135,55 +135,44 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio } }; - auto speed = [&](auto &&... xs) { + auto directional_dimension = [&]() { + switch (relative_distance_type) { + case RelativeDistanceType::longitudinal: + return std::optional(DirectionalDimension::longitudinal); + case RelativeDistanceType::lateral: + return std::optional(DirectionalDimension::lateral); + default: + case RelativeDistanceType::euclidianDistance: + return std::optional(std::nullopt); + }; + }; + + auto speed = [&]() { if (time_to_collision_condition_target.is()) { return RelativeSpeedCondition::evaluate( - entities, // - triggering_entity, // - time_to_collision_condition_target.as(), std::forward(xs)...); + entities, triggering_entity, time_to_collision_condition_target.as(), + directional_dimension()); } else { - return SpeedCondition::evaluate( - entities, triggering_entity, std::forward(xs)...); + return SpeedCondition::evaluate(entities, triggering_entity, directional_dimension()); } }; - switch (relative_distance_type) { - case RelativeDistanceType::longitudinal: - /* - There is no need to treat the cases where the speed is zero, NaN, or - infinity specially. When zero, NaN, or infinity appear in the - denominator, the Time-To-Collision will be infinity, NaN, or zero, - respectively, which are the desired return values to distinguish - between "no collision after infinite time", "undefined", and - "already a collision". - */ - return distance() / speed(DirectionalDimension::longitudinal); - - case RelativeDistanceType::lateral: - /* - There is no need to treat the cases where the speed is zero, NaN, or - infinity specially. When zero, NaN, or infinity appear in the - denominator, the Time-To-Collision will be infinity, NaN, or zero, - respectively, which are the desired return values to distinguish - between "no collision after infinite time", "undefined", and - "already a collision". - */ - return distance() / speed(DirectionalDimension::lateral); - - default: - case RelativeDistanceType::euclidianDistance: - if (time_to_collision_condition_target.is()) { - if (freespace) { - return evaluateCartesianTimeToCollisionCondition( - triggering_entity, time_to_collision_condition_target.as()); - } else { - // TODO - return std::numeric_limits::quiet_NaN(); - } - } else { - // TODO - return distance() / speed(std::nullopt); - } + if ( + time_to_collision_condition_target.is() and + coordinate_system == CoordinateSystem::entity and + relative_distance_type == RelativeDistanceType::euclidianDistance and freespace) { + return evaluateTimeToCollisionCondition( + triggering_entity, time_to_collision_condition_target.as()); + } else { + /* + There is no need to treat the cases where the speed is zero, NaN, or + infinity specially. When zero, NaN, or infinity appear in the + denominator, the Time-To-Collision will be infinity, NaN, or zero, + respectively, which are the desired return values to distinguish + between "no collision after infinite time", "undefined", and "already + a collision". + */ + return distance() / speed(); } } diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml index da9326acb0f..f5de56f82df 100644 --- a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml +++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml @@ -106,10 +106,21 @@ OpenSCENARIO: - name: '' delay: 0 conditionEdge: none - ByValueCondition: - SimulationTimeCondition: - value: 180 - rule: greaterThan + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: [ entityRef: ego ] + EntityCondition: + TimeToCollisionCondition: + freespace: true + rule: lessThan + value: 0 + relativeDistanceType: longitudinal + coordinateSystem: lane + routingAlgorithm: undefined + TimeToCollisionConditionTarget: + EntityRef: + entityRef: vehicle_01 - Condition: - name: '' delay: 0 @@ -120,7 +131,7 @@ OpenSCENARIO: EntityRef: [ entityRef: ego ] EntityCondition: TimeToCollisionCondition: - freespace: true + freespace: false rule: lessThan value: 0 relativeDistanceType: longitudinal @@ -129,6 +140,64 @@ OpenSCENARIO: TimeToCollisionConditionTarget: EntityRef: entityRef: vehicle_01 + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: [ entityRef: ego ] + EntityCondition: + TimeToCollisionCondition: + freespace: true + rule: lessThan + value: 0 + relativeDistanceType: longitudinal + coordinateSystem: entity + routingAlgorithm: undefined + TimeToCollisionConditionTarget: + EntityRef: + entityRef: vehicle_01 + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: [ entityRef: ego ] + EntityCondition: + TimeToCollisionCondition: + freespace: false + rule: lessThan + value: 0 + relativeDistanceType: longitudinal + coordinateSystem: entity + routingAlgorithm: undefined + TimeToCollisionConditionTarget: + EntityRef: + entityRef: vehicle_01 + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: [ entityRef: ego ] + EntityCondition: + TimeToCollisionCondition: + freespace: true + rule: lessThan + value: 0 + relativeDistanceType: cartesianDistance + coordinateSystem: entity + routingAlgorithm: undefined + TimeToCollisionConditionTarget: + EntityRef: + entityRef: vehicle_01 + - Condition: - name: '' delay: 0 conditionEdge: none From 64da48270d347f71a5f44a9c55c4085ea9d6e1bf Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 26 Nov 2024 15:07:40 +0900 Subject: [PATCH 042/149] Fix `TimeToCollisionCondition` to return inf if relative speed < zero Signed-off-by: yamacir-kit --- .../syntax/time_to_collision_condition.hpp | 10 +--------- ...ition.EntityCondition.TimeToCollisionCondition.yaml | 2 +- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index dd606281e7d..681e1045bfb 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -164,15 +164,7 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio return evaluateTimeToCollisionCondition( triggering_entity, time_to_collision_condition_target.as()); } else { - /* - There is no need to treat the cases where the speed is zero, NaN, or - infinity specially. When zero, NaN, or infinity appear in the - denominator, the Time-To-Collision will be infinity, NaN, or zero, - respectively, which are the desired return values to distinguish - between "no collision after infinite time", "undefined", and "already - a collision". - */ - return distance() / speed(); + return distance() / std::max(speed(), 0.0); } } diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml index f5de56f82df..640fc08fc46 100644 --- a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml +++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml @@ -64,7 +64,7 @@ OpenSCENARIO: Properties: Property: - name: maxSpeed - value: '1' + value: '0.5' Story: - name: '' Act: From 4f2863a5ba42d7f2fd09d61b9396feb8b044127e Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 26 Nov 2024 19:50:31 +0900 Subject: [PATCH 043/149] Fix `evaluateTimeToCollisionCondition` to not return meaningless value when collisions cannot occur Signed-off-by: yamacir-kit --- .../simulator_core.hpp | 96 +++++++++++++++---- ...ityCondition.TimeToCollisionCondition.yaml | 58 ++++++++++- 2 files changed, 133 insertions(+), 21 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 0b440c48199..795f44377af 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -625,6 +625,19 @@ class SimulatorCore static auto evaluateTimeToCollisionCondition(const Entity & from, const Entity & to) { + /* + This function uses the Separating Axis Theorem (SAT) to find the + Time-To-Collision (TTC) of two entities in the world coordinate + system. + + First, the expected time of collision is obtained independently for + each separating axis. Then, we check whether the polygons of the + entities actually collide after that time. If no collision occurs, the + expected collision time is rejected. The lowest value of the expected + collision times obtained for each separate axis is then returned as + the TTC. + */ + if (const auto entity_a = core->getEntity(from.name())) { if (const auto entity_b = core->getEntity(to.name())) { struct OrientedBoundingBox @@ -659,12 +672,11 @@ class SimulatorCore const auto axes = std::array{ a.axis(0), - b.axis(0), - a.axis(1), - b.axis(1), - a.axis(2), + + b.axis(0), + b.axis(1), b.axis(2), a.axis(0).cross(b.axis(0)), @@ -680,22 +692,74 @@ class SimulatorCore a.axis(2).cross(b.axis(2)), }; - auto t_min = std::numeric_limits::infinity(); - Eigen::Vector3d d = b.position - a.position; - for (const auto & axis : axes) { - auto project = [&](const auto & obb) { - return std::abs(axis.dot(obb.axis(0)) * obb.size(0)) + - std::abs(axis.dot(obb.axis(1)) * obb.size(1)) + - std::abs(axis.dot(obb.axis(2)) * obb.size(2)); - }; + auto t_min = std::numeric_limits::infinity(); - if (const auto relative_speed = v.dot(axis); relative_speed < 0) { - const auto separation = std::abs(d.dot(axis)) - project(a) - project(b); + auto projection = [&](const auto & box, const auto & axis) { + return std::abs(axis.dot(box.axis(0)) * box.size(0)) + + std::abs(axis.dot(box.axis(1)) * box.size(1)) + + std::abs(axis.dot(box.axis(2)) * box.size(2)); + }; - if (const auto t = separation / -relative_speed; 0 < t) { - t_min = std::min(t_min, t); + for (const auto & axis : axes) { + /* + The variable `separation` means how far apart two Oriented + Bounding Boxes (OBBs) are on the separation axis. When it is + positive, it means they are separated by `separation`, i.e. the + OBBs do not overlap on the separation axis. When it is less than + zero, it means the OBBs overlap on the separation axis. + + d.dot(axis) is the projection of the center distance between the + two OBBs onto the separation axis, and projection(a, axis) and + projection(b, axis) are half the projected range of the boxes + when the two OBBs are projected onto the separation axis. + */ + if (const auto separation = + std::abs(d.dot(axis)) - projection(a, axis) - projection(b, axis); + 0 < separation) { + /* + v.dot(axis) is the projection of the relative distance onto + the separation axis. In other words, we want to find the TTC + for each separation axis. The relative velocity is the + velocity vector of the observed object measured in the + observer's rest frame, so a negative value means that the two + OBBs are approaching each other. + */ + if (const auto relative_speed = v.dot(axis); relative_speed < 0) { + /* + Since this t is only a Time-To-Collision on this separation + axis, it is necessary to check that a and b are not + separated on all separation axes after t seconds. + */ + const auto t = separation / -relative_speed; + + const auto a_t = a; + + const auto b_t = [&]() { + /* + Since t is the exact time when contact occurs, it may not + be possible to say that a and b are in collision after t + seconds. Therefore, 0.01 seconds is added to t so that + collision can be expected to occur with certainty. This + value "0.01" has no technical basis and is an arbitrary + value. + */ + constexpr auto tolerance = 0.01; + auto copy = b; + copy.position += v * (t + tolerance); + return copy; + }(); + + Eigen::Vector3d d_t = b_t.position - a_t.position; + + if (std::all_of(axes.begin(), axes.end(), [&](const auto & axis) { + const auto separation_t = + std::abs(d_t.dot(axis)) - projection(a_t, axis) - projection(b_t, axis); + return separation_t <= 0; + })) { + t_min = std::min(t_min, t); + } } } } diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml index 640fc08fc46..555aad18bad 100644 --- a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml +++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml @@ -25,6 +25,13 @@ OpenSCENARIO: name: '' Properties: Property: [] + - name: vehicle_02 + CatalogReference: { catalogName: sample_vehicle, entryName: sample_vehicle } + ObjectController: + Controller: + name: '' + Properties: + Property: [] Storyboard: Init: Actions: @@ -35,8 +42,8 @@ OpenSCENARIO: Position: LanePosition: roadId: '' - laneId: '34513' - s: 5 + laneId: '34976' + s: 10 offset: 0 Orientation: type: relative @@ -49,8 +56,8 @@ OpenSCENARIO: Position: LanePosition: roadId: '' - laneId: '34510' - s: 10 + laneId: '34579' + s: 0 offset: 0 Orientation: type: relative @@ -65,6 +72,28 @@ OpenSCENARIO: Property: - name: maxSpeed value: '0.5' + - entityRef: vehicle_02 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34576' + s: 0 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - ControllerAction: + AssignControllerAction: + Controller: + name: '' + Properties: + Property: + - name: maxSpeed + value: '2' # If 2.5, collision will occur. Story: - name: '' Act: @@ -89,7 +118,7 @@ OpenSCENARIO: conditionEdge: none ByValueCondition: SimulationTimeCondition: - value: 20 + value: 30 rule: greaterThan Action: - name: '' @@ -216,6 +245,25 @@ OpenSCENARIO: TimeToCollisionConditionTarget: EntityRef: entityRef: vehicle_01 + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: [ entityRef: ego ] + EntityCondition: + TimeToCollisionCondition: + freespace: true + rule: lessThan + value: 0 + relativeDistanceType: cartesianDistance + coordinateSystem: entity + routingAlgorithm: undefined + TimeToCollisionConditionTarget: + EntityRef: + entityRef: vehicle_02 Action: - name: '' UserDefinedAction: From 7262f70261e43f3065cdf2cd952f6acda46f7084 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 28 Nov 2024 14:07:05 +0900 Subject: [PATCH 044/149] Set default value for Start/StopTrigger in Act --- .../include/openscenario_interpreter/syntax/act.hpp | 2 +- .../include/openscenario_interpreter/syntax/trigger.hpp | 3 +++ openscenario/openscenario_interpreter/src/syntax/act.cpp | 4 ++-- openscenario/openscenario_interpreter/src/syntax/trigger.cpp | 2 ++ 4 files changed, 8 insertions(+), 3 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp index 5c34a9c1bb6..b91dba297f9 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp @@ -30,7 +30,7 @@ inline namespace syntax * * * - * + * * * * diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/trigger.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/trigger.hpp index 37c798576e0..364a2332d79 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/trigger.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/trigger.hpp @@ -52,6 +52,9 @@ struct Trigger : public std::list auto activeConditionGroupDescription() const -> std::vector>; auto evaluate() -> Object; + + // Utility variables for some default triggers that are always evaluated to be true + static const Trigger always_true; }; auto operator<<(boost::json::object &, const Trigger &) -> boost::json::object &; diff --git a/openscenario/openscenario_interpreter/src/syntax/act.cpp b/openscenario/openscenario_interpreter/src/syntax/act.cpp index 5ad07cf8ac8..94c391ee0d1 100644 --- a/openscenario/openscenario_interpreter/src/syntax/act.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/act.cpp @@ -24,8 +24,8 @@ inline namespace syntax Act::Act(const pugi::xml_node & node, Scope & scope) : Scope(readAttribute("name", node, scope), scope), StoryboardElement( - readElement("StartTrigger", node, local()), - readElement("StopTrigger", node, local())) // NOTE: Optional element + readElement("StartTrigger", node, local(), Trigger::always_true), + readElement("StopTrigger", node, local(), Trigger::always_true)) { traverse<1, unbounded>(node, "ManeuverGroup", [&](auto && node) { return elements.push_back(readStoryboardElement(node, local())); diff --git a/openscenario/openscenario_interpreter/src/syntax/trigger.cpp b/openscenario/openscenario_interpreter/src/syntax/trigger.cpp index 51ea2dd41b6..97e415be459 100644 --- a/openscenario/openscenario_interpreter/src/syntax/trigger.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/trigger.cpp @@ -24,6 +24,8 @@ Trigger::Trigger(const pugi::xml_node & node, Scope & scope) traverse<0, unbounded>(node, "ConditionGroup", [&](auto && node) { emplace_back(node, scope); }); } +const Trigger Trigger::always_true = Trigger{{ConditionGroup()}}; + auto Trigger::evaluate() -> Object { /* ------------------------------------------------------------------------- From 6534221ccd2b619adc795a379b9fc2e586c10dc9 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 28 Nov 2024 14:08:47 +0900 Subject: [PATCH 045/149] Specify version on comment --- .../include/openscenario_interpreter/syntax/act.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp index b91dba297f9..7b70ade5772 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp @@ -25,7 +25,7 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- Act -------------------------------------------------------------------- +/* ---- Act 1.2 ------------------------------------------------------------- * * * From f66c84f3f9979ab420c35cd829801bdd38c92e97 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 28 Nov 2024 14:36:00 +0900 Subject: [PATCH 046/149] Fix version --- .../include/openscenario_interpreter/syntax/act.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp index 7b70ade5772..54afb28bb78 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp @@ -25,7 +25,7 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- Act 1.2 ------------------------------------------------------------- +/* ---- Act 1.3 ------------------------------------------------------------- * * * From 9a81a11fa9c78d9848ad7f7d2a623f86d24ab8f2 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 28 Nov 2024 14:47:35 +0900 Subject: [PATCH 047/149] Fix StopTrigger behavior --- openscenario/openscenario_interpreter/src/syntax/act.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/openscenario/openscenario_interpreter/src/syntax/act.cpp b/openscenario/openscenario_interpreter/src/syntax/act.cpp index 94c391ee0d1..9537a0c6868 100644 --- a/openscenario/openscenario_interpreter/src/syntax/act.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/act.cpp @@ -25,7 +25,7 @@ Act::Act(const pugi::xml_node & node, Scope & scope) : Scope(readAttribute("name", node, scope), scope), StoryboardElement( readElement("StartTrigger", node, local(), Trigger::always_true), - readElement("StopTrigger", node, local(), Trigger::always_true)) + readElement("StopTrigger", node, local())) { traverse<1, unbounded>(node, "ManeuverGroup", [&](auto && node) { return elements.push_back(readStoryboardElement(node, local())); From feddfe43b4752a2d9405183ba1852270d4ade2c5 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 28 Nov 2024 14:50:26 +0900 Subject: [PATCH 048/149] Use always_true in Event --- openscenario/openscenario_interpreter/src/syntax/event.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/openscenario/openscenario_interpreter/src/syntax/event.cpp b/openscenario/openscenario_interpreter/src/syntax/event.cpp index 9b9ffc19dd4..d3c29b8ac83 100644 --- a/openscenario/openscenario_interpreter/src/syntax/event.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/event.cpp @@ -26,7 +26,7 @@ Event::Event(const pugi::xml_node & node, Scope & scope, Maneuver & maneuver) StoryboardElement( readAttribute("maximumExecutionCount", node, local(), UnsignedInt(1)), // If there is no "StartTrigger" in the "Event", the default StartTrigger that always returns true is used. - readElement("StartTrigger", node, local(), Trigger({ConditionGroup()}))), + readElement("StartTrigger", node, local(), Trigger::always_true)), priority(readAttribute("priority", node, local())), parent_maneuver(maneuver) { From 2c91138dd832e2d551f141574c3c39ab85455061 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 28 Nov 2024 14:59:58 +0900 Subject: [PATCH 049/149] Update schema comment --- .../openscenario_interpreter/syntax/act.hpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp index 54afb28bb78..241c22c822d 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/act.hpp @@ -25,18 +25,18 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- Act 1.3 ------------------------------------------------------------- - * - * - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + Act(OpenSCENARIO XML 1.3) + + + + + + + + + +*/ struct Act : public Scope, public StoryboardElement { explicit Act(const pugi::xml_node &, Scope &); From b4ff0434fe6441426f042c5f434de7f15cb9636f Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 28 Nov 2024 16:12:53 +0900 Subject: [PATCH 050/149] Change default value definition --- .../include/openscenario_interpreter/syntax/trigger.hpp | 4 ++-- openscenario/openscenario_interpreter/src/syntax/act.cpp | 2 +- openscenario/openscenario_interpreter/src/syntax/event.cpp | 2 +- openscenario/openscenario_interpreter/src/syntax/trigger.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/trigger.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/trigger.hpp index 364a2332d79..3f7418de274 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/trigger.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/trigger.hpp @@ -53,8 +53,8 @@ struct Trigger : public std::list auto evaluate() -> Object; - // Utility variables for some default triggers that are always evaluated to be true - static const Trigger always_true; + // Utility function for some default triggers that are always evaluated to be true + static auto truthy() noexcept -> Trigger; }; auto operator<<(boost::json::object &, const Trigger &) -> boost::json::object &; diff --git a/openscenario/openscenario_interpreter/src/syntax/act.cpp b/openscenario/openscenario_interpreter/src/syntax/act.cpp index 9537a0c6868..0aa2c9f5e69 100644 --- a/openscenario/openscenario_interpreter/src/syntax/act.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/act.cpp @@ -24,7 +24,7 @@ inline namespace syntax Act::Act(const pugi::xml_node & node, Scope & scope) : Scope(readAttribute("name", node, scope), scope), StoryboardElement( - readElement("StartTrigger", node, local(), Trigger::always_true), + readElement("StartTrigger", node, local(), Trigger::truthy()), readElement("StopTrigger", node, local())) { traverse<1, unbounded>(node, "ManeuverGroup", [&](auto && node) { diff --git a/openscenario/openscenario_interpreter/src/syntax/event.cpp b/openscenario/openscenario_interpreter/src/syntax/event.cpp index d3c29b8ac83..0643a086a9a 100644 --- a/openscenario/openscenario_interpreter/src/syntax/event.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/event.cpp @@ -26,7 +26,7 @@ Event::Event(const pugi::xml_node & node, Scope & scope, Maneuver & maneuver) StoryboardElement( readAttribute("maximumExecutionCount", node, local(), UnsignedInt(1)), // If there is no "StartTrigger" in the "Event", the default StartTrigger that always returns true is used. - readElement("StartTrigger", node, local(), Trigger::always_true)), + readElement("StartTrigger", node, local(), Trigger::truthy())), priority(readAttribute("priority", node, local())), parent_maneuver(maneuver) { diff --git a/openscenario/openscenario_interpreter/src/syntax/trigger.cpp b/openscenario/openscenario_interpreter/src/syntax/trigger.cpp index 97e415be459..80cb517f923 100644 --- a/openscenario/openscenario_interpreter/src/syntax/trigger.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/trigger.cpp @@ -24,7 +24,7 @@ Trigger::Trigger(const pugi::xml_node & node, Scope & scope) traverse<0, unbounded>(node, "ConditionGroup", [&](auto && node) { emplace_back(node, scope); }); } -const Trigger Trigger::always_true = Trigger{{ConditionGroup()}}; +auto Trigger::truthy() noexcept -> Trigger { return Trigger{{ConditionGroup()}}; } auto Trigger::evaluate() -> Object { From 2095cba08bdb189327c736c69127991c7b8709b1 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 28 Nov 2024 17:27:52 +0100 Subject: [PATCH 051/149] TrafficSink refactor with despawn functionality --- .../src/traffic_sink/auto_sink_vehicle.cpp | 9 +-- .../include/traffic_simulator/api/api.hpp | 19 +----- .../entity/entity_manager.hpp | 1 - .../traffic/traffic_controller.hpp | 16 ++--- .../traffic/traffic_sink.hpp | 35 ++++++----- .../src/traffic/traffic_controller.cpp | 27 ++++----- .../src/traffic/traffic_sink.cpp | 58 ++++++++++++++----- 7 files changed, 88 insertions(+), 77 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index fe1b2110b33..ccdcdef41ff 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -30,7 +30,8 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode explicit AutoSinkVehicleScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "auto_sink_vehicle", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option, true, {traffic_simulator::EntityType::VEHICLE}) + "lanelet2_map.osm", __FILE__, false, option, true, + {traffic_simulator::EntityType::PEDESTRIAN}) { start(); } @@ -39,10 +40,10 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { if (api_.getCurrentTime() >= 0.1) { - if (api_.entityExists("bob") && !api_.entityExists("ego")) { - stop(cpp_mock_scenarios::Result::FAILURE); - } else { + if (api_.entityExists("ego") and not api_.entityExists("bob")) { stop(cpp_mock_scenarios::Result::SUCCESS); + } else { + stop(cpp_mock_scenarios::Result::FAILURE); } } } diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index a30855dc2cc..604e1d648be 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -75,23 +76,7 @@ class API node, entity_manager_ptr_->getHdmapUtils(), getROS2Parameter("architecture_type", "awf/universe"))), traffic_controller_ptr_(std::make_shared( - entity_manager_ptr_->getHdmapUtils(), [this]() { return API::getEntityNames(); }, - [this](const auto & entity_name) { - if (const auto entity = getEntity(entity_name)) { - return entity->getEntityType(); - } else { - THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); - } - }, - configuration.sinkable_entity_type, - [this](const auto & entity_name) { - if (const auto entity = getEntity(entity_name)) { - return entity->getMapPose(); - } else { - THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); - } - }, - [this](const auto & name) { return API::despawn(name); }, configuration.auto_sink)), + entity_manager_ptr_, configuration.sinkable_entity_type, configuration.auto_sink)), clock_pub_(rclcpp::create_publisher( node, "/clock", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), rclcpp::PublisherOptionsWithAllocator())), 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 89c2bbec9d3..75af7e4972a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -37,7 +37,6 @@ #include #include #include -#include #include #include #include diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 1dac416f036..50d0414ad3d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -29,9 +29,9 @@ #include #include #include +#include #include #include -#include #include #include @@ -43,12 +43,8 @@ class TrafficController { public: explicit TrafficController( - std::shared_ptr hdmap_utils, - const std::function(void)> & get_entity_names, - const std::function & get_entity_type, - const std::set & sinkable_entity_type, - const std::function & get_entity_pose, - const std::function & despawn, bool auto_sink = false); + const std::shared_ptr entity_manager_ptr, + const std::set & sinkable_entity_type, bool auto_sink = false); template void addModule(Ts &&... xs) @@ -61,13 +57,9 @@ class TrafficController private: void autoSink(); - const std::shared_ptr hdmap_utils_; + const std::shared_ptr entity_manager_ptr; std::vector> modules_; - const std::function(void)> get_entity_names; - const std::function get_entity_type; const std::set sinkable_entity_type; - const std::function get_entity_pose; - const std::function despawn; public: const bool auto_sink; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 81fbc8f5ad8..3b3ed35348d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -32,6 +32,7 @@ #include #include #include +#include #include #include @@ -50,30 +51,38 @@ class TrafficSink : public TrafficModuleBase * @param position Position of the traffic sink. * @param get_entity_names Function to get the name of entity * @param get_entity_type Function to get the type of entity - * @param sinkable_entity_type If this type is applicable, the entity despawn only when it approaches radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. + * @param sinkable_entity_type If this type is applicable, the entity despawn only when it approaches + * radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. * @param get_entity_pose Function to get the pose of entity. * @param despawn Function to despawn entity. */ explicit TrafficSink( + const std::shared_ptr entity_manager_ptr, const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, - const std::function(void)> & get_entity_names, - const std::function & get_entity_type, - const std::set & sinkable_entity_type, - const std::function & get_entity_pose, - const std::function & despawn); - const lanelet::Id lanelet_id; - const double radius; - const geometry_msgs::msg::Point position; + const std::set & sinkable_entity_type); void execute(const double current_time, const double step_time) override; auto appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void override; + const lanelet::Id lanelet_id; + const double radius; + const geometry_msgs::msg::Point position; + private: - const std::function(void)> get_entity_names; - const std::function get_entity_type; + auto getEntityNames() const -> std::vector; + auto getEntityType(const std::string & entity_name) const noexcept(false) + -> traffic_simulator::EntityType; + auto getEntityPose(const std::string & entity_name) const noexcept(false) + -> geometry_msgs::msg::Pose; + /** + * @note Despawns the entity only when both: + * 1. Its distance from the TrafficSink is <= radius [m]. + * 2. Its EntityType is in sinkable_entity_type or sinkable_entity_type is empty. + */ + auto despawn(const std::string & entity_name) const -> void; + + const std::shared_ptr entity_manager_ptr; const std::set sinkable_entity_type; - const std::function get_entity_pose; - const std::function despawn; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 1510323a4f8..f2f220b848c 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -38,18 +39,10 @@ namespace traffic_simulator namespace traffic { TrafficController::TrafficController( - std::shared_ptr hdmap_utils, - const std::function(void)> & get_entity_names, - const std::function & get_entity_type, - const std::set & sinkable_entity_type, - const std::function & get_entity_pose, - const std::function & despawn, bool auto_sink) -: hdmap_utils_(hdmap_utils), - get_entity_names(get_entity_names), - get_entity_type(get_entity_type), + const std::shared_ptr entity_manager_ptr, + const std::set & sinkable_entity_type, bool auto_sink) +: entity_manager_ptr(entity_manager_ptr), sinkable_entity_type(sinkable_entity_type), - get_entity_pose(get_entity_pose), - despawn(despawn), auto_sink(auto_sink) { if (auto_sink) { @@ -59,15 +52,15 @@ TrafficController::TrafficController( void TrafficController::autoSink() { - for (const auto & lanelet_id : hdmap_utils_->getLaneletIds()) { - if (hdmap_utils_->getNextLaneletIds(lanelet_id).empty()) { + const auto hdmap_utils_ptr = entity_manager_ptr->getHdmapUtils(); + for (const auto & lanelet_id : hdmap_utils_ptr->getLaneletIds()) { + if (hdmap_utils_ptr->getNextLaneletIds(lanelet_id).empty()) { LaneletPose lanelet_pose; lanelet_pose.lanelet_id = lanelet_id; - lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_); - const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_); + lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_ptr); + const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_ptr); addModule( - lanelet_id, 1, pose.position, get_entity_names, get_entity_type, sinkable_entity_type, - get_entity_pose, despawn); + entity_manager_ptr, lanelet_id, 1, pose.position, sinkable_entity_type); } } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 82096d5bb02..5c1809cf03b 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -39,36 +40,30 @@ namespace traffic_simulator namespace traffic { TrafficSink::TrafficSink( + const std::shared_ptr entity_manager_ptr, const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, - const std::function(void)> & get_entity_names, - const std::function & get_entity_type, - const std::set & sinkable_entity_type, - const std::function & get_entity_pose, - const std::function & despawn) + const std::set & sinkable_entity_type) : TrafficModuleBase(), + entity_manager_ptr(entity_manager_ptr), lanelet_id(lanelet_id), radius(radius), position(position), - get_entity_names(get_entity_names), - get_entity_type(get_entity_type), - sinkable_entity_type(sinkable_entity_type), - get_entity_pose(get_entity_pose), - despawn(despawn) + sinkable_entity_type(sinkable_entity_type) { } void TrafficSink::execute( [[maybe_unused]] const double current_time, [[maybe_unused]] const double step_time) { - const auto names = get_entity_names(); + const auto names = getEntityNames(); for (const auto & name : names) { const auto is_sinkable_entity = [this](const auto & entity_name) { return sinkable_entity_type.empty() ? true - : sinkable_entity_type.find(get_entity_type(entity_name).type) != + : sinkable_entity_type.find(getEntityType(entity_name).type) != sinkable_entity_type.end(); }; - const auto pose = get_entity_pose(name); + const auto pose = getEntityPose(name); if (is_sinkable_entity(name) and math::geometry::getDistance(position, pose) <= radius) { despawn(name); } @@ -103,5 +98,42 @@ auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marke text_marker.scale.z = 0.6; marker_array.markers.emplace_back(text_marker); } + +auto TrafficSink::getEntityNames() const -> std::vector +{ + return entity_manager_ptr->getEntityNames(); +} + +auto TrafficSink::getEntityType(const std::string & entity_name) const noexcept(false) + -> traffic_simulator::EntityType +{ + if (const auto entity = entity_manager_ptr->getEntity(entity_name)) { + return entity->getEntityType(); + } else { + THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); + } +} +auto TrafficSink::getEntityPose(const std::string & entity_name) const noexcept(false) + -> geometry_msgs::msg::Pose +{ + if (const auto entity = entity_manager_ptr->getEntity(entity_name)) { + return entity->getMapPose(); + } else { + THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); + } +} +auto TrafficSink::despawn(const std::string & entity_name) const -> void +{ + const auto entity_position = getEntityPose(entity_name).position; + const bool in_despawn_proximity = math::geometry::hypot(entity_position, position) <= radius; + + const std::uint8_t entity_type = getEntityType(entity_name).type; + const bool is_despawn_candidate = + sinkable_entity_type.empty() or + sinkable_entity_type.find(entity_type) != sinkable_entity_type.cend(); + if (is_despawn_candidate and in_despawn_proximity) { + entity_manager_ptr->despawnEntity(entity_name); + } +} } // namespace traffic } // namespace traffic_simulator From ebec87979308c977c4286e4504281193d1a7c78d Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 28 Nov 2024 17:36:55 +0100 Subject: [PATCH 052/149] spell-check happy --- .../include/traffic_simulator/traffic/traffic_sink.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 3b3ed35348d..27e4a7fa24d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -75,7 +75,7 @@ class TrafficSink : public TrafficModuleBase auto getEntityPose(const std::string & entity_name) const noexcept(false) -> geometry_msgs::msg::Pose; /** - * @note Despawns the entity only when both: + * @note Despawn the entity only when both: * 1. Its distance from the TrafficSink is <= radius [m]. * 2. Its EntityType is in sinkable_entity_type or sinkable_entity_type is empty. */ From e1a4989223756db2b6cd8e79d9dd5adc98d0ec85 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Wed, 27 Nov 2024 19:36:47 +0900 Subject: [PATCH 053/149] fix(api): request enable autoware control Signed-off-by: satoshi-ota --- simulation/traffic_simulator/src/api/api.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 1f24e201b3c..1b8690d2ab1 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -94,6 +94,7 @@ auto API::respawn( entity_manager_ptr_->asFieldOperatorApplication(name).clearRoute(); entity_manager_ptr_->asFieldOperatorApplication(name).plan({goal_pose}); + entity_manager_ptr_->asFieldOperatorApplication(name).enableAutowareControl(); entity_manager_ptr_->asFieldOperatorApplication(name).engage(); } } From 5a94c73813d2bb854001028d8f517f1e380b6de6 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 29 Nov 2024 10:20:11 +0900 Subject: [PATCH 054/149] fix(concealer): increase max time to request enable autoware control Signed-off-by: satoshi-ota --- .../src/field_operator_application_for_autoware_universe.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index 072a42b628f..00bd97a1d00 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -475,7 +475,7 @@ auto FieldOperatorApplicationFor::enableAutowareControl() -> v { task_queue.delay([this]() { auto request = std::make_shared(); - requestEnableAutowareControl(request); + requestEnableAutowareControl(request, 30); }); } From a19355ca6be06aa28438069c8af965ec378129a1 Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Tue, 3 Dec 2024 20:59:22 +0900 Subject: [PATCH 055/149] Remove the commented out code. Replace this declaration by a structured binding declaration. --- .../src/spline/catmull_rom_spline.cpp | 24 +++++++++---------- ..._visualization_condition_groups_plugin.cpp | 1 - .../visualization/visualization_component.cpp | 9 ++++--- 3 files changed, 16 insertions(+), 18 deletions(-) diff --git a/common/math/geometry/src/spline/catmull_rom_spline.cpp b/common/math/geometry/src/spline/catmull_rom_spline.cpp index d9cd6025150..434eefb7a12 100644 --- a/common/math/geometry/src/spline/catmull_rom_spline.cpp +++ b/common/math/geometry/src/spline/catmull_rom_spline.cpp @@ -470,8 +470,8 @@ auto CatmullRomSpline::getSquaredDistanceIn2D( } return line_segments_[0].getSquaredDistanceIn2D(point, s, true); default: - const auto index_and_s = getCurveIndexAndS(s); - return curves_[index_and_s.first].getSquaredDistanceIn2D(point, index_and_s.second, true); + const auto [index, s_value] = getCurveIndexAndS(s); + return curves_[index].getSquaredDistanceIn2D(point, s_value, true); } } @@ -508,8 +508,8 @@ auto CatmullRomSpline::getSquaredDistanceVector( } return line_segments_[0].getSquaredDistanceVector(point, s, true); default: - const auto index_and_s = getCurveIndexAndS(s); - return curves_[index_and_s.first].getSquaredDistanceVector(point, index_and_s.second, true); + const auto [index, s_value] = getCurveIndexAndS(s); + return curves_[index].getSquaredDistanceVector(point, s_value, true); } } @@ -542,8 +542,8 @@ auto CatmullRomSpline::getPoint(const double s) const -> geometry_msgs::msg::Poi } return line_segments_[0].getPoint(s, true); default: - const auto index_and_s = getCurveIndexAndS(s); - return curves_[index_and_s.first].getPoint(index_and_s.second, true); + const auto [index, s_value] = getCurveIndexAndS(s); + return curves_[index].getPoint(s_value, true); } } @@ -597,8 +597,8 @@ auto CatmullRomSpline::getNormalVector(const double s) const -> geometry_msgs::m "This message is not originally intended to be displayed, if you see it, please " "contact the developer of traffic_simulator."); default: - const auto index_and_s = getCurveIndexAndS(s); - return curves_[index_and_s.first].getNormalVector(index_and_s.second, true); + const auto [index, s_value] = getCurveIndexAndS(s); + return curves_[index].getNormalVector(s_value, true); } } @@ -634,8 +634,8 @@ auto CatmullRomSpline::getTangentVector(const double s) const -> geometry_msgs:: "This message is not originally intended to be displayed, if you see it, please " "contact the developer of traffic_simulator."); default: - const auto index_and_s = getCurveIndexAndS(s); - return curves_[index_and_s.first].getTangentVector(index_and_s.second, true); + const auto [index, s_value] = getCurveIndexAndS(s); + return curves_[index].getNormalVector(s_value, true); } } @@ -665,8 +665,8 @@ auto CatmullRomSpline::getPose(const double s, const bool fill_pitch) const } return line_segments_[0].getPose(s, true, fill_pitch); default: - const auto index_and_s = getCurveIndexAndS(s); - return curves_[index_and_s.first].getPose(index_and_s.second, true, fill_pitch); + const auto [index, s_value] = getCurveIndexAndS(s); + return curves_[index].getPose(s_value, true, fill_pitch); } } diff --git a/rviz_plugins/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.cpp b/rviz_plugins/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.cpp index 0d1c54a46f5..18387b98dc8 100644 --- a/rviz_plugins/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.cpp +++ b/rviz_plugins/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.cpp @@ -171,7 +171,6 @@ void VisualizationConditionGroupsDisplay::processMessage(const Context::ConstSha QPainter painter(&hud); painter.setRenderHint(QPainter::Antialiasing, true); - // QColor text_color = property_text_color_->getColor(); QColor text_color(property_text_color_->getColor()); text_color.setAlpha(255); painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); diff --git a/simulation/traffic_simulator/src/visualization/visualization_component.cpp b/simulation/traffic_simulator/src/visualization/visualization_component.cpp index 2af92b13bfe..10c1bc5d968 100644 --- a/simulation/traffic_simulator/src/visualization/visualization_component.cpp +++ b/simulation/traffic_simulator/src/visualization/visualization_component.cpp @@ -74,14 +74,14 @@ void VisualizationComponent::entityStatusCallback( entity_name_lists.emplace_back(data.status.name); } std::vector erase_names; - for (const auto & marker : markers_) { - auto itr = std::find(entity_name_lists.begin(), entity_name_lists.end(), marker.first); + for (const auto & [key, _] : markers_) { + auto itr = std::find(entity_name_lists.begin(), entity_name_lists.end(), key); if (itr == entity_name_lists.end()) { - auto delete_marker = generateDeleteMarker(marker.first); + auto delete_marker = generateDeleteMarker(key); std::copy( delete_marker.markers.begin(), delete_marker.markers.end(), std::back_inserter(current_marker.markers)); - erase_names.emplace_back(marker.first); + erase_names.emplace_back(key); } } for (const auto & name : erase_names) { @@ -336,7 +336,6 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke arrow.id = 2; arrow.action = visualization_msgs::msg::Marker::ADD; - // constexpr double arrow_size = 0.3; double arrow_size = 0.4 * status.bounding_box.dimensions.y; constexpr double arrow_ratio = 1.0; geometry_msgs::msg::Point pf, pl, pr; From 2f1da242bc5232201d8430d3d1891f73caf51409 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 3 Dec 2024 16:43:12 +0100 Subject: [PATCH 056/149] reorder fix --- simulation/traffic_simulator/src/traffic/traffic_sink.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 5c1809cf03b..dbf60347d63 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -44,10 +44,10 @@ TrafficSink::TrafficSink( const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::set & sinkable_entity_type) : TrafficModuleBase(), - entity_manager_ptr(entity_manager_ptr), lanelet_id(lanelet_id), radius(radius), position(position), + entity_manager_ptr(entity_manager_ptr), sinkable_entity_type(sinkable_entity_type) { } From 17b965be5bdbf2142ccba26215024017ce00e0b3 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 5 Dec 2024 12:20:09 +0900 Subject: [PATCH 057/149] Add some test cases for getClosestPoses --- .../geometry/test/src/test_bounding_box.cpp | 69 +++++++++++++++++-- 1 file changed, 65 insertions(+), 4 deletions(-) diff --git a/common/math/geometry/test/src/test_bounding_box.cpp b/common/math/geometry/test/src/test_bounding_box.cpp index b3449f994e4..a954c2c734a 100644 --- a/common/math/geometry/test/src/test_bounding_box.cpp +++ b/common/math/geometry/test/src/test_bounding_box.cpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "expect_eq_macros.hpp" +#include "geometry/distance.hpp" +#include "test_utils.hpp" #include #include #include -#include "expect_eq_macros.hpp" -#include "test_utils.hpp" +#include TEST(BoundingBox, getPointsFromBboxDefault) { @@ -83,7 +84,8 @@ TEST(BoundingBox, toPolygon2D_onlyTranslation) } /** - * @note Test obtaining polygon from bounding box with full transformation applied (translation + rotation). + * @note Test obtaining polygon from bounding box with full transformation applied (translation + + * rotation). */ TEST(BoundingBox, toPolygon2D_fullPose) { @@ -135,6 +137,65 @@ TEST(BoundingBox, getPolygonDistanceWithoutCollision) EXPECT_DOUBLE_EQ(ans.value(), 3.0); } +TEST(BoudingBox, getClosestPoses) +{ + traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(1.0, 1.0, 1.0); + geometry_msgs::msg::Pose pose0; + geometry_msgs::msg::Pose pose1 = makePose(5.0, 5.0); + + { + const auto actual = math::geometry::getClosestPoses(pose0, bbox, pose1, bbox); + ASSERT_TRUE(actual); + + geometry_msgs::msg::Pose expected_pose0 = makePose(0.5, 0.5); + geometry_msgs::msg::Pose expected_pose1 = makePose(4.5, 4.5); + EXPECT_POSE_EQ(actual.value().first, expected_pose1); + EXPECT_POSE_EQ(actual.value().second, expected_pose0); + } + + { // reverse order + const auto actual = math::geometry::getClosestPoses(pose1, bbox, pose0, bbox); + ASSERT_TRUE(actual); + + geometry_msgs::msg::Pose expected_pose0 = makePose(0.5, 0.5); + geometry_msgs::msg::Pose expected_pose1 = makePose(4.5, 4.5); + EXPECT_POSE_EQ(actual.value().first, expected_pose0); + EXPECT_POSE_EQ(actual.value().second, expected_pose1); + } +} + +TEST(BoudingBox, getClosestPosesWithAlmostTouch) +{ + traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(1.0, 1.0, 1.0); + geometry_msgs::msg::Pose pose0; + traffic_simulator_msgs::msg::BoundingBox bbox1 = makeBbox(1.0, 10.0, 1.0); + geometry_msgs::msg::Pose pose1 = makePose(1.1, 0.0); + + { + const auto actual = math::geometry::getClosestPoses(pose0, bbox0, pose1, bbox1); + ASSERT_TRUE(actual); + const auto distance = math::geometry::getDistance(actual.value().first, actual.value().second); + EXPECT_NEAR(distance, 0.1, 0.0001); + } + + { // reverse order + const auto actual = math::geometry::getClosestPoses(pose1, bbox1, pose0, bbox0); + ASSERT_TRUE(actual); + const auto distance = math::geometry::getDistance(actual.value().first, actual.value().second); + EXPECT_NEAR(distance, 0.1, 0.0001); + } +} + +TEST(BoudingBox, getClosestPosesWithIntersection) +{ + traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(1.0, 1.0, 1.0); + geometry_msgs::msg::Pose pose0; + geometry_msgs::msg::Pose pose1 = makePose(0.6, 0.6); + + const auto actual = math::geometry::getClosestPoses(pose0, bbox, pose1, bbox); + ASSERT_FALSE(actual); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From dc04dcf9d2816d471195f17d09e038357c2f65ec Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 5 Dec 2024 12:47:38 +0900 Subject: [PATCH 058/149] Apply clang-format --- common/math/geometry/test/src/test_bounding_box.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/common/math/geometry/test/src/test_bounding_box.cpp b/common/math/geometry/test/src/test_bounding_box.cpp index a954c2c734a..d213e98f1e4 100644 --- a/common/math/geometry/test/src/test_bounding_box.cpp +++ b/common/math/geometry/test/src/test_bounding_box.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "expect_eq_macros.hpp" -#include "geometry/distance.hpp" -#include "test_utils.hpp" +#include #include #include #include -#include +#include "expect_eq_macros.hpp" +#include "geometry/distance.hpp" +#include "test_utils.hpp" TEST(BoundingBox, getPointsFromBboxDefault) { From 4afe1e73728c4e9c5925a73bde0363916a1fa5f0 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 5 Dec 2024 12:47:58 +0900 Subject: [PATCH 059/149] Fix getClosestPoses with naive algorithm --- common/math/geometry/src/bounding_box.cpp | 27 +++++++++++------------ 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/common/math/geometry/src/bounding_box.cpp b/common/math/geometry/src/bounding_box.cpp index db3d75b2f9f..49b90bf48dc 100644 --- a/common/math/geometry/src/bounding_box.cpp +++ b/common/math/geometry/src/bounding_box.cpp @@ -62,22 +62,12 @@ std::optional> get const auto poly0 = toPolygon2D(pose0, bbox0); const auto poly1 = toPolygon2D(pose1, bbox1); - if (boost::geometry::intersects(poly0, poly1)) { - return std::nullopt; - } - if (boost::geometry::intersects(poly1, poly0)) { - return std::nullopt; - } if (boost::geometry::disjoint(poly0, poly1)) { auto point0 = boost_point(); auto point1 = boost_point(); auto min_distance = boost::numeric::bounds::highest(); - auto segments = boost::make_iterator_range( - boost::geometry::segments_begin(poly0), boost::geometry::segments_end(poly0)); - auto points = boost::make_iterator_range( - boost::geometry::points_begin(poly1), boost::geometry::points_end(poly1)); - auto findNearestPointInSegment = [&](const auto & segment, const auto & points) { + auto findNearestPointToSegment = [&](const auto & segment, const auto & points) { for (auto && point : points) { auto nearest_point_from_segment = pointToSegmentProjection(point, *segment.first, *segment.second); @@ -90,9 +80,18 @@ std::optional> get } }; - for (auto && segment : segments) { - findNearestPointInSegment(segment, points); - } + auto findNearestPointInPolygon = [&](const auto & poly0, const auto & poly1) { + auto segments = boost::make_iterator_range( + boost::geometry::segments_begin(poly0), boost::geometry::segments_end(poly0)); + auto points = boost::geometry::exterior_ring(poly1); + + for (auto && segment : segments) { + findNearestPointToSegment(segment, points); + } + }; + + findNearestPointInPolygon(poly0, poly1); + findNearestPointInPolygon(poly1, poly0); return std::make_pair(toPose(point0), toPose(point1)); } From 9f3abdbd4ab520ad52bbeb987ac5fa0e04a62217 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 5 Dec 2024 12:54:00 +0900 Subject: [PATCH 060/149] Fix spell mistake --- common/math/geometry/test/src/test_bounding_box.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/common/math/geometry/test/src/test_bounding_box.cpp b/common/math/geometry/test/src/test_bounding_box.cpp index d213e98f1e4..00232501d72 100644 --- a/common/math/geometry/test/src/test_bounding_box.cpp +++ b/common/math/geometry/test/src/test_bounding_box.cpp @@ -137,7 +137,7 @@ TEST(BoundingBox, getPolygonDistanceWithoutCollision) EXPECT_DOUBLE_EQ(ans.value(), 3.0); } -TEST(BoudingBox, getClosestPoses) +TEST(BoundingBox, getClosestPoses) { traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(1.0, 1.0, 1.0); geometry_msgs::msg::Pose pose0; @@ -164,7 +164,7 @@ TEST(BoudingBox, getClosestPoses) } } -TEST(BoudingBox, getClosestPosesWithAlmostTouch) +TEST(BoundingBox, getClosestPosesWithAlmostTouch) { traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(1.0, 1.0, 1.0); geometry_msgs::msg::Pose pose0; @@ -186,7 +186,7 @@ TEST(BoudingBox, getClosestPosesWithAlmostTouch) } } -TEST(BoudingBox, getClosestPosesWithIntersection) +TEST(BoundingBox, getClosestPosesWithIntersection) { traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(1.0, 1.0, 1.0); geometry_msgs::msg::Pose pose0; From 646726861e064e0bfd3b0f8035f91c0c87f2cdb4 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 6 Dec 2024 16:30:19 +0900 Subject: [PATCH 061/149] Remove class `concealer::Autoware` Signed-off-by: yamacir-kit --- external/concealer/CMakeLists.txt | 1 - .../concealer/include/concealer/autoware.hpp | 83 ------------------- .../include/concealer/autoware_universe.hpp | 54 ++++++++---- external/concealer/src/autoware.cpp | 59 ------------- external/concealer/src/autoware_universe.cpp | 7 +- .../ego_entity_simulation.hpp | 4 +- .../traffic_simulator/entity/ego_entity.hpp | 1 - 7 files changed, 41 insertions(+), 168 deletions(-) delete mode 100644 external/concealer/include/concealer/autoware.hpp delete mode 100644 external/concealer/src/autoware.cpp diff --git a/external/concealer/CMakeLists.txt b/external/concealer/CMakeLists.txt index 5a0ffb32056..2fac64c1f9a 100644 --- a/external/concealer/CMakeLists.txt +++ b/external/concealer/CMakeLists.txt @@ -14,7 +14,6 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() ament_auto_add_library(${PROJECT_NAME} SHARED - src/autoware.cpp src/autoware_universe.cpp src/execute.cpp src/field_operator_application.cpp diff --git a/external/concealer/include/concealer/autoware.hpp b/external/concealer/include/concealer/autoware.hpp deleted file mode 100644 index fcda771cf9f..00000000000 --- a/external/concealer/include/concealer/autoware.hpp +++ /dev/null @@ -1,83 +0,0 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CONCEALER__AUTOWARE_HPP_ -#define CONCEALER__AUTOWARE_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace concealer -{ -/** - * Provides an abstraction to communicate with Autoware in order to: - * - receive vehicle commands to simulate vehicle kinematics - * - provide vehicle state reports on an appropriate topics - * NOTE: This class is intended to be move to simple_sensor_simulator - */ -class Autoware : public rclcpp::Node, public ContinuousTransformBroadcaster -{ -protected: - std::atomic current_acceleration; - - std::atomic current_twist; - - std::atomic current_pose; - -public: - CONCEALER_PUBLIC explicit Autoware(); - - virtual auto getAcceleration() const -> double = 0; - - virtual auto getGearCommand() const -> autoware_vehicle_msgs::msg::GearCommand; - - virtual auto getSteeringAngle() const -> double = 0; - - virtual auto getVelocity() const -> double = 0; - - // returns -1.0 when gear is reverse and 1.0 otherwise - virtual auto getGearSign() const -> double = 0; - - virtual auto getTurnIndicatorsCommand() const - -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand; - - virtual auto getVehicleCommand() const - -> std::tuple = 0; - - virtual auto getRouteLanelets() const -> std::vector = 0; - - virtual auto getControlModeReport() const -> autoware_vehicle_msgs::msg::ControlModeReport = 0; - - auto set(const geometry_msgs::msg::Accel &) -> void; - - auto set(const geometry_msgs::msg::Twist &) -> void; - - auto set(const geometry_msgs::msg::Pose &) -> void; - - virtual auto setManualMode() -> void = 0; - - virtual auto rethrow() -> void; -}; -} // namespace concealer - -#endif // CONCEALER__AUTOWARE_HPP_ diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 0692c7717c0..6bbedcc2f5e 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -15,28 +15,33 @@ #ifndef CONCEALER__AUTOWARE_UNIVERSE_HPP_ #define CONCEALER__AUTOWARE_UNIVERSE_HPP_ +#include #include #include +#include #include #include +#include #include #include #include -#include +#include #include #include +#include #include +#include +#include #include +#include #include namespace concealer { -/* - * Implements Autoware interface for Autoware Universe - * NOTE: This class is intended to be move to simple_sensor_simulator - */ -class AutowareUniverse : public Autoware +class AutowareUniverse : public rclcpp::Node, + public ContinuousTransformBroadcaster { +public: // clang-format off using AccelWithCovarianceStamped = geometry_msgs::msg::AccelWithCovarianceStamped; using Control = autoware_control_msgs::msg::Control; @@ -52,7 +57,7 @@ class AutowareUniverse : public Autoware using VelocityReport = autoware_vehicle_msgs::msg::VelocityReport; SubscriberWrapper getCommand; - SubscriberWrapper getGearCommandImpl; + SubscriberWrapper getGearCommand; SubscriberWrapper getTurnIndicatorsCommand; SubscriberWrapper getPathWithLaneId; @@ -66,6 +71,7 @@ class AutowareUniverse : public Autoware PublisherWrapper setTurnIndicatorsReport; // clang-format on +private: rclcpp::Service::SharedPtr control_mode_request_server; const rclcpp::TimerBase::SharedPtr localization_update_timer; @@ -84,36 +90,48 @@ class AutowareUniverse : public Autoware auto stopAndJoin() -> void; + std::atomic current_acceleration; + + std::atomic current_twist; + + std::atomic current_pose; + public: CONCEALER_PUBLIC explicit AutowareUniverse(bool); ~AutowareUniverse(); - auto rethrow() -> void override; + auto rethrow() -> void; - auto getAcceleration() const -> double override; + auto getAcceleration() const -> double; - auto getSteeringAngle() const -> double override; + auto getSteeringAngle() const -> double; - auto getVelocity() const -> double override; + auto getVelocity() const -> double; auto updateLocalization() -> void; auto updateVehicleState() -> void; - auto getGearCommand() const -> GearCommand override; + auto getGearSign() const -> double; - auto getGearSign() const -> double override; + auto getVehicleCommand() const -> std::tuple; - auto getVehicleCommand() const -> std::tuple override; + auto getRouteLanelets() const -> std::vector; - auto getRouteLanelets() const -> std::vector override; + auto getControlModeReport() const -> ControlModeReport; - auto getControlModeReport() const -> ControlModeReport override; + auto setManualMode() -> void; - auto setManualMode() -> void override; -}; + auto set(const geometry_msgs::msg::Accel & acceleration) -> void + { + current_acceleration.store(acceleration); + } + + auto set(const geometry_msgs::msg::Twist & twist) -> void { current_twist.store(twist); } + auto set(const geometry_msgs::msg::Pose & pose) -> void { current_pose.store(pose); } +}; } // namespace concealer #endif // CONCEALER__AUTOWARE_UNIVERSE_HPP_ diff --git a/external/concealer/src/autoware.cpp b/external/concealer/src/autoware.cpp deleted file mode 100644 index f29cbd93e2d..00000000000 --- a/external/concealer/src/autoware.cpp +++ /dev/null @@ -1,59 +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 - -namespace concealer -{ -Autoware::Autoware() -: rclcpp::Node("concealer", "simulation", rclcpp::NodeOptions().use_global_arguments(false)), - current_acceleration(geometry_msgs::msg::Accel()), - current_twist(geometry_msgs::msg::Twist()), - current_pose(geometry_msgs::msg::Pose()) -{ -} - -auto Autoware::getGearCommand() const -> autoware_vehicle_msgs::msg::GearCommand -{ - static auto gear_command = []() { - autoware_vehicle_msgs::msg::GearCommand gear_command; - gear_command.command = autoware_vehicle_msgs::msg::GearCommand::DRIVE; - return gear_command; - }(); - gear_command.stamp = now(); - return gear_command; -} - -auto Autoware::set(const geometry_msgs::msg::Accel & acceleration) -> void -{ - current_acceleration.store(acceleration); -} - -auto Autoware::set(const geometry_msgs::msg::Twist & twist) -> void { current_twist.store(twist); } - -auto Autoware::set(const geometry_msgs::msg::Pose & pose) -> void { current_pose.store(pose); } - -auto Autoware::getTurnIndicatorsCommand() const -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand -{ - static auto turn_indicators_command = []() { - autoware_vehicle_msgs::msg::TurnIndicatorsCommand turn_indicators_command; - turn_indicators_command.command = autoware_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND; - return turn_indicators_command; - }(); - turn_indicators_command.stamp = now(); - return turn_indicators_command; -} - -auto Autoware::rethrow() -> void {} -} // namespace concealer diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index da3d240d571..5553f008a47 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -17,8 +17,9 @@ namespace concealer { AutowareUniverse::AutowareUniverse(bool simulate_localization) -: getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), - getGearCommandImpl("/control/command/gear_cmd", rclcpp::QoS(1), *this), +: rclcpp::Node("concealer", "simulation", rclcpp::NodeOptions().use_global_arguments(false)), + getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), + getGearCommand("/control/command/gear_cmd", rclcpp::QoS(1), *this), getTurnIndicatorsCommand("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), getPathWithLaneId( "/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), @@ -186,8 +187,6 @@ auto AutowareUniverse::updateVehicleState() -> void }()); } -auto AutowareUniverse::getGearCommand() const -> GearCommand { return getGearCommandImpl(); } - auto AutowareUniverse::getGearSign() const -> double { /// @todo Add support for GearCommand::NONE to return 0.0 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 9ba9aae8869..8e09148b912 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 @@ -15,7 +15,7 @@ #ifndef TRAFFIC_SIMULATOR__VEHICLE_SIMULATION__EGO_ENTITY_SIMULATION_HPP_ #define TRAFFIC_SIMULATOR__VEHICLE_SIMULATION__EGO_ENTITY_SIMULATION_HPP_ -#include +#include #include #include #include @@ -40,7 +40,7 @@ enum class VehicleModelType { class EgoEntitySimulation { public: - const std::unique_ptr autoware; + const std::unique_ptr autoware; private: const VehicleModelType vehicle_model_type_; 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 69fc2a2de1a..91cab0f7f97 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -17,7 +17,6 @@ #include #include -#include #include #include #include From d54d506205356314896e14e07ef8ea05c36aa1d1 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 6 Dec 2024 18:59:50 +0900 Subject: [PATCH 062/149] Remove member function `AutowareUniverse::stopAndJoin` Signed-off-by: yamacir-kit --- .../include/concealer/autoware_universe.hpp | 2 -- external/concealer/src/autoware_universe.cpp | 12 +++++------- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 6bbedcc2f5e..43cea43706c 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -88,8 +88,6 @@ class AutowareUniverse : public rclcpp::Node, std::exception_ptr thrown; - auto stopAndJoin() -> void; - std::atomic current_acceleration; std::atomic current_twist; diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index 5553f008a47..af5e5f91a65 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -79,7 +79,11 @@ AutowareUniverse::AutowareUniverse(bool simulate_localization) { } -AutowareUniverse::~AutowareUniverse() { stopAndJoin(); } +AutowareUniverse::~AutowareUniverse() +{ + is_stop_requested.store(true); + localization_and_vehicle_state_update_thread.join(); +} auto AutowareUniverse::rethrow() -> void { @@ -88,12 +92,6 @@ auto AutowareUniverse::rethrow() -> void } } -auto AutowareUniverse::stopAndJoin() -> void -{ - is_stop_requested.store(true); - localization_and_vehicle_state_update_thread.join(); -} - auto AutowareUniverse::getAcceleration() const -> double { return getCommand().longitudinal.acceleration; From 564b75a5023819b0fa4858e0818f684d9296f108 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 9 Dec 2024 13:22:16 +0900 Subject: [PATCH 063/149] Remove member function `AutowareUniverse::set` Signed-off-by: yamacir-kit --- .../include/concealer/autoware_universe.hpp | 19 ++++--------------- .../ego_entity_simulation.cpp | 6 +++--- 2 files changed, 7 insertions(+), 18 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 43cea43706c..20df13aafac 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -69,6 +69,10 @@ class AutowareUniverse : public rclcpp::Node, PublisherWrapper setControlModeReport; PublisherWrapper setVelocityReport; PublisherWrapper setTurnIndicatorsReport; + + std::atomic current_acceleration; + std::atomic current_pose; + std::atomic current_twist; // clang-format on private: @@ -88,12 +92,6 @@ class AutowareUniverse : public rclcpp::Node, std::exception_ptr thrown; - std::atomic current_acceleration; - - std::atomic current_twist; - - std::atomic current_pose; - public: CONCEALER_PUBLIC explicit AutowareUniverse(bool); @@ -120,15 +118,6 @@ class AutowareUniverse : public rclcpp::Node, auto getControlModeReport() const -> ControlModeReport; auto setManualMode() -> void; - - auto set(const geometry_msgs::msg::Accel & acceleration) -> void - { - current_acceleration.store(acceleration); - } - - auto set(const geometry_msgs::msg::Twist & twist) -> void { current_twist.store(twist); } - - auto set(const geometry_msgs::msg::Pose & pose) -> void { current_pose.store(pose); } }; } // namespace concealer 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 756f3f4db4f..bc050b391f6 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 @@ -177,15 +177,15 @@ auto EgoEntitySimulation::makeSimulationModel( auto EgoEntitySimulation::setAutowareStatus() -> void { - autoware->set([this]() { + autoware->current_acceleration.store([this]() { geometry_msgs::msg::Accel message; message.linear.x = vehicle_model_ptr_->getAx(); return message; }()); - autoware->set(status_.getMapPose()); + autoware->current_pose.store(status_.getMapPose()); - autoware->set(getCurrentTwist()); + autoware->current_twist.store(getCurrentTwist()); } void EgoEntitySimulation::requestSpeedChange(double value) From d524164dd48559fcaac0fe0e0ad9f920ccbdccdb Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 9 Dec 2024 16:33:34 +0900 Subject: [PATCH 064/149] Cleanup Signed-off-by: yamacir-kit --- .../geometry/include/geometry/vector3/norm.hpp | 1 - .../openscenario_interpreter/simulator_core.hpp | 12 ++---------- .../syntax/time_to_collision_condition.hpp | 14 ++++++++++---- .../syntax/time_to_collision_condition_target.hpp | 2 +- .../src/syntax/add_entity_action.cpp | 2 +- .../src/syntax/speed_condition.cpp | 2 -- 6 files changed, 14 insertions(+), 19 deletions(-) diff --git a/common/math/geometry/include/geometry/vector3/norm.hpp b/common/math/geometry/include/geometry/vector3/norm.hpp index 6746c7ec7c6..44fa57c9e62 100644 --- a/common/math/geometry/include/geometry/vector3/norm.hpp +++ b/common/math/geometry/include/geometry/vector3/norm.hpp @@ -15,7 +15,6 @@ #ifndef GEOMETRY__VECTOR3__NORM_HPP_ #define GEOMETRY__VECTOR3__NORM_HPP_ -#include #include namespace math diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 82e3e2a0623..1e53a012a36 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -17,8 +17,6 @@ #include #include -#include -#include #include #include #include @@ -564,15 +562,9 @@ class SimulatorCore if (const auto observer = core->getEntity(from.name())) { if (const auto observed = core->getEntity(to.name())) { auto velocity = [](const auto & entity) -> Eigen::Vector3d { - auto direction = [](auto orientation) -> Eigen::Vector3d { - const auto euler_angle = math::geometry::convertQuaternionToEulerAngle(orientation); - const auto r = euler_angle.x; - const auto p = euler_angle.y; - const auto y = euler_angle.z; - return Eigen::Vector3d( - std::cos(y) * std::cos(p), std::sin(y) * std::cos(p), std::sin(p)); + auto direction = [](const auto & q) -> Eigen::Vector3d { + return Eigen::Quaternion(q.w, q.x, q.y, q.z) * Eigen::Vector3d::UnitX(); }; - return direction(entity->getMapPose().orientation) * entity->getCurrentTwist().linear.x; }; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index 681e1045bfb..17a34204c03 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -29,7 +29,7 @@ namespace openscenario_interpreter inline namespace syntax { /* - TimeToCollisionCondition (OpenSCENARIO XML 1.3) + TimeToCollisionCondition (OpenSCENARIO XML 1.3.1) The currently predicted time to collision of a triggering entity/entities and either a reference entity or an explicit position is compared to a given @@ -106,9 +106,15 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio { auto description = std::stringstream(); - description << triggering_entities.description() << "'s time to collision to given entity " - << "TODO-RELATIVE-DISTANCE-TARGET" - << " = "; + description << triggering_entities.description() << "'s time-to-collision to given "; + + if (time_to_collision_condition_target.is()) { + description << " entity " << time_to_collision_condition_target.as(); + } else { + description << " position"; + } + + description << " = "; print_to(description, evaluations); diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp index ac1b051008a..8e15f1f6f3c 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp @@ -23,7 +23,7 @@ namespace openscenario_interpreter inline namespace syntax { /* - TimeToCollisionConditionTarget 1.3 + TimeToCollisionConditionTarget (OpenSCENARIO XML 1.3.1) Target position used in the TimeToCollisionCondition. Can be defined as either an explicit position, or the position of a reference entity. diff --git a/openscenario/openscenario_interpreter/src/syntax/add_entity_action.cpp b/openscenario/openscenario_interpreter/src/syntax/add_entity_action.cpp index ae11d05f50c..d0408597bf5 100644 --- a/openscenario/openscenario_interpreter/src/syntax/add_entity_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/add_entity_action.cpp @@ -47,7 +47,7 @@ auto AddEntityAction::endsImmediately() noexcept -> bool // auto AddEntityAction::operator()(const EntityRef & entity_ref) const -> void try { - const auto entity = global().entities->ref(entity_ref); + const auto entity = global().entities->at(entity_ref); const auto add_entity = overload( [&](const Vehicle & vehicle) { diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp index 8d994c6f14f..cc8d64b0c41 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp @@ -12,9 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include -#include #include #include #include From b375c5fdb655b0a2f385b30ddcdc2114798ef9e2 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Fri, 6 Dec 2024 17:20:59 +0100 Subject: [PATCH 065/149] [RJD-1369] Improve lanelet matching - 3D support - Enhanced lanelet matching algorithm (`toLaneletPose` method) by incorporating lanelet altitude. - Defined the `altitude_threshold` parameter that sets the maximum altitude difference to determine when an entity can be matched with a specific lanelet. --- .../hdmap_utils/hdmap_utils.hpp | 19 ++++++++++++++++++- .../src/hdmap_utils/hdmap_utils.cpp | 13 ++++++++++++- 2 files changed, 30 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 07dced38f8f..618b63b688d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -160,7 +160,7 @@ class HdMapUtils const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids; - auto getHeight(const traffic_simulator_msgs::msg::LaneletPose &) const -> double; + auto getAltitude(const traffic_simulator_msgs::msg::LaneletPose &) const -> double; auto getLaneChangeTrajectory( const geometry_msgs::msg::Pose & from, @@ -380,7 +380,24 @@ class HdMapUtils auto toMapPose(const traffic_simulator_msgs::msg::LaneletPose &, const bool fill_pitch = true) const -> geometry_msgs::msg::PoseStamped; + auto isAltitudeMatching(const double current_altitude, const double target_altitude) const + -> bool; + private: + /* + Using a fixed `altitude_threshold` value of 1.0 [m] is justified because the + entity's Z-position is always relative to its base. This eliminates the + need to dynamically adjust the threshold based on the entity's dimensions, + ensuring consistent altitude matching regardless of the entity type. + + The position of the entity is defined relative to its base, typically + the center of the rear axle projected onto the ground in the case of vehicles. + + There is no technical basis for this value; it was determined based on + experiments conducted by Szymon Parapura. + */ + static constexpr double altitude_threshold_ = 1.0; + /** @defgroup cache * Declared mutable for caching */ diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index b6f9406a8d8..55a71850e49 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -332,7 +332,7 @@ auto HdMapUtils::getNearbyLaneletIds( return lanelet_ids; } -auto HdMapUtils::getHeight(const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose) const +auto HdMapUtils::getAltitude(const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose) const -> double { return toMapPose(lanelet_pose).pose.position.z; @@ -593,6 +593,11 @@ auto HdMapUtils::toLaneletPose( return std::nullopt; } auto pose_on_centerline = spline->getPose(s.value()); + + if (!isAltitudeMatching(pose.position.z, pose_on_centerline.position.z)) { + return std::nullopt; + } + auto rpy = math::geometry::convertQuaternionToEulerAngle( math::geometry::getRotation(pose_on_centerline.orientation, pose.orientation)); double offset = std::sqrt(spline->getSquaredDistanceIn2D(pose.position, s.value())); @@ -616,6 +621,12 @@ auto HdMapUtils::toLaneletPose( return lanelet_pose; } +auto HdMapUtils::isAltitudeMatching( + const double current_altitude, const double target_altitude) const -> bool +{ + return std::abs(current_altitude - target_altitude) <= altitude_threshold_; +} + auto HdMapUtils::toLaneletPose( const geometry_msgs::msg::Pose & pose, const lanelet::Ids & lanelet_ids, const double matching_distance) const -> std::optional From c1b4d1f9d55f704435018e070bb60b9a7a0bc99d Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Sun, 8 Dec 2024 17:17:16 +0100 Subject: [PATCH 066/149] [RJD-1369] Improve Collision Solving for Multi-Level Support - Enhanced BehaviorTree to consider altitude when detecting potential obstacles, allowing to ignore objects located at different altitudes. - Modified the detection sensor by introducing Ego plane determination to exclude objects below the Ego plane, preventing unnecessary slowing or stopping caused by incorrect detections. --- .../math/geometry/include/geometry/plane.hpp | 42 +++++++++++++++ .../quaternion/get_angle_difference.hpp | 39 ++++++++++++++ .../geometry/quaternion/get_normal_vector.hpp | 41 +++++++++++++++ common/math/geometry/src/plane.cpp | 48 +++++++++++++++++ .../behavior_tree_plugin/action_node.hpp | 2 + .../behavior_tree_plugin/src/action_node.cpp | 10 +++- .../detection_sensor/detection_sensor.hpp | 42 ++++++++++++++- .../detection_sensor/detection_sensor.cpp | 52 +++++++++++++++++++ 8 files changed, 274 insertions(+), 2 deletions(-) create mode 100644 common/math/geometry/include/geometry/plane.hpp create mode 100644 common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp create mode 100644 common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp create mode 100644 common/math/geometry/src/plane.cpp diff --git a/common/math/geometry/include/geometry/plane.hpp b/common/math/geometry/include/geometry/plane.hpp new file mode 100644 index 00000000000..a718c319bd7 --- /dev/null +++ b/common/math/geometry/include/geometry/plane.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 GEOMETRY__PLANE_HPP_ +#define GEOMETRY__PLANE_HPP_ + +#include +#include +#include +#include + +namespace math +{ +namespace geometry +{ +struct Plane +{ + geometry_msgs::msg::Vector3 normal_; + double d_; + + Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal); + auto calculateOffset(const geometry_msgs::msg::Point & point) -> double; +}; + +auto makePlane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal) + -> std::optional; + +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__PLANE_HPP_ diff --git a/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp b/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp new file mode 100644 index 00000000000..298a9e2d481 --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp @@ -0,0 +1,39 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_ +#define GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_ + +#include // Załaduj biblioteki Eigen +#include + +namespace math +{ +namespace geometry +{ +template < + typename T, std::enable_if_t>, std::nullptr_t> = nullptr> +auto getAngleDifference(const T & quat1, const T & quat2) -> double +{ + const Eigen::Quaterniond q1(quat1.w, quat1.x, quat1.y, quat1.z); + const Eigen::Quaterniond q2(quat2.w, quat2.x, quat2.y, quat2.z); + + Eigen::AngleAxisd delta(q1.inverse() * q2); + + return std::abs(delta.angle()); // [rad] +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_ diff --git a/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp b/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp new file mode 100644 index 00000000000..b315df00c1a --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp @@ -0,0 +1,41 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_ +#define GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_ + +#include +#include +#include + +namespace math +{ +namespace geometry +{ +template < + typename T, std::enable_if_t>, std::nullptr_t> = nullptr> +auto getNormalVector(const T & orientation) -> geometry_msgs::msg::Vector3 +{ + const Eigen::Matrix3d rotation_matrix = getRotationMatrix(orientation); + + return geometry_msgs::build() + .x(rotation_matrix(0, 2)) + .y(rotation_matrix(1, 2)) + .z(rotation_matrix(2, 2)); +} + +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_ \ No newline at end of file diff --git a/common/math/geometry/src/plane.cpp b/common/math/geometry/src/plane.cpp new file mode 100644 index 00000000000..ea4805b2bc0 --- /dev/null +++ b/common/math/geometry/src/plane.cpp @@ -0,0 +1,48 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +namespace math +{ +namespace geometry +{ + +Plane::Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal) +: normal_(normal), d_(-(normal.x * point.x + normal.y * point.y + normal.z * point.z)) +{ +} + +auto Plane::calculateOffset(const geometry_msgs::msg::Point & point) -> double +{ + return normal_.x * point.x + normal_.y * point.y + normal_.z * point.z + d_; +} + +auto makePlane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal) + -> std::optional +{ + if (normal.x == 0.0 && normal.y == 0.0 && normal.z == 0.0) { + return std::nullopt; + } + + if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) { + return std::nullopt; + } + + return Plane(point, normal); +} + +} // namespace geometry +} // namespace math 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 4bcba6c41cc..0b2c7350e5d 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 @@ -139,6 +139,8 @@ class ActionNode : public BT::ActionNodeBase -> std::vector; auto getConflictingEntityStatusOnLane(const lanelet::Ids & route_lanelets) const -> std::vector; + auto isEntityAltitudeCompatible( + const traffic_simulator::CanonicalizedEntityStatus & other_entity_status) const -> bool; }; } // namespace entity_behavior diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index cbe86b1c649..82f77d43358 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -316,7 +316,7 @@ auto ActionNode::getDistanceToTargetEntityPolygon( double width_extension_left, double length_extension_front, double length_extension_rear) const -> std::optional { - if (status.laneMatchingSucceed()) { + if (status.laneMatchingSucceed() && isEntityAltitudeCompatible(status)) { const auto polygon = math::geometry::transformPoints( status.getMapPose(), math::geometry::getPointsFromBbox( status.getBoundingBox(), width_extension_right, width_extension_left, @@ -326,6 +326,14 @@ auto ActionNode::getDistanceToTargetEntityPolygon( return std::nullopt; } +auto ActionNode::isEntityAltitudeCompatible( + const traffic_simulator::CanonicalizedEntityStatus & other_entity_status) const -> bool +{ + return hdmap_utils->isAltitudeMatching( + hdmap_utils->getAltitude(canonicalized_entity_status->getLaneletPose()), + hdmap_utils->getAltitude(other_entity_status.getLaneletPose())); +} + auto ActionNode::getDistanceToConflictingEntity( const lanelet::Ids & route_lanelets, const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index f5545a0d9d4..33de5b9b8f1 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -17,7 +17,10 @@ #include +#include +#include #include +#include #include #include #include @@ -37,7 +40,11 @@ class DetectionSensorBase explicit DetectionSensorBase( const double current_simulation_time, const simulation_api_schema::DetectionSensorConfiguration & configuration) - : previous_simulation_time_(current_simulation_time), configuration_(configuration) + : previous_simulation_time_(current_simulation_time), + configuration_(configuration), + ego_pose_(), + previous_ego_pose_(std::nullopt), + ego_plane_(std::nullopt) { } @@ -48,6 +55,9 @@ class DetectionSensorBase const std::vector &) const -> std::vector::const_iterator; + auto isOnOrAboveEgoPlane( + const geometry_msgs::Pose & npc_pose, const geometry_msgs::Pose & ego_pose) -> bool; + public: virtual ~DetectionSensorBase() = default; @@ -55,6 +65,36 @@ class DetectionSensorBase const double current_simulation_time, const std::vector &, const rclcpp::Time & current_ros_time, const std::vector & lidar_detected_entities) = 0; + +private: + /* + The threshold for detecting significant changes in ego vehicle's orientation (unit: radian). + The value determines the minimum angular difference required to consider the ego orientation + as "changed". + + There is no technical basis for this value, it was determined based on Szymon Parapura + experiments. + */ + constexpr static double rotation_threshold_ = 0.04; + + /* + Maximum downward offset in Z-axis relative to the ego position (unit: meter). + If the NPC is lower than this offset relative to the ego position, + the NPC will be excluded from detection + + There is no technical basis for this value, it was determined based on Szymon Parapura + experiments. + */ + constexpr static double max_downward_z_offset_ = 1.0; + + geometry_msgs::msg::Pose ego_pose_; + std::optional previous_ego_pose_; + std::optional ego_plane_; + + auto isAltitudeDifferenceWithinThreshold(const geometry_msgs::msg::Pose & entity_pose) const + -> bool; + auto needToUpdateEgoPlane() const -> bool; + auto hasEgoOrientationChanged() const -> bool; }; template diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index 23b38f4b094..187d93eab60 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include #include #include @@ -62,6 +64,55 @@ auto DetectionSensorBase::findEgoEntityStatusToWhichThisSensorIsAttached( } } +auto DetectionSensorBase::isOnOrAboveEgoPlane( + const geometry_msgs::Pose & npc_pose, const geometry_msgs::Pose & ego_pose) -> bool +{ + using math::geometry::getNormalVector; + using math::geometry::makePlane; + + geometry_msgs::msg::Pose entity_pose; + simulation_interface::toMsg(ego_pose, ego_pose_); + simulation_interface::toMsg(npc_pose, entity_pose); + + if (isAltitudeDifferenceWithinThreshold(entity_pose)) { + return true; + } + + if (needToUpdateEgoPlane()) { + ego_plane_ = makePlane(ego_pose_.position, getNormalVector(ego_pose_.orientation)); + if (ego_plane_) { + previous_ego_pose_ = ego_pose_; + } else { + return false; + } + } + + return ego_plane_.value().calculateOffset(entity_pose.position) >= 0.0; +} + +auto DetectionSensorBase::isAltitudeDifferenceWithinThreshold( + const geometry_msgs::msg::Pose & entity_pose) const -> bool +{ + return entity_pose.position.z >= (ego_pose_.position.z - max_downward_z_offset_); +} + +auto DetectionSensorBase::needToUpdateEgoPlane() const -> bool +{ + return !ego_plane_ || hasEgoOrientationChanged(); +} + +auto DetectionSensorBase::hasEgoOrientationChanged() const -> bool +{ + using math::geometry::getAngleDifference; + + if (!previous_ego_pose_) { + return true; + } + + return getAngleDifference(ego_pose_.orientation, previous_ego_pose_.value().orientation) > + rotation_threshold_; +} + template auto make(From &&...) -> To; @@ -322,6 +373,7 @@ auto DetectionSensor::updat auto is_in_range = [&](const auto & status) { return not isEgoEntityStatusToWhichThisSensorIsAttached(status) and distance(status.pose(), ego_entity_status->pose()) <= configuration_.range() and + isOnOrAboveEgoPlane(status.pose(), ego_entity_status->pose()) and (configuration_.detect_all_objects_in_range() or std::find( lidar_detected_entities.begin(), lidar_detected_entities.end(), status.name()) != From 0a4b2b01b2bda8a115a53c1e85b31a7184ef8c14 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 9 Dec 2024 18:01:56 +0900 Subject: [PATCH 067/149] Update `FieldOperatorApplication::getTurnIndicatorsCommand` to pure virtual Signed-off-by: yamacir-kit --- .../include/concealer/field_operator_application.hpp | 2 +- .../concealer/src/field_operator_application.cpp | 12 ------------ 2 files changed, 1 insertion(+), 13 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 2e35afb5ef6..7bd7df1c0c0 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -145,7 +145,7 @@ class FieldOperatorApplication : public rclcpp::Node virtual auto restrictTargetSpeed(double) const -> double = 0; virtual auto getTurnIndicatorsCommand() const - -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand; + -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand = 0; virtual auto rethrow() const noexcept(false) -> void; diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 747c204a68c..3095a17d2b1 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -146,18 +146,6 @@ auto FieldOperatorApplication::shutdownAutoware() -> void } } -auto FieldOperatorApplication::getTurnIndicatorsCommand() const - -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand -{ - static auto turn_indicators_command = []() { - autoware_vehicle_msgs::msg::TurnIndicatorsCommand turn_indicators_command; - turn_indicators_command.command = autoware_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND; - return turn_indicators_command; - }(); - turn_indicators_command.stamp = now(); - return turn_indicators_command; -} - auto FieldOperatorApplication::rethrow() const -> void { task_queue.rethrow(); } } // namespace concealer From 21baa9e093eb4b46c925097736aa8e34755ecaed Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Mon, 9 Dec 2024 10:17:04 +0100 Subject: [PATCH 068/149] Fix missing newline at end of file --- .../geometry/include/geometry/quaternion/get_normal_vector.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp b/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp index b315df00c1a..318a239c16d 100644 --- a/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp +++ b/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp @@ -38,4 +38,4 @@ auto getNormalVector(const T & orientation) -> geometry_msgs::msg::Vector3 } // namespace geometry } // namespace math -#endif // GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_ \ No newline at end of file +#endif // GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_ From 064ac141640434fd09aabcc3a3b847c80a7d6bd4 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 9 Dec 2024 18:26:17 +0900 Subject: [PATCH 069/149] Remove stream input/output operator for `TurnIndicatorsCommand` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 7 --- .../src/field_operator_application.cpp | 55 ------------------- .../syntax/user_defined_value_condition.cpp | 14 ++++- 3 files changed, 12 insertions(+), 64 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 7bd7df1c0c0..5d1dc4c2bbe 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -157,11 +157,4 @@ class FieldOperatorApplication : public rclcpp::Node }; } // namespace concealer -namespace autoware_vehicle_msgs::msg -{ -auto operator<<(std::ostream &, const TurnIndicatorsCommand &) -> std::ostream &; - -auto operator>>(std::istream &, TurnIndicatorsCommand &) -> std::istream &; -} // namespace autoware_vehicle_msgs::msg - #endif // CONCEALER__AUTOWARE_USER_HPP_ diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 3095a17d2b1..efdcc388a40 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -148,58 +148,3 @@ auto FieldOperatorApplication::shutdownAutoware() -> void auto FieldOperatorApplication::rethrow() const -> void { task_queue.rethrow(); } } // namespace concealer - -namespace autoware_vehicle_msgs::msg -{ -auto operator<<( - std::ostream & out, const autoware_vehicle_msgs::msg::TurnIndicatorsCommand & message) - -> std::ostream & -{ -#define CASE(IDENTIFIER) \ - case autoware_vehicle_msgs::msg::TurnIndicatorsCommand::IDENTIFIER: \ - out << #IDENTIFIER; \ - break - - switch (message.command) { - CASE(DISABLE); - CASE(ENABLE_LEFT); - CASE(ENABLE_RIGHT); - CASE(NO_COMMAND); - - default: - throw common::Error( - "Unsupported TurnIndicatorsCommand, state number : ", static_cast(message.command)); - } - -#undef CASE - - return out; -} - -auto operator>>(std::istream & is, autoware_vehicle_msgs::msg::TurnIndicatorsCommand & message) - -> std::istream & -{ -#define STATE(IDENTIFIER) \ - {#IDENTIFIER, autoware_vehicle_msgs::msg::TurnIndicatorsCommand::IDENTIFIER} - - std::unordered_map state_dictionary{ - STATE(DISABLE), - STATE(ENABLE_LEFT), - STATE(ENABLE_RIGHT), - STATE(NO_COMMAND), - }; - -#undef STATE - - std::string command_string; - is >> command_string; - - if (auto iter = state_dictionary.find(command_string); iter != state_dictionary.end()) { - message.set__command(iter->second); - } else { - throw common::Error("Unsupported TurnIndicatorsCommand::command : ", command_string.c_str()); - } - - return is; -} -} // namespace autoware_vehicle_msgs::msg diff --git a/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp index f7c0bb10c68..b9cfae284bc 100644 --- a/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp @@ -147,8 +147,18 @@ UserDefinedValueCondition::UserDefinedValueCondition(const pugi::xml_node & node std::make_pair( "currentTurnIndicatorsState", [result]() { - return make(boost::lexical_cast( - asFieldOperatorApplication(result.str(1)).getTurnIndicatorsCommand())); + switch (asFieldOperatorApplication(result.str(1)).getTurnIndicatorsCommand().command) { + case autoware_vehicle_msgs::msg::TurnIndicatorsCommand::DISABLE: + return make("DISABLE"); + case autoware_vehicle_msgs::msg::TurnIndicatorsCommand::ENABLE_LEFT: + return make("ENABLE_LEFT"); + case autoware_vehicle_msgs::msg::TurnIndicatorsCommand::ENABLE_RIGHT: + return make("ENABLE_RIGHT"); + case autoware_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND: + return make("NO_COMMAND"); + default: + return make(); + } }), }; evaluate_value = dispatch.at(result.str(2)); // XXX catch From 580c1d2e30d8fa912652aa14199c89c0bea12c60 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 9 Dec 2024 18:36:28 +0900 Subject: [PATCH 070/149] Remove member function `FieldOperatorApplication::restrictTargetSpeed` Signed-off-by: yamacir-kit --- .../include/concealer/field_operator_application.hpp | 3 --- .../field_operator_application_for_autoware_universe.hpp | 2 -- .../field_operator_application_for_autoware_universe.cpp | 7 ------- simulation/traffic_simulator/src/entity/ego_entity.cpp | 1 - 4 files changed, 13 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 5d1dc4c2bbe..b20a738cc78 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -141,9 +141,6 @@ class FieldOperatorApplication : public rclcpp::Node virtual auto requestAutoModeForCooperation(const std::string &, bool) -> void = 0; - // different autowares accept different initial target speed - virtual auto restrictTargetSpeed(double) const -> double = 0; - virtual auto getTurnIndicatorsCommand() const -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand = 0; diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index cd6e156170c..2948ae427bd 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -192,8 +192,6 @@ class FieldOperatorApplicationFor auto requestAutoModeForCooperation(const std::string &, bool) -> void override; - auto restrictTargetSpeed(double) const -> double override; - auto sendCooperateCommand(const std::string &, const std::string &) -> void override; auto setVelocityLimit(double) -> void override; diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index e75ff70ab01..ca2bdb483d0 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -405,13 +405,6 @@ auto FieldOperatorApplicationFor::getTurnIndicatorsCommand() c return getTurnIndicatorsCommandImpl(); } -auto FieldOperatorApplicationFor::restrictTargetSpeed(double value) const - -> double -{ - // no restrictions here - return value; -} - auto FieldOperatorApplicationFor::getAutowareStateName() const -> std::string { return autoware_state; diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index e5cce78c26c..ee67a46d9ce 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -278,7 +278,6 @@ auto EgoEntity::requestSpeedChange(double value, bool /* continuous */) -> void THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario."); } else { target_speed_ = value; - field_operator_application->restrictTargetSpeed(value); } } From 830b8f1173f17d2e54340ebf77f6ccb85aaaa9ab Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 9 Dec 2024 18:47:48 +0900 Subject: [PATCH 071/149] Remove member function `FieldOperatorApplication::checkAutowareProcess` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 2 - .../src/field_operator_application.cpp | 47 +++++++++---------- 2 files changed, 21 insertions(+), 28 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index b20a738cc78..2d12c82470c 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -60,8 +60,6 @@ class FieldOperatorApplication : public rclcpp::Node bool is_autoware_exited = false; - auto checkAutowareProcess() -> void; - protected: const pid_t process_id = 0; diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index efdcc388a40..6d154b97b46 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -36,36 +36,31 @@ auto FieldOperatorApplication::isStopRequested() const noexcept -> bool auto FieldOperatorApplication::spinSome() -> void { if (rclcpp::ok() and not isStopRequested()) { - checkAutowareProcess(); - rclcpp::spin_some(get_node_base_interface()); - } -} + if (process_id != 0) { + int wstatus = 0; + int ret = waitpid(process_id, &wstatus, WNOHANG); + if (ret == 0) { + return; + } else if (ret < 0) { + if (errno == ECHILD) { + is_autoware_exited = true; + throw common::AutowareError("Autoware process is already terminated"); + } else { + AUTOWARE_SYSTEM_ERROR("waitpid"); + std::exit(EXIT_FAILURE); + } + } -auto FieldOperatorApplication::checkAutowareProcess() -> void -{ - if (process_id != 0) { - int wstatus = 0; - int ret = waitpid(process_id, &wstatus, WNOHANG); - if (ret == 0) { - return; - } else if (ret < 0) { - if (errno == ECHILD) { + if (WIFEXITED(wstatus)) { is_autoware_exited = true; - throw common::AutowareError("Autoware process is already terminated"); - } else { - AUTOWARE_SYSTEM_ERROR("waitpid"); - std::exit(EXIT_FAILURE); + throw common::AutowareError( + "Autoware process is unintentionally exited. exit code: ", WEXITSTATUS(wstatus)); + } else if (WIFSIGNALED(wstatus)) { + is_autoware_exited = true; + throw common::AutowareError("Autoware process is killed. signal is ", WTERMSIG(wstatus)); } } - - if (WIFEXITED(wstatus)) { - is_autoware_exited = true; - throw common::AutowareError( - "Autoware process is unintentionally exited. exit code: ", WEXITSTATUS(wstatus)); - } else if (WIFSIGNALED(wstatus)) { - is_autoware_exited = true; - throw common::AutowareError("Autoware process is killed. signal is ", WTERMSIG(wstatus)); - } + rclcpp::spin_some(get_node_base_interface()); } } From d27b8f5ebe2f11a8b167dc7d614b1c60f86fb836 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Mon, 9 Dec 2024 11:20:02 +0100 Subject: [PATCH 072/149] Removed unrecognized words because spell-check flagged them as invalid --- .../sensor_simulation/detection_sensor/detection_sensor.hpp | 6 ++---- .../include/traffic_simulator/hdmap_utils/hdmap_utils.hpp | 2 +- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index 33de5b9b8f1..b220571ef7a 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -72,8 +72,7 @@ class DetectionSensorBase The value determines the minimum angular difference required to consider the ego orientation as "changed". - There is no technical basis for this value, it was determined based on Szymon Parapura - experiments. + There is no technical basis for this value, it was determined based on experiments. */ constexpr static double rotation_threshold_ = 0.04; @@ -82,8 +81,7 @@ class DetectionSensorBase If the NPC is lower than this offset relative to the ego position, the NPC will be excluded from detection - There is no technical basis for this value, it was determined based on Szymon Parapura - experiments. + There is no technical basis for this value, it was determined based on experiments. */ constexpr static double max_downward_z_offset_ = 1.0; diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 618b63b688d..6e5bec89712 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -394,7 +394,7 @@ class HdMapUtils the center of the rear axle projected onto the ground in the case of vehicles. There is no technical basis for this value; it was determined based on - experiments conducted by Szymon Parapura. + experiments. */ static constexpr double altitude_threshold_ = 1.0; From 8a5d5577e674cee5b4df7c685984d97343855ab4 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 9 Dec 2024 13:46:48 +0100 Subject: [PATCH 073/149] remove lanelet_id from the constructor --- .../traffic_simulator/traffic/traffic_sink.hpp | 12 ++++-------- .../src/traffic/traffic_controller.cpp | 2 +- .../traffic_simulator/src/traffic/traffic_sink.cpp | 6 +++--- 3 files changed, 8 insertions(+), 12 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 27e4a7fa24d..a6b53a05b8f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -45,26 +45,21 @@ class TrafficSink : public TrafficModuleBase public: /** * @brief Construct a new Traffic Sink object - * @param lanelet_id Lanelet ID for visualization - * @todo lanelet_id value is only used for visualization and its very confusing. So it should be refactor. + * @param entity_manager_ptr Shared pointer, refers to the EntityManager * @param radius The entity despawn when the distance between the entity's coordinates in the Map coordinate system and the TrafficSink's coordinates is less than this value. * @param position Position of the traffic sink. - * @param get_entity_names Function to get the name of entity - * @param get_entity_type Function to get the type of entity * @param sinkable_entity_type If this type is applicable, the entity despawn only when it approaches * radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. - * @param get_entity_pose Function to get the pose of entity. - * @param despawn Function to despawn entity. */ explicit TrafficSink( const std::shared_ptr entity_manager_ptr, - const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, + const double radius, const geometry_msgs::msg::Point & position, const std::set & sinkable_entity_type); void execute(const double current_time, const double step_time) override; auto appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void override; - const lanelet::Id lanelet_id; + const std::size_t unique_id; const double radius; const geometry_msgs::msg::Point position; @@ -83,6 +78,7 @@ class TrafficSink : public TrafficModuleBase const std::shared_ptr entity_manager_ptr; const std::set sinkable_entity_type; + inline static std::size_t unique_id_counter = 0UL; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index f2f220b848c..4f3c452061b 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -60,7 +60,7 @@ void TrafficController::autoSink() lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_ptr); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_ptr); addModule( - entity_manager_ptr, lanelet_id, 1, pose.position, sinkable_entity_type); + entity_manager_ptr, 1, pose.position, sinkable_entity_type); } } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index dbf60347d63..c73cb2360b1 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -41,10 +41,10 @@ namespace traffic { TrafficSink::TrafficSink( const std::shared_ptr entity_manager_ptr, - const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, + const double radius, const geometry_msgs::msg::Point & position, const std::set & sinkable_entity_type) : TrafficModuleBase(), - lanelet_id(lanelet_id), + unique_id(unique_id_counter++), radius(radius), position(position), entity_manager_ptr(entity_manager_ptr), @@ -73,7 +73,7 @@ void TrafficSink::execute( auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void { - const auto lanelet_text = std::to_string(lanelet_id); + const auto lanelet_text = std::string("sink_") + std::to_string(unique_id); visualization_msgs::msg::Marker traffic_sink_marker; traffic_sink_marker.header.frame_id = "map"; traffic_sink_marker.ns = "traffic_controller/traffic_sink/" + lanelet_text; From 4aefb2acba9996c884b37d3f21cbb646e5c626a5 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 9 Dec 2024 14:43:28 +0100 Subject: [PATCH 074/149] review suggestions --- .../src/crosswalk/stop_at_crosswalk.cpp | 2 +- .../src/traffic_sink/auto_sink_vehicle.cpp | 13 +++++-------- .../traffic/traffic_controller.hpp | 6 +++--- .../traffic_simulator/traffic/traffic_sink.hpp | 14 ++++++-------- .../src/traffic/traffic_controller.cpp | 5 ++--- .../traffic_simulator/src/traffic/traffic_sink.cpp | 8 +++----- 6 files changed, 20 insertions(+), 28 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index c0d70ef03e7..74dbf25598b 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -30,7 +30,7 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode explicit StopAtCrosswalkScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "stop_at_crosswalk", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option, true) + "lanelet2_map.osm", __FILE__, false, option) { start(); } diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index ccdcdef41ff..59b2ad42b21 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -50,17 +50,14 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { + const auto canonicalized_lanelet_pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34774, 11.0, 0.0, api_.getHdmapUtils()); api_.spawn( - "ego", - traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34774, 11.0, 0.0, api_.getHdmapUtils())), + "ego", traffic_simulator::pose::toMapPose(canonicalized_lanelet_pose), getVehicleParameters()); api_.spawn( - "bob", - traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34774, 11.0, 0.0, api_.getHdmapUtils())), + "bob", traffic_simulator::pose::toMapPose(canonicalized_lanelet_pose), getPedestrianParameters()); } }; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 50d0414ad3d..43b11d15b12 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -43,7 +43,7 @@ class TrafficController { public: explicit TrafficController( - const std::shared_ptr entity_manager_ptr, + const std::shared_ptr entity_manager_ptr, const std::set & sinkable_entity_type, bool auto_sink = false); template @@ -57,8 +57,8 @@ class TrafficController private: void autoSink(); - const std::shared_ptr entity_manager_ptr; - std::vector> modules_; + const std::shared_ptr entity_manager_ptr; + std::vector> modules_; const std::set sinkable_entity_type; public: diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index a6b53a05b8f..7ac9c9062bb 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -46,14 +46,13 @@ class TrafficSink : public TrafficModuleBase /** * @brief Construct a new Traffic Sink object * @param entity_manager_ptr Shared pointer, refers to the EntityManager - * @param radius The entity despawn when the distance between the entity's coordinates in the Map coordinate system and the TrafficSink's coordinates is less than this value. + * @param radius Radius of the sink * @param position Position of the traffic sink. - * @param sinkable_entity_type If this type is applicable, the entity despawn only when it approaches - * radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. + * @param sinkable_entity_type Candidates for despawn. If empty, all entities are sinkable */ explicit TrafficSink( - const std::shared_ptr entity_manager_ptr, - const double radius, const geometry_msgs::msg::Point & position, + const std::shared_ptr entity_manager_ptr, const double radius, + const geometry_msgs::msg::Point & position, const std::set & sinkable_entity_type); void execute(const double current_time, const double step_time) override; auto appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const @@ -65,8 +64,7 @@ class TrafficSink : public TrafficModuleBase private: auto getEntityNames() const -> std::vector; - auto getEntityType(const std::string & entity_name) const noexcept(false) - -> traffic_simulator::EntityType; + auto getEntityType(const std::string & entity_name) const noexcept(false) -> EntityType; auto getEntityPose(const std::string & entity_name) const noexcept(false) -> geometry_msgs::msg::Pose; /** @@ -76,7 +74,7 @@ class TrafficSink : public TrafficModuleBase */ auto despawn(const std::string & entity_name) const -> void; - const std::shared_ptr entity_manager_ptr; + const std::shared_ptr entity_manager_ptr; const std::set sinkable_entity_type; inline static std::size_t unique_id_counter = 0UL; }; diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 4f3c452061b..3db9b7aabde 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -39,7 +39,7 @@ namespace traffic_simulator namespace traffic { TrafficController::TrafficController( - const std::shared_ptr entity_manager_ptr, + const std::shared_ptr entity_manager_ptr, const std::set & sinkable_entity_type, bool auto_sink) : entity_manager_ptr(entity_manager_ptr), sinkable_entity_type(sinkable_entity_type), @@ -59,8 +59,7 @@ void TrafficController::autoSink() lanelet_pose.lanelet_id = lanelet_id; lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_ptr); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_ptr); - addModule( - entity_manager_ptr, 1, pose.position, sinkable_entity_type); + addModule(entity_manager_ptr, 1, pose.position, sinkable_entity_type); } } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index c73cb2360b1..a3373b2363d 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -40,9 +40,8 @@ namespace traffic_simulator namespace traffic { TrafficSink::TrafficSink( - const std::shared_ptr entity_manager_ptr, - const double radius, const geometry_msgs::msg::Point & position, - const std::set & sinkable_entity_type) + const std::shared_ptr entity_manager_ptr, const double radius, + const geometry_msgs::msg::Point & position, const std::set & sinkable_entity_type) : TrafficModuleBase(), unique_id(unique_id_counter++), radius(radius), @@ -104,8 +103,7 @@ auto TrafficSink::getEntityNames() const -> std::vector return entity_manager_ptr->getEntityNames(); } -auto TrafficSink::getEntityType(const std::string & entity_name) const noexcept(false) - -> traffic_simulator::EntityType +auto TrafficSink::getEntityType(const std::string & entity_name) const noexcept(false) -> EntityType { if (const auto entity = entity_manager_ptr->getEntity(entity_name)) { return entity->getEntityType(); From 729c31f61c7c3846493b9b4f99e526f4c5c56be7 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Mon, 9 Dec 2024 16:13:52 +0100 Subject: [PATCH 075/149] Fix an issue with unit tests - distanceTest --- .../test/src/hdmap_utils/test_hdmap_utils.cpp | 6 +- .../test/src/helper_functions.hpp | 5 +- .../test/src/utils/test_distance.cpp | 116 +++++++++--------- 3 files changed, 65 insertions(+), 62 deletions(-) diff --git a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp index eaa36c849a4..dd0dda7330f 100644 --- a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp +++ b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp @@ -2098,9 +2098,11 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_differentLanelet) TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLongitudinalDistance_differentLaneletNoRoute) { const auto pose_to = hdmap_utils.toLaneletPose( - makePose(makePoint(81590.79, 50067.66), makeQuaternionFromYaw(90.0)), lanelet::Id{3002185}); + makePose(makePoint(81590.79, 50067.66, 35.0), makeQuaternionFromYaw(90.0)), + lanelet::Id{3002185}); const auto pose_from = hdmap_utils.toLaneletPose( - makePose(makePoint(81596.20, 50068.04), makeQuaternionFromYaw(90.0)), lanelet::Id{3002166}); + makePose(makePoint(81596.20, 50068.04, 35.0), makeQuaternionFromYaw(90.0)), + lanelet::Id{3002166}); ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index a12b5e33328..c10cc1d4931 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -63,13 +63,14 @@ auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion geometry_msgs::build().x(0.0).y(0.0).z(yaw)); } -auto makePose(const double x, const double y, const double yaw_deg) -> geometry_msgs::msg::Pose +auto makePose(const double x, const double y, const double z, const double yaw_deg) + -> geometry_msgs::msg::Pose { /** * @note +x axis is 0 degrees; +y axis is 90 degrees */ return geometry_msgs::build() - .position(geometry_msgs::build().x(x).y(y).z(0.0)) + .position(geometry_msgs::build().x(x).y(y).z(z)) .orientation(math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z( convertDegToRad(yaw_deg)))); diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index ebe1558fb98..d670eeef614 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -238,10 +238,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81595.44, 50006.09, 100.0), false, hdmap_utils_ptr); + makePose(81595.44, 50006.09, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81584.48, 50084.76, 100.0), false, hdmap_utils_ptr); + makePose(81584.48, 50084.76, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -260,10 +260,10 @@ TEST_F(distanceTest_StandardMap, longitudinalDistance_noAdjacent_noOpposite_noCh { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(3800.05, 73715.77, 30.0), false, hdmap_utils_ptr); + makePose(3800.05, 73715.77, 0.5, 30.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(3841.26, 73748.80, 110.0), false, hdmap_utils_ptr); + makePose(3841.26, 73748.80, 0.5, 110.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -283,10 +283,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81585.79, 50042.62, 100.0), false, hdmap_utils_ptr); + makePose(81585.79, 50042.62, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81588.34, 50083.23, 100.0), false, hdmap_utils_ptr); + makePose(81588.34, 50083.23, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -305,10 +305,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.02, 50065.76, 280.0), false, hdmap_utils_ptr); + makePose(81599.02, 50065.76, 35.0, 280.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.61, 50045.16, 280.0), false, hdmap_utils_ptr); + makePose(81599.61, 50045.16, 35.0, 280.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -328,10 +328,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81595.47, 49982.80, 100.0), false, hdmap_utils_ptr); + makePose(81595.47, 49982.80, 36.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.34, 50022.34, 100.0), false, hdmap_utils_ptr); + makePose(81599.34, 50022.34, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -343,10 +343,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81612.35, 50015.63, 280.0), false, hdmap_utils_ptr); + makePose(81612.35, 50015.63, 35.0, 280.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81612.95, 49991.30, 280.0), false, hdmap_utils_ptr); + makePose(81612.95, 49991.30, 35.5, 280.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -367,10 +367,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81592.96, 49997.94, 100.0), false, hdmap_utils_ptr); + makePose(81592.96, 49997.94, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81570.56, 50141.75, 100.0), false, hdmap_utils_ptr); + makePose(81570.56, 50141.75, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -383,10 +383,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81587.31, 50165.57, 100.0), false, hdmap_utils_ptr); + makePose(81587.31, 50165.57, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81610.25, 49988.59, 100.0), false, hdmap_utils_ptr); + makePose(81610.25, 49988.59, 35.5, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -406,10 +406,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86627.71, 44972.06, 340.0), false, hdmap_utils_ptr); + makePose(86627.71, 44972.06, 3.0, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr); + makePose(86647.23, 44882.51, 3.0, 240.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -421,10 +421,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86555.38, 45000.88, 340.0), false, hdmap_utils_ptr); + makePose(86555.38, 45000.88, 3.0, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr); + makePose(86647.23, 44882.51, 3.0, 240.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -436,10 +436,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr); + makePose(86788.82, 44993.77, 3.0, 210.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86553.48, 44990.56, 150.0), false, hdmap_utils_ptr); + makePose(86553.48, 44990.56, 3.0, 150.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -451,10 +451,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr); + makePose(86788.82, 44993.77, 3.0, 210.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86579.91, 44979.00, 150.0), false, hdmap_utils_ptr); + makePose(86579.91, 44979.00, 3.0, 150.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -475,10 +475,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86736.13, 44969.63, 210.0), false, hdmap_utils_ptr); + makePose(86736.13, 44969.63, 3.0, 210.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86642.95, 44958.78, 340.0), false, hdmap_utils_ptr); + makePose(86642.95, 44958.78, 3.0, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -490,10 +490,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86732.06, 44976.58, 210.0), false, hdmap_utils_ptr); + makePose(86732.06, 44976.58, 3.0, 210.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86704.59, 44927.32, 340.0), false, hdmap_utils_ptr); + makePose(86704.59, 44927.32, 3.0, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -514,9 +514,9 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86637.19, 44967.35, 340.0), false, hdmap_utils_ptr); + makePose(86637.19, 44967.35, 3.0, 340.0), false, hdmap_utils_ptr); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86648.82, 44886.19, 240.0), false, hdmap_utils_ptr); + makePose(86648.82, 44886.19, 3.0, 240.0), false, hdmap_utils_ptr); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; @@ -528,10 +528,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86719.94, 44957.20, 210.0), false, hdmap_utils_ptr); + makePose(86719.94, 44957.20, 3.0, 210.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86599.32, 44975.01, 180.0), false, hdmap_utils_ptr); + makePose(86599.32, 44975.01, 3.0, 180.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -550,8 +550,8 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch */ TEST(distance, boundingBoxDistance_intersection) { - const auto pose_from = makePose(100.0, 100.0, 0.0); - const auto pose_to = makePose(120.0, 100.0, 90.0); + const auto pose_from = makePose(100.0, 100.0, 0.0, 0.0); + const auto pose_to = makePose(120.0, 100.0, 0.0, 90.0); const auto bounding_box_from = makeCustom2DBoundingBox(30.0, 1.0); const auto bounding_box_to = makeCustom2DBoundingBox(1.0, 30.0); @@ -569,8 +569,8 @@ TEST(distance, boundingBoxDistance_intersection) */ TEST(distance, boundingBoxDistance_disjoint) { - const auto pose_from = makePose(100.0, 100.0, 0.0); - const auto pose_to = makePose(120.0, 100.0, 90.0); + const auto pose_from = makePose(100.0, 100.0, 0.0, 0.0); + const auto pose_to = makePose(120.0, 100.0, 0.0, 90.0); const auto bounding_box_from = makeCustom2DBoundingBox(1.0, 30.0); const auto bounding_box_to = makeCustom2DBoundingBox(30.0, 1.0); @@ -592,56 +592,56 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_single) constexpr lanelet::Id lanelet_id = 34741L; constexpr double tolerance = 0.1; { - const auto pose = makePose(3818.33, 73726.18, 30.0); + const auto pose = makePose(3818.33, 73726.18, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 0.5, tolerance); } { - const auto pose = makePose(3816.89, 73723.09, 30.0); + const auto pose = makePose(3816.89, 73723.09, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 2.6, tolerance); } { - const auto pose = makePose(3813.42, 73721.11, 30.0); + const auto pose = makePose(3813.42, 73721.11, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 2.7, tolerance); } { - const auto pose = makePose(3813.42, 73721.11, 120.0); + const auto pose = makePose(3813.42, 73721.11, 0.0, 120.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 1.3, tolerance); } { - const auto pose = makePose(3810.99, 73721.40, 30.0); + const auto pose = makePose(3810.99, 73721.40, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 1.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 1.4, tolerance); } { - const auto pose = makePose(3810.99, 73721.40, 30.0); + const auto pose = makePose(3810.99, 73721.40, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, -1.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 2.4, tolerance); } { - const auto pose = makePose(3680.81, 73757.27, 30.0); + const auto pose = makePose(3680.81, 73757.27, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, 34684L, hdmap_utils_ptr); EXPECT_NEAR(result, 5.1, tolerance); } { - const auto pose = makePose(3692.79, 73753.00, 30.0); + const auto pose = makePose(3692.79, 73753.00, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, 34684L, hdmap_utils_ptr); @@ -656,7 +656,7 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_single) TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_multipleVector) { const auto lanelet_ids = lanelet::Ids{34603L, 34600L, 34621L, 34741L}; - const auto pose = makePose(3836.16, 73757.99, 120.0); + const auto pose = makePose(3836.16, 73757.99, 0.0, 120.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double actual_distance = std::transform_reduce( lanelet_ids.cbegin(), lanelet_ids.cend(), std::numeric_limits::max(), @@ -678,7 +678,7 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_multipleVector) TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_singleVector) { constexpr lanelet::Id lanelet_id = 34426L; - const auto pose = makePose(3693.34, 73738.37, 300.0); + const auto pose = makePose(3693.34, 73738.37, 0.0, 300.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double actual_distance = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); @@ -693,7 +693,7 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_singleVector) */ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_emptyVector) { - const auto pose = makePose(3825.87, 73773.08, 135.0); + const auto pose = makePose(3825.87, 73773.08, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); EXPECT_THROW( traffic_simulator::distance::distanceToLeftLaneBound( @@ -709,56 +709,56 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_single) constexpr lanelet::Id lanelet_id = 660L; constexpr double tolerance = 0.1; { - const auto pose = makePose(86651.84, 44941.47, 135.0); + const auto pose = makePose(86651.84, 44941.47, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 4.1, tolerance); } { - const auto pose = makePose(86653.05, 44946.74, 135.0); + const auto pose = makePose(86653.05, 44946.74, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 0.6, tolerance); } { - const auto pose = makePose(86651.47, 44941.07, 120.0); + const auto pose = makePose(86651.47, 44941.07, 0.0, 120.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 4.3, tolerance); } { - const auto pose = makePose(86651.47, 44941.07, 210.0); + const auto pose = makePose(86651.47, 44941.07, 0.0, 210.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 3.1, tolerance); } { - const auto pose = makePose(86644.10, 44951.86, 150.0); + const auto pose = makePose(86644.10, 44951.86, 0.0, 150.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 1.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 2.0, tolerance); } { - const auto pose = makePose(86644.10, 44951.86, 150.0); + const auto pose = makePose(86644.10, 44951.86, 0.0, 150.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, -1.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 1.1, tolerance); } { - const auto pose = makePose(86644.11, 44941.21, 0.0); + const auto pose = makePose(86644.11, 44941.21, 0.0, 0.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 11.2, tolerance); } { - const auto pose = makePose(86656.83, 44946.96, 0.0); + const auto pose = makePose(86656.83, 44946.96, 0.0, 0.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); @@ -773,7 +773,7 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_single) TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_multipleVector) { const auto lanelet_ids = lanelet::Ids{660L, 663L, 684L, 654L, 686L}; - const auto pose = makePose(86642.05, 44902.61, 60.0); + const auto pose = makePose(86642.05, 44902.61, 0.0, 60.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double actual_distance = std::transform_reduce( lanelet_ids.cbegin(), lanelet_ids.cend(), std::numeric_limits::max(), @@ -795,7 +795,7 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_multipleVector) TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_singleVector) { constexpr lanelet::Id lanelet_id = 654L; - const auto pose = makePose(86702.79, 44929.05, 150.0); + const auto pose = makePose(86702.79, 44929.05, 0.0, 150.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double actual_distance = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); @@ -810,7 +810,7 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_singleVector) */ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_emptyVector) { - const auto pose = makePose(3825.87, 73773.08, 135.0); + const auto pose = makePose(3825.87, 73773.08, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); EXPECT_THROW( traffic_simulator::distance::distanceToRightLaneBound( From 7be6fcee435a381d5aae18164af50bc483d23244 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 9 Dec 2024 16:35:44 +0100 Subject: [PATCH 076/149] AutoSinkConfig --- .../cpp_mock_scenarios/cpp_scenario_node.hpp | 13 +++++++------ .../src/cpp_scenario_node.cpp | 8 +++----- .../src/traffic_sink/auto_sink_vehicle.cpp | 7 ++++--- .../src/openscenario_interpreter.cpp | 2 +- .../include/traffic_simulator/api/api.hpp | 4 +++- .../traffic_simulator/api/configuration.hpp | 4 ++-- .../traffic/traffic_controller.hpp | 11 ++++++++--- .../traffic_simulator/traffic/traffic_sink.hpp | 8 ++++---- .../src/traffic/traffic_controller.cpp | 14 +++++++------- .../src/traffic/traffic_sink.cpp | 18 +++++++++++++----- 10 files changed, 52 insertions(+), 37 deletions(-) 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 680e4e128ef..70bea75da81 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 @@ -33,8 +33,8 @@ class CppScenarioNode : public rclcpp::Node explicit CppScenarioNode( const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, - const bool verbose, const rclcpp::NodeOptions & option, const bool auto_sink = false, - const std::set sinkable_entity_type = {}); + const bool verbose, const rclcpp::NodeOptions & option, + const traffic_simulator::traffic::AutoSinkConfig & auto_sink_config = {false, {}}); void start(); void stop(Result result, const std::string & description = ""); void expectThrow() { exception_expect_ = true; } @@ -72,8 +72,9 @@ class CppScenarioNode : public rclcpp::Node int timeout_; auto configure( const std::string & map_path, const std::string & lanelet2_map_file, - const std::string & scenario_filename, const bool verbose, const bool auto_sink = false, - const std::set sinkable_entity_type = {}) -> traffic_simulator::Configuration + const std::string & scenario_filename, const bool verbose, + const traffic_simulator::traffic::AutoSinkConfig & auto_sink_config = {false, {}}) + -> traffic_simulator::Configuration { auto configuration = traffic_simulator::Configuration(map_path); { @@ -81,8 +82,8 @@ class CppScenarioNode : public rclcpp::Node // configuration.lanelet2_map_file = "lanelet2_map_with_private_road_and_walkway_ele_fix.osm"; configuration.scenario_path = scenario_filename; configuration.verbose = verbose; - configuration.auto_sink = auto_sink; - configuration.sinkable_entity_type = sinkable_entity_type; + configuration.generate_auto_sink = auto_sink_config.generate_auto_sink; + configuration.default_sinkable_entity_type = auto_sink_config.default_sinkable_entity_type; } checkConfiguration(configuration); return configuration; diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index d49605402be..ecabc7967ed 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -20,13 +20,11 @@ namespace cpp_mock_scenarios CppScenarioNode::CppScenarioNode( const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, - const rclcpp::NodeOptions & option, const bool auto_sink, - const std::set sinkable_entity_type) + const rclcpp::NodeOptions & option, + const traffic_simulator::traffic::AutoSinkConfig & auto_sink_config /*= {false, {}}*/) : Node(node_name, option), api_( - this, - configure( - map_path, lanelet2_map_file, scenario_filename, verbose, auto_sink, sinkable_entity_type), + this, configure(map_path, lanelet2_map_file, scenario_filename, verbose, auto_sink_config), declare_parameter("global_real_time_factor", 1.0), declare_parameter("global_frame_rate", 20.0)), scenario_filename_(scenario_filename), diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index 59b2ad42b21..3e67930bfb4 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -30,8 +31,8 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode explicit AutoSinkVehicleScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "auto_sink_vehicle", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option, true, - {traffic_simulator::EntityType::PEDESTRIAN}) + "lanelet2_map.osm", __FILE__, false, option, + traffic_simulator::traffic::AutoSinkConfig{true, {traffic_simulator::EntityType::PEDESTRIAN}}) { start(); } @@ -39,7 +40,7 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - if (api_.getCurrentTime() >= 0.1) { + if (api_.getCurrentTime() >= 10.1) { if (api_.entityExists("ego") and not api_.entityExists("bob")) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index d2f7c910cc0..c1597f52251 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -70,7 +70,7 @@ auto Interpreter::makeCurrentConfiguration() const -> traffic_simulator::Configu auto configuration = traffic_simulator::Configuration( logic_file.isDirectory() ? logic_file : logic_file.filepath.parent_path()); { - configuration.auto_sink = false; + configuration.generate_auto_sink = false; configuration.scenario_path = osc_path; // XXX DIRTY HACK!!! diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 604e1d648be..300b9589e78 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -76,7 +76,9 @@ class API node, entity_manager_ptr_->getHdmapUtils(), getROS2Parameter("architecture_type", "awf/universe"))), traffic_controller_ptr_(std::make_shared( - entity_manager_ptr_, configuration.sinkable_entity_type, configuration.auto_sink)), + entity_manager_ptr_, + traffic_simulator::traffic::AutoSinkConfig{ + configuration.generate_auto_sink, configuration.default_sinkable_entity_type})), clock_pub_(rclcpp::create_publisher( node, "/clock", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), rclcpp::PublisherOptionsWithAllocator())), diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index c2b17c99d5e..07dbd4ff012 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -33,9 +33,9 @@ struct Configuration using Pathname = boost::filesystem::path; - bool auto_sink = true; + bool generate_auto_sink = true; - std::set sinkable_entity_type = {}; + std::unordered_set default_sinkable_entity_type = {}; bool verbose = false; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 43b11d15b12..9783161929f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -39,12 +39,18 @@ namespace traffic_simulator { namespace traffic { +struct AutoSinkConfig +{ + bool generate_auto_sink = true; + std::unordered_set default_sinkable_entity_type = {}; + static constexpr double radius = 1.0; +}; class TrafficController { public: explicit TrafficController( const std::shared_ptr entity_manager_ptr, - const std::set & sinkable_entity_type, bool auto_sink = false); + const AutoSinkConfig & auto_sink_config = {false, {}}); template void addModule(Ts &&... xs) @@ -59,10 +65,9 @@ class TrafficController void autoSink(); const std::shared_ptr entity_manager_ptr; std::vector> modules_; - const std::set sinkable_entity_type; public: - const bool auto_sink; + const AutoSinkConfig auto_sink_config; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 7ac9c9062bb..2d0acde43bd 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -53,12 +53,13 @@ class TrafficSink : public TrafficModuleBase explicit TrafficSink( const std::shared_ptr entity_manager_ptr, const double radius, const geometry_msgs::msg::Point & position, - const std::set & sinkable_entity_type); + const std::unordered_set & sinkable_entity_type, + const std::optional lanelet_id_opt = std::nullopt); void execute(const double current_time, const double step_time) override; auto appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void override; - const std::size_t unique_id; + const std::string description; const double radius; const geometry_msgs::msg::Point position; @@ -75,8 +76,7 @@ class TrafficSink : public TrafficModuleBase auto despawn(const std::string & entity_name) const -> void; const std::shared_ptr entity_manager_ptr; - const std::set sinkable_entity_type; - inline static std::size_t unique_id_counter = 0UL; + const std::unordered_set sinkable_entity_type; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 3db9b7aabde..a07e19305ec 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -40,12 +40,10 @@ namespace traffic { TrafficController::TrafficController( const std::shared_ptr entity_manager_ptr, - const std::set & sinkable_entity_type, bool auto_sink) -: entity_manager_ptr(entity_manager_ptr), - sinkable_entity_type(sinkable_entity_type), - auto_sink(auto_sink) + const AutoSinkConfig & auto_sink_config /* = {false, {}}*/) +: entity_manager_ptr(entity_manager_ptr), auto_sink_config(auto_sink_config) { - if (auto_sink) { + if (auto_sink_config.generate_auto_sink) { autoSink(); } } @@ -59,7 +57,9 @@ void TrafficController::autoSink() lanelet_pose.lanelet_id = lanelet_id; lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_ptr); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_ptr); - addModule(entity_manager_ptr, 1, pose.position, sinkable_entity_type); + addModule( + entity_manager_ptr, auto_sink_config.radius, pose.position, + auto_sink_config.default_sinkable_entity_type, std::make_optional(lanelet_id)); } } } @@ -75,7 +75,7 @@ auto TrafficController::makeDebugMarker() const -> const visualization_msgs::msg { static const auto marker_array = [&]() { visualization_msgs::msg::MarkerArray marker_array; - for (size_t i = 0; i < modules_.size(); ++i) { + for (size_t i = 0UL; i < modules_.size(); ++i) { modules_[i]->appendDebugMarker(marker_array); } return marker_array; diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index a3373b2363d..7c85b19b434 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -41,9 +41,18 @@ namespace traffic { TrafficSink::TrafficSink( const std::shared_ptr entity_manager_ptr, const double radius, - const geometry_msgs::msg::Point & position, const std::set & sinkable_entity_type) + const geometry_msgs::msg::Point & position, + const std::unordered_set & sinkable_entity_type, + const std::optional lanelet_id_opt /*= std::nullopt*/) : TrafficModuleBase(), - unique_id(unique_id_counter++), + description([](std::optional lanelet_id_opt) -> std::string { + static long unique_id = 0L; + if (lanelet_id_opt.has_value()) { + return std::string("auto_") + std::to_string(lanelet_id_opt.value()); + } else { + return std::string("custom_") + std::to_string(unique_id++); + } + }(lanelet_id_opt)), radius(radius), position(position), entity_manager_ptr(entity_manager_ptr), @@ -72,10 +81,9 @@ void TrafficSink::execute( auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void { - const auto lanelet_text = std::string("sink_") + std::to_string(unique_id); visualization_msgs::msg::Marker traffic_sink_marker; traffic_sink_marker.header.frame_id = "map"; - traffic_sink_marker.ns = "traffic_controller/traffic_sink/" + lanelet_text; + traffic_sink_marker.ns = "traffic_controller/traffic_sink/" + description; traffic_sink_marker.id = 0; traffic_sink_marker.action = traffic_sink_marker.ADD; traffic_sink_marker.type = 3; // cylinder @@ -92,7 +100,7 @@ auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marke text_marker = traffic_sink_marker; text_marker.id = 1; text_marker.type = 9; //text - text_marker.text = lanelet_text; + text_marker.text = description; text_marker.color = color_names::makeColorMsg("white", 0.99); text_marker.scale.z = 0.6; marker_array.markers.emplace_back(text_marker); From 69918d7652db2f704cd59285a0f4a0410dd7103e Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 9 Dec 2024 17:28:14 +0100 Subject: [PATCH 077/149] TrafficSinkConfig --- .../src/traffic_sink/auto_sink_vehicle.cpp | 2 +- .../traffic_simulator/api/configuration.hpp | 2 +- .../traffic/traffic_controller.hpp | 4 +- .../traffic/traffic_sink.hpp | 47 +++++++++++++---- .../src/traffic/traffic_controller.cpp | 9 ++-- .../src/traffic/traffic_sink.cpp | 50 +++++++------------ 6 files changed, 64 insertions(+), 50 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index 3e67930bfb4..ae39e75c7c3 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -40,7 +40,7 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - if (api_.getCurrentTime() >= 10.1) { + if (api_.getCurrentTime() >= 0.1) { if (api_.entityExists("ego") and not api_.entityExists("bob")) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index 07dbd4ff012..cfd33d0f306 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -35,7 +35,7 @@ struct Configuration bool generate_auto_sink = true; - std::unordered_set default_sinkable_entity_type = {}; + std::set default_sinkable_entity_type = {}; bool verbose = false; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 9783161929f..323073114bb 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -41,8 +41,8 @@ namespace traffic { struct AutoSinkConfig { - bool generate_auto_sink = true; - std::unordered_set default_sinkable_entity_type = {}; + const bool generate_auto_sink = true; + const std::set default_sinkable_entity_type = {}; static constexpr double radius = 1.0; }; class TrafficController diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 2d0acde43bd..3d80a8bc042 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -40,28 +40,54 @@ namespace traffic_simulator { namespace traffic { +struct TrafficSinkConfig +{ + /** + * @brief Construct a new TrafficSinkConfig object + * @param radius Radius of the traffic sink + * @param position Position of the traffic sink + * @param sinkable_entity_type Candidates for despawn. If empty, all entities are sinkable + */ + explicit TrafficSinkConfig( + const double radius, const geometry_msgs::msg::Point & position, + const std::set & sinkable_entity_type, + const std::optional lanelet_id_opt) + : radius(radius), + position(position), + sinkable_entity_type(sinkable_entity_type), + description([](std::optional lanelet_id_opt) -> std::string { + static long unique_id = 0L; + if (lanelet_id_opt.has_value()) { + return std::string("auto_") + std::to_string(lanelet_id_opt.value()); + } else { + return std::string("custom_") + std::to_string(unique_id++); + } + }(lanelet_id_opt)) + { + } + + const double radius; + const geometry_msgs::msg::Point position; + const std::set sinkable_entity_type; + const std::string description; +}; + class TrafficSink : public TrafficModuleBase { public: /** * @brief Construct a new Traffic Sink object * @param entity_manager_ptr Shared pointer, refers to the EntityManager - * @param radius Radius of the sink - * @param position Position of the traffic sink. - * @param sinkable_entity_type Candidates for despawn. If empty, all entities are sinkable + * @param config TrafficSink configuration */ explicit TrafficSink( - const std::shared_ptr entity_manager_ptr, const double radius, - const geometry_msgs::msg::Point & position, - const std::unordered_set & sinkable_entity_type, - const std::optional lanelet_id_opt = std::nullopt); + const std::shared_ptr entity_manager_ptr, + const TrafficSinkConfig & config); void execute(const double current_time, const double step_time) override; auto appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void override; - const std::string description; - const double radius; - const geometry_msgs::msg::Point position; + const TrafficSinkConfig config; private: auto getEntityNames() const -> std::vector; @@ -76,7 +102,6 @@ class TrafficSink : public TrafficModuleBase auto despawn(const std::string & entity_name) const -> void; const std::shared_ptr entity_manager_ptr; - const std::unordered_set sinkable_entity_type; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index a07e19305ec..28fa9b214f6 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -41,7 +41,7 @@ namespace traffic TrafficController::TrafficController( const std::shared_ptr entity_manager_ptr, const AutoSinkConfig & auto_sink_config /* = {false, {}}*/) -: entity_manager_ptr(entity_manager_ptr), auto_sink_config(auto_sink_config) +: entity_manager_ptr(entity_manager_ptr), modules_(), auto_sink_config(auto_sink_config) { if (auto_sink_config.generate_auto_sink) { autoSink(); @@ -57,9 +57,10 @@ void TrafficController::autoSink() lanelet_pose.lanelet_id = lanelet_id; lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_ptr); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_ptr); - addModule( - entity_manager_ptr, auto_sink_config.radius, pose.position, - auto_sink_config.default_sinkable_entity_type, std::make_optional(lanelet_id)); + const auto traffic_sink_config = TrafficSinkConfig( + auto_sink_config.radius, pose.position, auto_sink_config.default_sinkable_entity_type, + std::make_optional(lanelet_id)); + addModule(entity_manager_ptr, traffic_sink_config); } } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 7c85b19b434..c50d14680ee 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -40,23 +40,8 @@ namespace traffic_simulator namespace traffic { TrafficSink::TrafficSink( - const std::shared_ptr entity_manager_ptr, const double radius, - const geometry_msgs::msg::Point & position, - const std::unordered_set & sinkable_entity_type, - const std::optional lanelet_id_opt /*= std::nullopt*/) -: TrafficModuleBase(), - description([](std::optional lanelet_id_opt) -> std::string { - static long unique_id = 0L; - if (lanelet_id_opt.has_value()) { - return std::string("auto_") + std::to_string(lanelet_id_opt.value()); - } else { - return std::string("custom_") + std::to_string(unique_id++); - } - }(lanelet_id_opt)), - radius(radius), - position(position), - entity_manager_ptr(entity_manager_ptr), - sinkable_entity_type(sinkable_entity_type) + const std::shared_ptr entity_manager_ptr, const TrafficSinkConfig & config) +: TrafficModuleBase(), config(config), entity_manager_ptr(entity_manager_ptr) { } @@ -66,13 +51,14 @@ void TrafficSink::execute( const auto names = getEntityNames(); for (const auto & name : names) { const auto is_sinkable_entity = [this](const auto & entity_name) { - return sinkable_entity_type.empty() - ? true - : sinkable_entity_type.find(getEntityType(entity_name).type) != - sinkable_entity_type.end(); + return config.sinkable_entity_type.empty() or + config.sinkable_entity_type.find(getEntityType(entity_name).type) != + config.sinkable_entity_type.end(); }; const auto pose = getEntityPose(name); - if (is_sinkable_entity(name) and math::geometry::getDistance(position, pose) <= radius) { + if ( + is_sinkable_entity(name) and + math::geometry::getDistance(config.position, pose) <= config.radius) { despawn(name); } } @@ -83,16 +69,17 @@ auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marke { visualization_msgs::msg::Marker traffic_sink_marker; traffic_sink_marker.header.frame_id = "map"; - traffic_sink_marker.ns = "traffic_controller/traffic_sink/" + description; + traffic_sink_marker.ns = "traffic_controller/traffic_sink/" + config.description; traffic_sink_marker.id = 0; traffic_sink_marker.action = traffic_sink_marker.ADD; traffic_sink_marker.type = 3; // cylinder traffic_sink_marker.pose = - geometry_msgs::build().position(position).orientation( - geometry_msgs::build().x(0).y(0).z(0).w(1)); + geometry_msgs::build() + .position(config.position) + .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); traffic_sink_marker.color = color_names::makeColorMsg("firebrick", 0.99); - traffic_sink_marker.scale.x = radius * 2; - traffic_sink_marker.scale.y = radius * 2; + traffic_sink_marker.scale.x = config.radius * 2.0; + traffic_sink_marker.scale.y = config.radius * 2.0; traffic_sink_marker.scale.z = 1.0; marker_array.markers.emplace_back(traffic_sink_marker); @@ -100,7 +87,7 @@ auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marke text_marker = traffic_sink_marker; text_marker.id = 1; text_marker.type = 9; //text - text_marker.text = description; + text_marker.text = config.description; text_marker.color = color_names::makeColorMsg("white", 0.99); text_marker.scale.z = 0.6; marker_array.markers.emplace_back(text_marker); @@ -131,12 +118,13 @@ auto TrafficSink::getEntityPose(const std::string & entity_name) const noexcept( auto TrafficSink::despawn(const std::string & entity_name) const -> void { const auto entity_position = getEntityPose(entity_name).position; - const bool in_despawn_proximity = math::geometry::hypot(entity_position, position) <= radius; + const bool in_despawn_proximity = + math::geometry::hypot(entity_position, config.position) <= config.radius; const std::uint8_t entity_type = getEntityType(entity_name).type; const bool is_despawn_candidate = - sinkable_entity_type.empty() or - sinkable_entity_type.find(entity_type) != sinkable_entity_type.cend(); + config.sinkable_entity_type.empty() or + config.sinkable_entity_type.find(entity_type) != config.sinkable_entity_type.cend(); if (is_despawn_candidate and in_despawn_proximity) { entity_manager_ptr->despawnEntity(entity_name); } From 2d56a8b3a0f1136e0778095732f422057a574fc5 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 9 Dec 2024 18:54:12 +0100 Subject: [PATCH 078/149] simplify auto_sink logic --- .../cpp_mock_scenarios/cpp_scenario_node.hpp | 8 ++--- .../src/cpp_scenario_node.cpp | 5 +-- .../src/traffic_sink/auto_sink_vehicle.cpp | 7 ++-- .../src/openscenario_interpreter.cpp | 1 - .../include/traffic_simulator/api/api.hpp | 4 +-- .../traffic_simulator/api/configuration.hpp | 4 +-- .../traffic/traffic_controller.hpp | 13 ++----- .../traffic/traffic_sink.hpp | 21 ++++++----- .../src/traffic/traffic_controller.cpp | 14 ++++---- .../src/traffic/traffic_sink.cpp | 35 +++++-------------- 10 files changed, 39 insertions(+), 73 deletions(-) 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 70bea75da81..45c2aa6962e 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 @@ -34,7 +34,7 @@ class CppScenarioNode : public rclcpp::Node const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, const rclcpp::NodeOptions & option, - const traffic_simulator::traffic::AutoSinkConfig & auto_sink_config = {false, {}}); + const std::set & auto_sink_entity_types = {}); void start(); void stop(Result result, const std::string & description = ""); void expectThrow() { exception_expect_ = true; } @@ -73,8 +73,7 @@ class CppScenarioNode : public rclcpp::Node auto configure( const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, - const traffic_simulator::traffic::AutoSinkConfig & auto_sink_config = {false, {}}) - -> traffic_simulator::Configuration + const std::set & auto_sink_entity_types = {}) -> traffic_simulator::Configuration { auto configuration = traffic_simulator::Configuration(map_path); { @@ -82,8 +81,7 @@ class CppScenarioNode : public rclcpp::Node // configuration.lanelet2_map_file = "lanelet2_map_with_private_road_and_walkway_ele_fix.osm"; configuration.scenario_path = scenario_filename; configuration.verbose = verbose; - configuration.generate_auto_sink = auto_sink_config.generate_auto_sink; - configuration.default_sinkable_entity_type = auto_sink_config.default_sinkable_entity_type; + configuration.auto_sink_entity_types = auto_sink_entity_types; } checkConfiguration(configuration); return configuration; diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index ecabc7967ed..362203733c9 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -21,10 +21,11 @@ CppScenarioNode::CppScenarioNode( const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, const rclcpp::NodeOptions & option, - const traffic_simulator::traffic::AutoSinkConfig & auto_sink_config /*= {false, {}}*/) + const std::set & auto_sink_entity_types /*= {}*/) : Node(node_name, option), api_( - this, configure(map_path, lanelet2_map_file, scenario_filename, verbose, auto_sink_config), + this, + configure(map_path, lanelet2_map_file, scenario_filename, verbose, auto_sink_entity_types), declare_parameter("global_real_time_factor", 1.0), declare_parameter("global_frame_rate", 20.0)), scenario_filename_(scenario_filename), diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index ae39e75c7c3..951684a587d 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -31,8 +31,7 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode explicit AutoSinkVehicleScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "auto_sink_vehicle", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option, - traffic_simulator::traffic::AutoSinkConfig{true, {traffic_simulator::EntityType::PEDESTRIAN}}) + "lanelet2_map.osm", __FILE__, false, option, {traffic_simulator::EntityType::PEDESTRIAN}) { start(); } @@ -41,7 +40,7 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { if (api_.getCurrentTime() >= 0.1) { - if (api_.entityExists("ego") and not api_.entityExists("bob")) { + if (api_.entityExists("car") and not api_.entityExists("bob")) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); @@ -55,7 +54,7 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34774, 11.0, 0.0, api_.getHdmapUtils()); api_.spawn( - "ego", traffic_simulator::pose::toMapPose(canonicalized_lanelet_pose), + "car", traffic_simulator::pose::toMapPose(canonicalized_lanelet_pose), getVehicleParameters()); api_.spawn( "bob", traffic_simulator::pose::toMapPose(canonicalized_lanelet_pose), diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index c1597f52251..c82d04b4601 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -70,7 +70,6 @@ auto Interpreter::makeCurrentConfiguration() const -> traffic_simulator::Configu auto configuration = traffic_simulator::Configuration( logic_file.isDirectory() ? logic_file : logic_file.filepath.parent_path()); { - configuration.generate_auto_sink = false; configuration.scenario_path = osc_path; // XXX DIRTY HACK!!! diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 300b9589e78..7001b325a8e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -76,9 +76,7 @@ class API node, entity_manager_ptr_->getHdmapUtils(), getROS2Parameter("architecture_type", "awf/universe"))), traffic_controller_ptr_(std::make_shared( - entity_manager_ptr_, - traffic_simulator::traffic::AutoSinkConfig{ - configuration.generate_auto_sink, configuration.default_sinkable_entity_type})), + entity_manager_ptr_, configuration.auto_sink_entity_types)), clock_pub_(rclcpp::create_publisher( node, "/clock", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), rclcpp::PublisherOptionsWithAllocator())), diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index cfd33d0f306..ea84ee189f8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -33,9 +33,7 @@ struct Configuration using Pathname = boost::filesystem::path; - bool generate_auto_sink = true; - - std::set default_sinkable_entity_type = {}; + std::set auto_sink_entity_types = {}; bool verbose = false; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 323073114bb..02de243fdb8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -39,18 +39,12 @@ namespace traffic_simulator { namespace traffic { -struct AutoSinkConfig -{ - const bool generate_auto_sink = true; - const std::set default_sinkable_entity_type = {}; - static constexpr double radius = 1.0; -}; class TrafficController { public: explicit TrafficController( const std::shared_ptr entity_manager_ptr, - const AutoSinkConfig & auto_sink_config = {false, {}}); + const std::set auto_sink_entity_types /*= {}*/); template void addModule(Ts &&... xs) @@ -62,12 +56,9 @@ class TrafficController auto makeDebugMarker() const -> const visualization_msgs::msg::MarkerArray; private: - void autoSink(); + void generateAutoSinks(const std::set & auto_sink_entity_types); const std::shared_ptr entity_manager_ptr; std::vector> modules_; - -public: - const AutoSinkConfig auto_sink_config; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 3d80a8bc042..5135b19497a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -46,16 +46,16 @@ struct TrafficSinkConfig * @brief Construct a new TrafficSinkConfig object * @param radius Radius of the traffic sink * @param position Position of the traffic sink - * @param sinkable_entity_type Candidates for despawn. If empty, all entities are sinkable + * @param sinkable_entity_types Candidates for despawn. */ explicit TrafficSinkConfig( const double radius, const geometry_msgs::msg::Point & position, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_types, const std::optional lanelet_id_opt) : radius(radius), position(position), - sinkable_entity_type(sinkable_entity_type), - description([](std::optional lanelet_id_opt) -> std::string { + sinkable_entity_types(sinkable_entity_types), + description([](const std::optional lanelet_id_opt) -> std::string { static long unique_id = 0L; if (lanelet_id_opt.has_value()) { return std::string("auto_") + std::to_string(lanelet_id_opt.value()); @@ -68,7 +68,7 @@ struct TrafficSinkConfig const double radius; const geometry_msgs::msg::Point position; - const std::set sinkable_entity_type; + const std::set sinkable_entity_types; const std::string description; }; @@ -83,6 +83,11 @@ class TrafficSink : public TrafficModuleBase explicit TrafficSink( const std::shared_ptr entity_manager_ptr, const TrafficSinkConfig & config); + /** + * @note execute calls despawn on each entity only when both: + * 1. Its distance from the TrafficSink is <= config.radius [m]. + * 2. Its EntityType is in config.sinkable_entity_types. + */ void execute(const double current_time, const double step_time) override; auto appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void override; @@ -94,12 +99,6 @@ class TrafficSink : public TrafficModuleBase auto getEntityType(const std::string & entity_name) const noexcept(false) -> EntityType; auto getEntityPose(const std::string & entity_name) const noexcept(false) -> geometry_msgs::msg::Pose; - /** - * @note Despawn the entity only when both: - * 1. Its distance from the TrafficSink is <= radius [m]. - * 2. Its EntityType is in sinkable_entity_type or sinkable_entity_type is empty. - */ - auto despawn(const std::string & entity_name) const -> void; const std::shared_ptr entity_manager_ptr; }; diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 28fa9b214f6..f069f4bd9a5 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -40,15 +40,15 @@ namespace traffic { TrafficController::TrafficController( const std::shared_ptr entity_manager_ptr, - const AutoSinkConfig & auto_sink_config /* = {false, {}}*/) -: entity_manager_ptr(entity_manager_ptr), modules_(), auto_sink_config(auto_sink_config) + const std::set auto_sink_entity_types) +: entity_manager_ptr(entity_manager_ptr), modules_() { - if (auto_sink_config.generate_auto_sink) { - autoSink(); + if (not auto_sink_entity_types.empty()) { + generateAutoSinks(auto_sink_entity_types); } } -void TrafficController::autoSink() +void TrafficController::generateAutoSinks(const std::set & auto_sink_entity_types) { const auto hdmap_utils_ptr = entity_manager_ptr->getHdmapUtils(); for (const auto & lanelet_id : hdmap_utils_ptr->getLaneletIds()) { @@ -57,9 +57,9 @@ void TrafficController::autoSink() lanelet_pose.lanelet_id = lanelet_id; lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_ptr); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_ptr); + static constexpr double sink_radius = 1.0; const auto traffic_sink_config = TrafficSinkConfig( - auto_sink_config.radius, pose.position, auto_sink_config.default_sinkable_entity_type, - std::make_optional(lanelet_id)); + sink_radius, pose.position, auto_sink_entity_types, std::make_optional(lanelet_id)); addModule(entity_manager_ptr, traffic_sink_config); } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index c50d14680ee..24494e5b0bf 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -48,18 +48,15 @@ TrafficSink::TrafficSink( void TrafficSink::execute( [[maybe_unused]] const double current_time, [[maybe_unused]] const double step_time) { - const auto names = getEntityNames(); - for (const auto & name : names) { - const auto is_sinkable_entity = [this](const auto & entity_name) { - return config.sinkable_entity_type.empty() or - config.sinkable_entity_type.find(getEntityType(entity_name).type) != - config.sinkable_entity_type.end(); - }; - const auto pose = getEntityPose(name); - if ( - is_sinkable_entity(name) and - math::geometry::getDistance(config.position, pose) <= config.radius) { - despawn(name); + const auto entity_names = getEntityNames(); + for (const auto & entity_name : entity_names) { + const bool is_in_sinkable_radius = + math::geometry::getDistance(config.position, getEntityPose(entity_name)) <= config.radius; + const bool has_sinkable_entity_type = + config.sinkable_entity_types.find(getEntityType(entity_name).type) != + config.sinkable_entity_types.end(); + if (has_sinkable_entity_type and is_in_sinkable_radius) { + entity_manager_ptr->despawnEntity(entity_name); } } } @@ -115,19 +112,5 @@ auto TrafficSink::getEntityPose(const std::string & entity_name) const noexcept( THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); } } -auto TrafficSink::despawn(const std::string & entity_name) const -> void -{ - const auto entity_position = getEntityPose(entity_name).position; - const bool in_despawn_proximity = - math::geometry::hypot(entity_position, config.position) <= config.radius; - - const std::uint8_t entity_type = getEntityType(entity_name).type; - const bool is_despawn_candidate = - config.sinkable_entity_type.empty() or - config.sinkable_entity_type.find(entity_type) != config.sinkable_entity_type.cend(); - if (is_despawn_candidate and in_despawn_proximity) { - entity_manager_ptr->despawnEntity(entity_name); - } -} } // namespace traffic } // namespace traffic_simulator From c2d4f1e324e32dd1f63e2af9f3d05fdc05808848 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 9 Dec 2024 19:13:38 +0100 Subject: [PATCH 079/149] sink pedestrian test fix --- mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index 74dbf25598b..4b94b5ee5d2 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -30,7 +30,7 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode explicit StopAtCrosswalkScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "stop_at_crosswalk", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option) + "lanelet2_map.osm", __FILE__, false, option, {traffic_simulator::EntityType::PEDESTRIAN}) { start(); } From fbd15d4c65da769a31a701bb284b82b9ee87acee Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Mon, 9 Dec 2024 19:03:00 +0100 Subject: [PATCH 080/149] [RJD-1370] Fix 3D Lanelet Matching Issue in cpp_mock_scenario - Updated the makeRandomPose method to correctly support 3D lanelet matching. --- .../hdmap_utils/hdmap_utils.hpp | 4 ++++ .../traffic/traffic_source.hpp | 3 ++- .../src/hdmap_utils/hdmap_utils.cpp | 12 ++++++++++++ .../src/traffic/traffic_source.cpp | 17 +++++++++++++++-- 4 files changed, 33 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 6e5bec89712..e8797a4f3ea 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -383,6 +383,10 @@ class HdMapUtils auto isAltitudeMatching(const double current_altitude, const double target_altitude) const -> bool; + auto getLaneletAltitude( + const lanelet::Id & lanelet_id, const geometry_msgs::msg::Pose & pose, + const double matching_distance = 1.0) const -> std::optional; + private: /* Using a fixed `altitude_threshold` value of 1.0 [m] is justified because the diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_source.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_source.hpp index cb2b70636cd..c9daa49273e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_source.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_source.hpp @@ -117,7 +117,8 @@ class TrafficSource : public TrafficModuleBase const std::size_t id; private: - auto makeRandomPose(const bool random_orientation = false) -> geometry_msgs::msg::Pose; + auto makeRandomPose(const bool random_orientation, const VehicleOrPedestrianParameter & parameter) + -> geometry_msgs::msg::Pose; auto isPoseValid(const VehicleOrPedestrianParameter &, const geometry_msgs::msg::Pose &) -> std::pair>; diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 55a71850e49..8b0909ecba6 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -2175,6 +2175,18 @@ auto HdMapUtils::toPolygon(const lanelet::ConstLineString3d & line_string) const return ret; } +auto HdMapUtils::getLaneletAltitude( + const lanelet::Id & lanelet_id, const geometry_msgs::msg::Pose & pose, + const double matching_distance) const -> std::optional +{ + if (const auto spline = getCenterPointsSpline(lanelet_id)) { + if (const auto s = spline->getSValue(pose, matching_distance)) { + return spline->getPoint(s.value()).z; + } + } + return std::nullopt; +} + HdMapUtils::RoutingGraphs::RoutingGraphs(const lanelet::LaneletMapPtr & lanelet_map) { vehicle.rules = lanelet::traffic_rules::TrafficRulesFactory::create( diff --git a/simulation/traffic_simulator/src/traffic/traffic_source.cpp b/simulation/traffic_simulator/src/traffic/traffic_source.cpp index 350920a42d1..68f82fdce61 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_source.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_source.cpp @@ -72,7 +72,9 @@ auto TrafficSource::Validator::operator()( }); } -auto TrafficSource::makeRandomPose(const bool random_orientation) -> geometry_msgs::msg::Pose +auto TrafficSource::makeRandomPose( + const bool random_orientation, const VehicleOrPedestrianParameter & parameter) + -> geometry_msgs::msg::Pose { const double angle = angle_distribution_(engine_); @@ -83,6 +85,17 @@ auto TrafficSource::makeRandomPose(const bool random_orientation) -> geometry_ms random_pose.position.x += radius * std::cos(angle); random_pose.position.y += radius * std::sin(angle); + if (const auto nearby_lanelets = hdmap_utils_->getNearbyLaneletIds( + random_pose.position, radius, std::holds_alternative(parameter)); + !nearby_lanelets.empty()) { + // Get the altitude of the first nearby lanelet + if ( + const auto altitude = + hdmap_utils_->getLaneletAltitude(nearby_lanelets.front(), random_pose, radius)) { + random_pose.position.z = altitude.value(); + } + } + if (random_orientation) { random_pose.orientation = math::geometry::convertEulerAngleToQuaternion( traffic_simulator::helper::constructRPY(0.0, 0.0, angle_distribution_(engine_))); @@ -106,7 +119,7 @@ void TrafficSource::execute( static constexpr auto max_randomization_attempts = 10000; for (auto tries = 0; tries < max_randomization_attempts; ++tries) { - auto candidate_pose = makeRandomPose(configuration_.use_random_orientation); + auto candidate_pose = makeRandomPose(configuration_.use_random_orientation, parameter); if (auto [valid, lanelet_pose] = isPoseValid(parameter, candidate_pose); valid) { return std::make_pair(candidate_pose, lanelet_pose); } From ab76fb074f6cc1028b8172df8acc6e5a5595b75a Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 10 Dec 2024 13:46:24 +0900 Subject: [PATCH 081/149] Update document `OpenSCENARIOSupport.md` Signed-off-by: yamacir-kit --- docs/developer_guide/OpenSCENARIOSupport.md | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/docs/developer_guide/OpenSCENARIOSupport.md b/docs/developer_guide/OpenSCENARIOSupport.md index 2f64a14c51c..d8821dd97da 100644 --- a/docs/developer_guide/OpenSCENARIOSupport.md +++ b/docs/developer_guide/OpenSCENARIOSupport.md @@ -486,8 +486,8 @@ Currently, the only way to know the result of the simulation is by viewing the s | TimeOfDay | 1.3 | | | TimeOfDayCondition | unimplemented | | | TimeReference | 1.3 | | -| TimeToCollisionCondition | unimplemented | | -| TimeToCollisionConditionTarget | unimplemented | | +| TimeToCollisionCondition | 1.3.1 (partial) | [detail](#TimeToCollisionCondition) | +| TimeToCollisionConditionTarget | 1.3.1 | | | Timing | 1.3 | | | TrafficAction | unimplemented | | | TrafficArea | unimplemented | | @@ -848,6 +848,13 @@ Currently, the only way to know the result of the simulation is by viewing the s - Property `freespace` is ignored. - The simulator behaves as if `freespace` is `false`. +#### TimeToCollisionCondition + +- Since `TimeToCollisionCondition` is implemented using `DistanceCondition`, + `RelativeDistanceCondition`, `SpeedCondition`, and `RelativeSpeedCondition`, + if a combination of properties that is not supported by those Conditions is + given to `TimeToCollisionCondition`, an error will be thrown. + #### TransitionDynamics - Property `followingMode` created in version 1.2 is ignored. From 32d7c23964102b589407dacb60eec001e07e32f6 Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Tue, 10 Dec 2024 20:03:52 +0900 Subject: [PATCH 082/149] Fixed bugs and added comments. --- .../math/geometry/src/spline/catmull_rom_spline.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/common/math/geometry/src/spline/catmull_rom_spline.cpp b/common/math/geometry/src/spline/catmull_rom_spline.cpp index 434eefb7a12..e91bff28596 100644 --- a/common/math/geometry/src/spline/catmull_rom_spline.cpp +++ b/common/math/geometry/src/spline/catmull_rom_spline.cpp @@ -634,8 +634,16 @@ auto CatmullRomSpline::getTangentVector(const double s) const -> geometry_msgs:: "This message is not originally intended to be displayed, if you see it, please " "contact the developer of traffic_simulator."); default: - const auto [index, s_value] = getCurveIndexAndS(s); - return curves_[index].getNormalVector(s_value, true); + /** + * @note The current implementation uses `index_and_s` instead of structured binding + * (`const auto [index, s_value] = getCurveIndexAndS(s)`) because some tests fail + * when using structured binding. The root cause of these test failures is under investigation. + */ + // const auto [index, s_value] = getCurveIndexAndS(s); + // return curves_[index].getTangentVector(s_value, true); + + const auto index_and_s = getCurveIndexAndS(s); + return curves_[index_and_s.first].getTangentVector(index_and_s.second, true); } } From ce85613d3f17af93294158fc9de3e93449235bcd Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 10 Dec 2024 12:25:39 +0100 Subject: [PATCH 083/149] code style --- .../src/traffic_sink/auto_sink_vehicle.cpp | 12 ++++------- .../traffic/traffic_controller.hpp | 8 +++---- .../src/traffic/traffic_controller.cpp | 21 ++++++++++--------- 3 files changed, 19 insertions(+), 22 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index 951684a587d..8d3a89c2ae7 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -50,15 +50,11 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { - const auto canonicalized_lanelet_pose = + const auto map_pose = traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34774, 11.0, 0.0, api_.getHdmapUtils()); - api_.spawn( - "car", traffic_simulator::pose::toMapPose(canonicalized_lanelet_pose), - getVehicleParameters()); - api_.spawn( - "bob", traffic_simulator::pose::toMapPose(canonicalized_lanelet_pose), - getPedestrianParameters()); + 34774, 11.0, 0.0, api_.getHdmapUtils())); + api_.spawn("car", map_pose, getVehicleParameters()); + api_.spawn("bob", map_pose, getPedestrianParameters()); } }; } // namespace cpp_mock_scenarios diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 02de243fdb8..3ed3561f75a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -52,12 +52,12 @@ class TrafficController auto module_ptr = std::make_shared(std::forward(xs)...); modules_.emplace_back(module_ptr); } - void execute(const double current_time, const double step_time); - auto makeDebugMarker() const -> const visualization_msgs::msg::MarkerArray; + auto execute(const double current_time, const double step_time) -> void; + auto makeDebugMarker() const -> visualization_msgs::msg::MarkerArray; private: - void generateAutoSinks(const std::set & auto_sink_entity_types); - const std::shared_ptr entity_manager_ptr; + auto appendAutoSinks(const std::set & auto_sink_entity_types) -> void; + const std::shared_ptr entity_manager_ptr_; std::vector> modules_; }; } // namespace traffic diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index f069f4bd9a5..042cca131d9 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -41,42 +41,43 @@ namespace traffic TrafficController::TrafficController( const std::shared_ptr entity_manager_ptr, const std::set auto_sink_entity_types) -: entity_manager_ptr(entity_manager_ptr), modules_() +: entity_manager_ptr_(entity_manager_ptr), modules_() { if (not auto_sink_entity_types.empty()) { - generateAutoSinks(auto_sink_entity_types); + appendAutoSinks(auto_sink_entity_types); } } -void TrafficController::generateAutoSinks(const std::set & auto_sink_entity_types) +auto TrafficController::appendAutoSinks(const std::set & auto_sink_entity_types) + -> void { - const auto hdmap_utils_ptr = entity_manager_ptr->getHdmapUtils(); + static constexpr double sink_radius = 1.0; + const auto hdmap_utils_ptr = entity_manager_ptr_->getHdmapUtils(); for (const auto & lanelet_id : hdmap_utils_ptr->getLaneletIds()) { if (hdmap_utils_ptr->getNextLaneletIds(lanelet_id).empty()) { LaneletPose lanelet_pose; lanelet_pose.lanelet_id = lanelet_id; lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_ptr); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_ptr); - static constexpr double sink_radius = 1.0; const auto traffic_sink_config = TrafficSinkConfig( sink_radius, pose.position, auto_sink_entity_types, std::make_optional(lanelet_id)); - addModule(entity_manager_ptr, traffic_sink_config); + addModule(entity_manager_ptr_, traffic_sink_config); } } } -void TrafficController::execute(const double current_time, const double step_time) +auto TrafficController::execute(const double current_time, const double step_time) -> void { for (const auto & module : modules_) { module->execute(current_time, step_time); } } -auto TrafficController::makeDebugMarker() const -> const visualization_msgs::msg::MarkerArray +auto TrafficController::makeDebugMarker() const -> visualization_msgs::msg::MarkerArray { - static const auto marker_array = [&]() { + static const auto marker_array = [this]() { visualization_msgs::msg::MarkerArray marker_array; - for (size_t i = 0UL; i < modules_.size(); ++i) { + for (std::size_t i = 0UL; i < modules_.size(); ++i) { modules_[i]->appendDebugMarker(marker_array); } return marker_array; From d64210b4171344b9b1c8edbcfe162b4761c6464c Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Tue, 10 Dec 2024 12:30:47 +0100 Subject: [PATCH 084/149] Remove comment --- .../include/geometry/quaternion/get_angle_difference.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp b/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp index 298a9e2d481..8c8ff46265f 100644 --- a/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp +++ b/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp @@ -15,7 +15,7 @@ #ifndef GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_ #define GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_ -#include // Załaduj biblioteki Eigen +#include #include namespace math From 562b1ad691c32f07f4f3b9521f9624ac483c3bf7 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 10 Dec 2024 12:32:42 +0100 Subject: [PATCH 085/149] code style --- .../include/traffic_simulator/traffic/traffic_sink.hpp | 2 +- .../traffic_simulator/src/traffic/traffic_sink.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 5135b19497a..55c70576c6d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -100,7 +100,7 @@ class TrafficSink : public TrafficModuleBase auto getEntityPose(const std::string & entity_name) const noexcept(false) -> geometry_msgs::msg::Pose; - const std::shared_ptr entity_manager_ptr; + const std::shared_ptr entity_manager_ptr_; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 24494e5b0bf..33323b7aa16 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -41,7 +41,7 @@ namespace traffic { TrafficSink::TrafficSink( const std::shared_ptr entity_manager_ptr, const TrafficSinkConfig & config) -: TrafficModuleBase(), config(config), entity_manager_ptr(entity_manager_ptr) +: TrafficModuleBase(), config(config), entity_manager_ptr_(entity_manager_ptr) { } @@ -56,7 +56,7 @@ void TrafficSink::execute( config.sinkable_entity_types.find(getEntityType(entity_name).type) != config.sinkable_entity_types.end(); if (has_sinkable_entity_type and is_in_sinkable_radius) { - entity_manager_ptr->despawnEntity(entity_name); + entity_manager_ptr_->despawnEntity(entity_name); } } } @@ -92,12 +92,12 @@ auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marke auto TrafficSink::getEntityNames() const -> std::vector { - return entity_manager_ptr->getEntityNames(); + return entity_manager_ptr_->getEntityNames(); } auto TrafficSink::getEntityType(const std::string & entity_name) const noexcept(false) -> EntityType { - if (const auto entity = entity_manager_ptr->getEntity(entity_name)) { + if (const auto entity = entity_manager_ptr_->getEntity(entity_name)) { return entity->getEntityType(); } else { THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); @@ -106,7 +106,7 @@ auto TrafficSink::getEntityType(const std::string & entity_name) const noexcept( auto TrafficSink::getEntityPose(const std::string & entity_name) const noexcept(false) -> geometry_msgs::msg::Pose { - if (const auto entity = entity_manager_ptr->getEntity(entity_name)) { + if (const auto entity = entity_manager_ptr_->getEntity(entity_name)) { return entity->getMapPose(); } else { THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); From 0cca11022dfe97a10cfc1c56f37d1fcd73ed610f Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 10 Dec 2024 14:54:28 +0100 Subject: [PATCH 086/149] remove unnecessary funcs --- .../include/traffic_simulator/api/api.hpp | 1 - .../traffic/traffic_module_base.hpp | 2 +- .../traffic/traffic_sink.hpp | 10 ++-- .../src/traffic/traffic_sink.cpp | 54 ++++++++----------- 4 files changed, 25 insertions(+), 42 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index d485964cb68..be13d11637e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include #include diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_module_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_module_base.hpp index 41e52aaf0e4..b660cb0a281 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_module_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_module_base.hpp @@ -36,7 +36,7 @@ class TrafficModuleBase { public: TrafficModuleBase() {} - virtual void execute(const double current_time, const double step_time) = 0; + virtual auto execute(const double current_time, const double step_time) -> void = 0; virtual auto appendDebugMarker(visualization_msgs::msg::MarkerArray &) const -> void{}; }; } // namespace traffic diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 55c70576c6d..e3a99f23a86 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -88,18 +88,14 @@ class TrafficSink : public TrafficModuleBase * 1. Its distance from the TrafficSink is <= config.radius [m]. * 2. Its EntityType is in config.sinkable_entity_types. */ - void execute(const double current_time, const double step_time) override; + auto execute(const double current_time, const double step_time) -> void override; auto appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void override; - const TrafficSinkConfig config; - private: - auto getEntityNames() const -> std::vector; - auto getEntityType(const std::string & entity_name) const noexcept(false) -> EntityType; - auto getEntityPose(const std::string & entity_name) const noexcept(false) - -> geometry_msgs::msg::Pose; + auto isEntitySinkable(const std::string & entity_name) const noexcept(false) -> bool; + const TrafficSinkConfig config_; const std::shared_ptr entity_manager_ptr_; }; } // namespace traffic diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 33323b7aa16..1c8238f1b7b 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -41,21 +41,16 @@ namespace traffic { TrafficSink::TrafficSink( const std::shared_ptr entity_manager_ptr, const TrafficSinkConfig & config) -: TrafficModuleBase(), config(config), entity_manager_ptr_(entity_manager_ptr) +: TrafficModuleBase(), config_(config), entity_manager_ptr_(entity_manager_ptr) { } -void TrafficSink::execute( - [[maybe_unused]] const double current_time, [[maybe_unused]] const double step_time) +auto TrafficSink::execute( + [[maybe_unused]] const double current_time, [[maybe_unused]] const double step_time) -> void { - const auto entity_names = getEntityNames(); + const auto entity_names = entity_manager_ptr_->getEntityNames(); for (const auto & entity_name : entity_names) { - const bool is_in_sinkable_radius = - math::geometry::getDistance(config.position, getEntityPose(entity_name)) <= config.radius; - const bool has_sinkable_entity_type = - config.sinkable_entity_types.find(getEntityType(entity_name).type) != - config.sinkable_entity_types.end(); - if (has_sinkable_entity_type and is_in_sinkable_radius) { + if (isEntitySinkable(entity_name)) { entity_manager_ptr_->despawnEntity(entity_name); } } @@ -66,51 +61,44 @@ auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marke { visualization_msgs::msg::Marker traffic_sink_marker; traffic_sink_marker.header.frame_id = "map"; - traffic_sink_marker.ns = "traffic_controller/traffic_sink/" + config.description; + traffic_sink_marker.ns = "traffic_controller/traffic_sink/" + config_.description; traffic_sink_marker.id = 0; traffic_sink_marker.action = traffic_sink_marker.ADD; traffic_sink_marker.type = 3; // cylinder traffic_sink_marker.pose = geometry_msgs::build() - .position(config.position) + .position(config_.position) .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); traffic_sink_marker.color = color_names::makeColorMsg("firebrick", 0.99); - traffic_sink_marker.scale.x = config.radius * 2.0; - traffic_sink_marker.scale.y = config.radius * 2.0; + traffic_sink_marker.scale.x = config_.radius * 2.0; + traffic_sink_marker.scale.y = config_.radius * 2.0; traffic_sink_marker.scale.z = 1.0; marker_array.markers.emplace_back(traffic_sink_marker); visualization_msgs::msg::Marker text_marker; text_marker = traffic_sink_marker; text_marker.id = 1; - text_marker.type = 9; //text - text_marker.text = config.description; + text_marker.type = 9; // text + text_marker.text = config_.description; text_marker.color = color_names::makeColorMsg("white", 0.99); text_marker.scale.z = 0.6; marker_array.markers.emplace_back(text_marker); } -auto TrafficSink::getEntityNames() const -> std::vector +auto TrafficSink::isEntitySinkable(const std::string & entity_name) const noexcept(false) -> bool { - return entity_manager_ptr_->getEntityNames(); -} - -auto TrafficSink::getEntityType(const std::string & entity_name) const noexcept(false) -> EntityType -{ - if (const auto entity = entity_manager_ptr_->getEntity(entity_name)) { - return entity->getEntityType(); - } else { + if (const auto entity = entity_manager_ptr_->getEntity(entity_name); entity == nullptr) { THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); - } -} -auto TrafficSink::getEntityPose(const std::string & entity_name) const noexcept(false) - -> geometry_msgs::msg::Pose -{ - if (const auto entity = entity_manager_ptr_->getEntity(entity_name)) { - return entity->getMapPose(); + } else if ( + config_.sinkable_entity_types.find(entity->getEntityType().type) == + config_.sinkable_entity_types.end()) { + return false; + } else if (math::geometry::getDistance(config_.position, entity->getMapPose()) > config_.radius) { + return false; } else { - THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); + return true; } } + } // namespace traffic } // namespace traffic_simulator From d0403a4440fad854e750ddb784b5610952c53292 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 10 Dec 2024 15:05:14 +0100 Subject: [PATCH 087/149] remove unnecessary include directives --- .../include/traffic_simulator/api/configuration.hpp | 1 - simulation/traffic_simulator/src/data_type/entity_status.cpp | 1 - simulation/traffic_simulator/src/traffic/traffic_controller.cpp | 2 -- 3 files changed, 4 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index ea84ee189f8..22007c7c6bc 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -23,7 +23,6 @@ #include #include #include -#include namespace traffic_simulator { diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index 6420e77aec3..b2bfb78432e 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -17,7 +17,6 @@ namespace traffic_simulator { - inline namespace entity_status { diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 042cca131d9..5e764e933c1 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -25,11 +25,9 @@ #include #include -#include #include #include #include -#include #include #include #include From 8e954f489857b95e8f9c5ab92f73e6a319c5a0bd Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Tue, 10 Dec 2024 15:43:58 +0100 Subject: [PATCH 088/149] Refactor code to improve readability based on SonarQube findings --- common/math/geometry/include/geometry/plane.hpp | 2 +- common/math/geometry/src/plane.cpp | 2 +- .../include/behavior_tree_plugin/action_node.hpp | 2 +- simulation/behavior_tree_plugin/src/action_node.cpp | 4 ++-- .../detection_sensor/detection_sensor.hpp | 13 ++++--------- .../detection_sensor/detection_sensor.cpp | 4 ++-- 6 files changed, 11 insertions(+), 16 deletions(-) diff --git a/common/math/geometry/include/geometry/plane.hpp b/common/math/geometry/include/geometry/plane.hpp index a718c319bd7..af442606a14 100644 --- a/common/math/geometry/include/geometry/plane.hpp +++ b/common/math/geometry/include/geometry/plane.hpp @@ -30,7 +30,7 @@ struct Plane double d_; Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal); - auto calculateOffset(const geometry_msgs::msg::Point & point) -> double; + auto calculateOffset(const geometry_msgs::msg::Point & point) const -> double; }; auto makePlane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal) diff --git a/common/math/geometry/src/plane.cpp b/common/math/geometry/src/plane.cpp index ea4805b2bc0..16eb761f2e7 100644 --- a/common/math/geometry/src/plane.cpp +++ b/common/math/geometry/src/plane.cpp @@ -25,7 +25,7 @@ Plane::Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg:: { } -auto Plane::calculateOffset(const geometry_msgs::msg::Point & point) -> double +auto Plane::calculateOffset(const geometry_msgs::msg::Point & point) const -> double { return normal_.x * point.x + normal_.y * point.y + normal_.z * point.z + d_; } 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 0b2c7350e5d..2ebc7b9446c 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 @@ -140,7 +140,7 @@ class ActionNode : public BT::ActionNodeBase auto getConflictingEntityStatusOnLane(const lanelet::Ids & route_lanelets) const -> std::vector; auto isEntityAltitudeCompatible( - const traffic_simulator::CanonicalizedEntityStatus & other_entity_status) const -> bool; + const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool; }; } // namespace entity_behavior diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 82f77d43358..49fd3ff33dc 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -327,11 +327,11 @@ auto ActionNode::getDistanceToTargetEntityPolygon( } auto ActionNode::isEntityAltitudeCompatible( - const traffic_simulator::CanonicalizedEntityStatus & other_entity_status) const -> bool + const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool { return hdmap_utils->isAltitudeMatching( hdmap_utils->getAltitude(canonicalized_entity_status->getLaneletPose()), - hdmap_utils->getAltitude(other_entity_status.getLaneletPose())); + hdmap_utils->getAltitude(entity_status.getLaneletPose())); } auto ActionNode::getDistanceToConflictingEntity( diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index 685ed421bd4..177c972247b 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -40,11 +40,7 @@ class DetectionSensorBase explicit DetectionSensorBase( const double current_simulation_time, const simulation_api_schema::DetectionSensorConfiguration & configuration) - : previous_simulation_time_(current_simulation_time), - configuration_(configuration), - ego_pose_(), - previous_ego_pose_(std::nullopt), - ego_plane_(std::nullopt) + : previous_simulation_time_(current_simulation_time), configuration_(configuration) { } @@ -86,11 +82,10 @@ class DetectionSensorBase constexpr static double max_downward_z_offset_ = 1.0; geometry_msgs::msg::Pose ego_pose_; - std::optional previous_ego_pose_; - std::optional ego_plane_; + std::optional previous_ego_pose_{std::nullopt}; + std::optional ego_plane_{std::nullopt}; - auto isAltitudeDifferenceWithinThreshold(const geometry_msgs::msg::Pose & entity_pose) const - -> bool; + auto isEntityAltitudeAcceptable(const geometry_msgs::msg::Pose & entity_pose) const -> bool; auto needToUpdateEgoPlane() const -> bool; auto hasEgoOrientationChanged() const -> bool; }; diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index 85f1411fa04..8feb188553f 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -74,7 +74,7 @@ auto DetectionSensorBase::isOnOrAboveEgoPlane( simulation_interface::toMsg(ego_pose, ego_pose_); simulation_interface::toMsg(npc_pose, entity_pose); - if (isAltitudeDifferenceWithinThreshold(entity_pose)) { + if (isEntityAltitudeAcceptable(entity_pose)) { return true; } @@ -90,7 +90,7 @@ auto DetectionSensorBase::isOnOrAboveEgoPlane( return ego_plane_.value().calculateOffset(entity_pose.position) >= 0.0; } -auto DetectionSensorBase::isAltitudeDifferenceWithinThreshold( +auto DetectionSensorBase::isEntityAltitudeAcceptable( const geometry_msgs::msg::Pose & entity_pose) const -> bool { return entity_pose.position.z >= (ego_pose_.position.z - max_downward_z_offset_); From c6e983e5b62a2a827ba282bc638c325f230796db Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 10 Dec 2024 20:08:15 +0100 Subject: [PATCH 089/149] ref(traffic_simulator, simple_sensor_simulator): refactor altitude checks --- .github/workflows/custom_spell.json | 4 +- .../math/geometry/include/geometry/plane.hpp | 23 +++--- .../quaternion/get_angle_difference.hpp | 2 +- common/math/geometry/src/plane.cpp | 25 ++----- .../behavior_tree_plugin/action_node.hpp | 2 +- .../behavior_tree_plugin/src/action_node.cpp | 7 +- .../detection_sensor/detection_sensor.hpp | 32 +------- .../detection_sensor/detection_sensor.cpp | 73 ++++++++++--------- .../simulation_interface/conversions.hpp | 15 ++-- .../behavior/behavior_plugin_base.hpp | 14 ++-- .../data_type/entity_status.hpp | 2 + .../hdmap_utils/hdmap_utils.hpp | 14 ---- .../src/data_type/entity_status.cpp | 5 ++ .../src/hdmap_utils/hdmap_utils.cpp | 15 +++- 14 files changed, 104 insertions(+), 129 deletions(-) diff --git a/.github/workflows/custom_spell.json b/.github/workflows/custom_spell.json index 676f628922b..50ca6f5fe94 100644 --- a/.github/workflows/custom_spell.json +++ b/.github/workflows/custom_spell.json @@ -46,6 +46,8 @@ "Tschirnhaus", "walltime", "xerces", - "xercesc" + "xercesc", + "Szymon", + "Parapura" ] } diff --git a/common/math/geometry/include/geometry/plane.hpp b/common/math/geometry/include/geometry/plane.hpp index a718c319bd7..509e3c6e63f 100644 --- a/common/math/geometry/include/geometry/plane.hpp +++ b/common/math/geometry/include/geometry/plane.hpp @@ -15,7 +15,6 @@ #ifndef GEOMETRY__PLANE_HPP_ #define GEOMETRY__PLANE_HPP_ -#include #include #include #include @@ -24,19 +23,23 @@ namespace math { namespace geometry { + +/// @class Plane +/// @brief Represents a plane in 3D space, defined by a normal vector and a point on the plane. +/// +/// The plane is described using the equation: +/// Ax + By + Cz + D = 0 +/// where: +/// - A, B, C are the components of the normal vector (normal_ attribute). +/// - D is the offset from the origin, calculated using the point and normal vector (d_ attribute). struct Plane { - geometry_msgs::msg::Vector3 normal_; - double d_; - Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal); - auto calculateOffset(const geometry_msgs::msg::Point & point) -> double; -}; - -auto makePlane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal) - -> std::optional; + auto offset(const geometry_msgs::msg::Point & point) const -> double; + const geometry_msgs::msg::Vector3 normal_; + const double d_; +}; } // namespace geometry } // namespace math - #endif // GEOMETRY__PLANE_HPP_ diff --git a/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp b/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp index 8c8ff46265f..4e9450b2b0a 100644 --- a/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp +++ b/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp @@ -29,7 +29,7 @@ auto getAngleDifference(const T & quat1, const T & quat2) -> double const Eigen::Quaterniond q1(quat1.w, quat1.x, quat1.y, quat1.z); const Eigen::Quaterniond q2(quat2.w, quat2.x, quat2.y, quat2.z); - Eigen::AngleAxisd delta(q1.inverse() * q2); + const Eigen::AngleAxisd delta(q1.inverse() * q2); return std::abs(delta.angle()); // [rad] } diff --git a/common/math/geometry/src/plane.cpp b/common/math/geometry/src/plane.cpp index ea4805b2bc0..6439839443c 100644 --- a/common/math/geometry/src/plane.cpp +++ b/common/math/geometry/src/plane.cpp @@ -14,35 +14,26 @@ #include #include +#include +#include namespace math { namespace geometry { - Plane::Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal) : normal_(normal), d_(-(normal.x * point.x + normal.y * point.y + normal.z * point.z)) { + if (normal.x == 0.0 && normal.y == 0.0 && normal.z == 0.0) { + THROW_SIMULATION_ERROR("Plane cannot be created using zero normal vector."); + } else if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) { + THROW_SIMULATION_ERROR("Plane cannot be created using point with NaN value."); + } } -auto Plane::calculateOffset(const geometry_msgs::msg::Point & point) -> double +auto Plane::offset(const geometry_msgs::msg::Point & point) const -> double { return normal_.x * point.x + normal_.y * point.y + normal_.z * point.z + d_; } - -auto makePlane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal) - -> std::optional -{ - if (normal.x == 0.0 && normal.y == 0.0 && normal.z == 0.0) { - return std::nullopt; - } - - if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) { - return std::nullopt; - } - - return Plane(point, normal); -} - } // namespace geometry } // namespace math 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 0b2c7350e5d..5e9d6bc968d 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 @@ -139,7 +139,7 @@ class ActionNode : public BT::ActionNodeBase -> std::vector; auto getConflictingEntityStatusOnLane(const lanelet::Ids & route_lanelets) const -> std::vector; - auto isEntityAltitudeCompatible( + auto isOtherEntityAtConsideredAltitude( const traffic_simulator::CanonicalizedEntityStatus & other_entity_status) const -> bool; }; } // namespace entity_behavior diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 82f77d43358..4e4d0677f84 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -316,7 +316,7 @@ auto ActionNode::getDistanceToTargetEntityPolygon( double width_extension_left, double length_extension_front, double length_extension_rear) const -> std::optional { - if (status.laneMatchingSucceed() && isEntityAltitudeCompatible(status)) { + if (status.laneMatchingSucceed() && isOtherEntityAtConsideredAltitude(status)) { const auto polygon = math::geometry::transformPoints( status.getMapPose(), math::geometry::getPointsFromBbox( status.getBoundingBox(), width_extension_right, width_extension_left, @@ -326,12 +326,11 @@ auto ActionNode::getDistanceToTargetEntityPolygon( return std::nullopt; } -auto ActionNode::isEntityAltitudeCompatible( +auto ActionNode::isOtherEntityAtConsideredAltitude( const traffic_simulator::CanonicalizedEntityStatus & other_entity_status) const -> bool { return hdmap_utils->isAltitudeMatching( - hdmap_utils->getAltitude(canonicalized_entity_status->getLaneletPose()), - hdmap_utils->getAltitude(other_entity_status.getLaneletPose())); + canonicalized_entity_status->getAltitude(), other_entity_status.getAltitude()); } auto ActionNode::getDistanceToConflictingEntity( diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index 685ed421bd4..9a35ef719fc 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -42,9 +42,8 @@ class DetectionSensorBase const simulation_api_schema::DetectionSensorConfiguration & configuration) : previous_simulation_time_(current_simulation_time), configuration_(configuration), - ego_pose_(), - previous_ego_pose_(std::nullopt), - ego_plane_(std::nullopt) + ego_plane_opt_(std::nullopt), + previous_ego_pose_opt_(std::nullopt) { } @@ -67,31 +66,8 @@ class DetectionSensorBase const std::vector & lidar_detected_entities) = 0; private: - /* - The threshold for detecting significant changes in ego vehicle's orientation (unit: radian). - The value determines the minimum angular difference required to consider the ego orientation - as "changed". - - There is no technical basis for this value, it was determined based on experiments. - */ - constexpr static double rotation_threshold_ = 0.04; - - /* - Maximum downward offset in Z-axis relative to the ego position (unit: meter). - If the NPC is lower than this offset relative to the ego position, - the NPC will be excluded from detection - - There is no technical basis for this value, it was determined based on experiments. - */ - constexpr static double max_downward_z_offset_ = 1.0; - - geometry_msgs::msg::Pose ego_pose_; - std::optional previous_ego_pose_; - std::optional ego_plane_; - - auto isAltitudeDifferenceWithinThreshold(const geometry_msgs::msg::Pose & entity_pose) const - -> bool; - auto needToUpdateEgoPlane() const -> bool; + std::optional ego_plane_opt_; + std::optional previous_ego_pose_opt_; auto hasEgoOrientationChanged() const -> bool; }; diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index 85f1411fa04..fd85e6d6b8f 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -65,52 +65,53 @@ auto DetectionSensorBase::findEgoEntityStatusToWhichThisSensorIsAttached( } auto DetectionSensorBase::isOnOrAboveEgoPlane( - const geometry_msgs::Pose & npc_pose, const geometry_msgs::Pose & ego_pose) -> bool + const geometry_msgs::Pose & entity_pose, const geometry_msgs::Pose & ego_pose) -> bool { - using math::geometry::getNormalVector; - using math::geometry::makePlane; + /* + The threshold for detecting significant changes in ego vehicle's orientation (unit: radian). + The value determines the minimum angular difference required to consider the ego orientation + as "changed". - geometry_msgs::msg::Pose entity_pose; - simulation_interface::toMsg(ego_pose, ego_pose_); - simulation_interface::toMsg(npc_pose, entity_pose); + There is no technical basis for this value, it was determined based on experiments. + */ + constexpr static double rotation_threshold_ = 0.04; + /* + Maximum downward offset in Z-axis relative to the ego position (unit: meter). + If the NPC is lower than this offset relative to the ego position, + the NPC will be excluded from detection - if (isAltitudeDifferenceWithinThreshold(entity_pose)) { - return true; - } + There is no technical basis for this value, it was determined based on experiments. + */ + constexpr static double max_downward_z_offset_ = 1.0; - if (needToUpdateEgoPlane()) { - ego_plane_ = makePlane(ego_pose_.position, getNormalVector(ego_pose_.orientation)); - if (ego_plane_) { - previous_ego_pose_ = ego_pose_; + geometry_msgs::msg::Pose ego_pose_ros, entity_pose_ros; + simulation_interface::toMsg(entity_pose, entity_pose_ros); + simulation_interface::toMsg(ego_pose, ego_pose_ros); + + const auto hasEgoOrientationChanged = [this, &entity_pose_ros]() { + if (previous_ego_pose_opt_) { + return true; } else { - return false; + return math::geometry::getAngleDifference( + entity_pose_ros.orientation, previous_ego_pose_opt_->orientation) > + rotation_threshold_; } - } - - return ego_plane_.value().calculateOffset(entity_pose.position) >= 0.0; -} - -auto DetectionSensorBase::isAltitudeDifferenceWithinThreshold( - const geometry_msgs::msg::Pose & entity_pose) const -> bool -{ - return entity_pose.position.z >= (ego_pose_.position.z - max_downward_z_offset_); -} - -auto DetectionSensorBase::needToUpdateEgoPlane() const -> bool -{ - return !ego_plane_ || hasEgoOrientationChanged(); -} + }; -auto DetectionSensorBase::hasEgoOrientationChanged() const -> bool -{ - using math::geometry::getAngleDifference; + // update ego plane if needed + if (!ego_plane_opt_ || hasEgoOrientationChanged()) { + ego_plane_opt_.emplace( + entity_pose_ros.position, math::geometry::getNormalVector(entity_pose_ros.orientation)); + } + previous_ego_pose_opt_ = entity_pose_ros; - if (!previous_ego_pose_) { + // if other entity is just above ego return true + if (entity_pose_ros.position.z >= (ego_pose_ros.position.z - max_downward_z_offset_)) { return true; + // otherwise check if other entity is above ego plane + } else { + return ego_plane_opt_->offset(entity_pose_ros.position) >= 0.0; } - - return getAngleDifference(ego_pose_.orientation, previous_ego_pose_.value().orientation) > - rotation_threshold_; } template diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 0bd1f09e53f..6e8ee4b7ec8 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -168,9 +168,9 @@ void toMsg(const rosgraph_msgs::Clock & proto, rosgraph_msgs::msg::Clock & time) void toProto(const std_msgs::msg::Header & header, std_msgs::Header & proto); void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header); -#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ - auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \ - auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void +#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ + auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &) -> void; \ + auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &) -> void DEFINE_CONVERSION(autoware_control_msgs, Lateral); DEFINE_CONVERSION(autoware_control_msgs, Longitudinal); @@ -190,8 +190,7 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr - { + auto convert_color = [](auto color) constexpr { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -206,8 +205,7 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr - { + auto convert_shape = [](auto shape) constexpr { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -235,8 +233,7 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr - { + auto convert_status = [](auto status) constexpr { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index f4162f3e9b0..c4dac740c60 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 @@ -44,13 +44,13 @@ class BehaviorPluginBase 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) \ - virtual TYPE get##NAME() = 0; \ - virtual void set##NAME(const TYPE & value) = 0; \ - auto get##NAME##Key() const->const std::string & \ - { \ - static const std::string key = KEY; \ - return key; \ +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + virtual TYPE get##NAME() = 0; \ + virtual void set##NAME(const TYPE & value) = 0; \ + auto get##NAME##Key() const -> const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ } // clang-format off 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 68a781808a3..b1d07af341f 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 @@ -54,6 +54,8 @@ class CanonicalizedEntityStatus auto getMapPose() const noexcept -> const geometry_msgs::msg::Pose &; auto setMapPose(const geometry_msgs::msg::Pose & pose) -> void; + auto getAltitude() const -> double; + auto getTwist() const noexcept -> const geometry_msgs::msg::Twist &; auto setTwist(const geometry_msgs::msg::Twist & twist) -> void; auto setLinearVelocity(double linear_velocity) -> void; diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index c3ea1354b54..9be623a2cab 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -388,20 +388,6 @@ class HdMapUtils const double matching_distance = 1.0) const -> std::optional; private: - /* - Using a fixed `altitude_threshold` value of 1.0 [m] is justified because the - entity's Z-position is always relative to its base. This eliminates the - need to dynamically adjust the threshold based on the entity's dimensions, - ensuring consistent altitude matching regardless of the entity type. - - The position of the entity is defined relative to its base, typically - the center of the rear axle projected onto the ground in the case of vehicles. - - There is no technical basis for this value; it was determined based on - experiments. - */ - static constexpr double altitude_threshold_ = 1.0; - /** @defgroup cache * Declared mutable for caching */ diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index b2bfb78432e..758d10b6abf 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -122,6 +122,11 @@ auto CanonicalizedEntityStatus::getMapPose() const noexcept -> const geometry_ms return entity_status_.pose; } +auto CanonicalizedEntityStatus::getAltitude() const -> double +{ + return entity_status_.pose.position.z; +} + auto CanonicalizedEntityStatus::getLaneletPose() const noexcept -> const LaneletPose & { if (canonicalized_lanelet_pose_) { diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index c35415806bd..eab04aaf2f2 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -625,7 +625,20 @@ auto HdMapUtils::toLaneletPose( auto HdMapUtils::isAltitudeMatching( const double current_altitude, const double target_altitude) const -> bool { - return std::abs(current_altitude - target_altitude) <= altitude_threshold_; + /* + Using a fixed `altitude_threshold` value of 1.0 [m] is justified because the + entity's Z-position is always relative to its base. This eliminates the + need to dynamically adjust the threshold based on the entity's dimensions, + ensuring consistent altitude matching regardless of the entity type. + + The position of the entity is defined relative to its base, typically + the center of the rear axle projected onto the ground in the case of vehicles. + + There is no technical basis for this value; it was determined based on + experiments. + */ + static constexpr double altitude_threshold = 1.0; + return std::abs(current_altitude - target_altitude) <= altitude_threshold; } auto HdMapUtils::toLaneletPose( From bb0a6bf74930a7302eda787009cabae7a8efdaec Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 10 Dec 2024 20:14:34 +0100 Subject: [PATCH 090/149] fix(traffic_simulator): revert clang changes --- .../include/simulation_interface/conversions.hpp | 15 +++++++++------ .../behavior/behavior_plugin_base.hpp | 14 +++++++------- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 6e8ee4b7ec8..0bd1f09e53f 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -168,9 +168,9 @@ void toMsg(const rosgraph_msgs::Clock & proto, rosgraph_msgs::msg::Clock & time) void toProto(const std_msgs::msg::Header & header, std_msgs::Header & proto); void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header); -#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ - auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &) -> void; \ - auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &) -> void +#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ + auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \ + auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void DEFINE_CONVERSION(autoware_control_msgs, Lateral); DEFINE_CONVERSION(autoware_control_msgs, Longitudinal); @@ -190,7 +190,8 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr { + auto convert_color = [](auto color) constexpr + { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -205,7 +206,8 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr { + auto convert_shape = [](auto shape) constexpr + { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -233,7 +235,8 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr { + auto convert_status = [](auto status) constexpr + { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index c4dac740c60..f4162f3e9b0 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 @@ -44,13 +44,13 @@ class BehaviorPluginBase 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) \ - virtual TYPE get##NAME() = 0; \ - virtual void set##NAME(const TYPE & value) = 0; \ - auto get##NAME##Key() const -> const std::string & \ - { \ - static const std::string key = KEY; \ - return key; \ +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + virtual TYPE get##NAME() = 0; \ + virtual void set##NAME(const TYPE & value) = 0; \ + auto get##NAME##Key() const->const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ } // clang-format off From 8cf2c39ad0187868be85e4aa488c0c1eb99c15d2 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 10 Dec 2024 20:35:37 +0100 Subject: [PATCH 091/149] fix(simple_sensor_simulator): fix after detection_sensor refactor --- .../detection_sensor/detection_sensor.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index fd85e6d6b8f..c25fafb8719 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -88,13 +88,12 @@ auto DetectionSensorBase::isOnOrAboveEgoPlane( simulation_interface::toMsg(entity_pose, entity_pose_ros); simulation_interface::toMsg(ego_pose, ego_pose_ros); - const auto hasEgoOrientationChanged = [this, &entity_pose_ros]() { + const auto hasEgoOrientationChanged = [this, &ego_pose_ros]() { if (previous_ego_pose_opt_) { return true; } else { return math::geometry::getAngleDifference( - entity_pose_ros.orientation, previous_ego_pose_opt_->orientation) > - rotation_threshold_; + ego_pose_ros.orientation, previous_ego_pose_opt_->orientation) > rotation_threshold_; } }; @@ -103,7 +102,7 @@ auto DetectionSensorBase::isOnOrAboveEgoPlane( ego_plane_opt_.emplace( entity_pose_ros.position, math::geometry::getNormalVector(entity_pose_ros.orientation)); } - previous_ego_pose_opt_ = entity_pose_ros; + previous_ego_pose_opt_ = ego_pose_ros; // if other entity is just above ego return true if (entity_pose_ros.position.z >= (ego_pose_ros.position.z - max_downward_z_offset_)) { From 80c5c89aecbe623d989c3c846fc1c612ca7050bb Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 10 Dec 2024 20:48:27 +0100 Subject: [PATCH 092/149] fix(simple_senor_simulator): fix after Szymon discussion --- .../detection_sensor/detection_sensor.hpp | 4 +-- .../detection_sensor/detection_sensor.cpp | 34 ++++++++----------- 2 files changed, 17 insertions(+), 21 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index 9a35ef719fc..237468b0fd8 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -43,7 +43,7 @@ class DetectionSensorBase : previous_simulation_time_(current_simulation_time), configuration_(configuration), ego_plane_opt_(std::nullopt), - previous_ego_pose_opt_(std::nullopt) + ego_plane_pose_opt_(std::nullopt) { } @@ -67,7 +67,7 @@ class DetectionSensorBase private: std::optional ego_plane_opt_; - std::optional previous_ego_pose_opt_; + std::optional ego_plane_pose_opt_; auto hasEgoOrientationChanged() const -> bool; }; diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index c25fafb8719..13dad82fb9e 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -84,31 +84,27 @@ auto DetectionSensorBase::isOnOrAboveEgoPlane( */ constexpr static double max_downward_z_offset_ = 1.0; - geometry_msgs::msg::Pose ego_pose_ros, entity_pose_ros; - simulation_interface::toMsg(entity_pose, entity_pose_ros); - simulation_interface::toMsg(ego_pose, ego_pose_ros); - - const auto hasEgoOrientationChanged = [this, &ego_pose_ros]() { - if (previous_ego_pose_opt_) { - return true; - } else { - return math::geometry::getAngleDifference( - ego_pose_ros.orientation, previous_ego_pose_opt_->orientation) > rotation_threshold_; - } + const auto hasEgoOrientationChanged = [this](const geometry_msgs::msg::Pose & ego_pose_ros) { + return math::geometry::getAngleDifference( + ego_pose_ros.orientation, ego_plane_pose_opt_->orientation) > rotation_threshold_; }; - // update ego plane if needed - if (!ego_plane_opt_ || hasEgoOrientationChanged()) { - ego_plane_opt_.emplace( - entity_pose_ros.position, math::geometry::getNormalVector(entity_pose_ros.orientation)); - } - previous_ego_pose_opt_ = ego_pose_ros; - // if other entity is just above ego return true - if (entity_pose_ros.position.z >= (ego_pose_ros.position.z - max_downward_z_offset_)) { + if (entity_pose.position().z() >= (ego_pose.position().z() - max_downward_z_offset_)) { return true; // otherwise check if other entity is above ego plane } else { + // update ego plane if needed + geometry_msgs::msg::Pose ego_pose_ros; + simulation_interface::toMsg(ego_pose, ego_pose_ros); + if (!ego_plane_opt_ || ego_plane_pose_opt_ || hasEgoOrientationChanged(ego_pose_ros)) { + ego_plane_opt_.emplace( + ego_pose_ros.position, math::geometry::getNormalVector(ego_pose_ros.orientation)); + ego_plane_pose_opt_ = ego_pose_ros; + } + + geometry_msgs::msg::Pose entity_pose_ros; + simulation_interface::toMsg(entity_pose, entity_pose_ros); return ego_plane_opt_->offset(entity_pose_ros.position) >= 0.0; } } From 547cb0adbbfe337355e18e206e746860d111d47e Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Tue, 10 Dec 2024 21:22:38 +0100 Subject: [PATCH 093/149] Remove unused function and update comment --- .../sensor_simulation/detection_sensor/detection_sensor.hpp | 1 - .../src/sensor_simulation/detection_sensor/detection_sensor.cpp | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index 237468b0fd8..67fb5bd3025 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -68,7 +68,6 @@ class DetectionSensorBase private: std::optional ego_plane_opt_; std::optional ego_plane_pose_opt_; - auto hasEgoOrientationChanged() const -> bool; }; template diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index 13dad82fb9e..c5f180d70a2 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -89,7 +89,7 @@ auto DetectionSensorBase::isOnOrAboveEgoPlane( ego_pose_ros.orientation, ego_plane_pose_opt_->orientation) > rotation_threshold_; }; - // if other entity is just above ego return true + // if other entity is at the same altitude as Ego or within max_downward_z_offset_ below Ego if (entity_pose.position().z() >= (ego_pose.position().z() - max_downward_z_offset_)) { return true; // otherwise check if other entity is above ego plane From 682acd3e04f8faa9c5bbe8bd3fe4be44280e1b02 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Tue, 10 Dec 2024 23:57:59 +0100 Subject: [PATCH 094/149] fix(simple_sensor_simulator): Fix if condition by adding negation to ensure proper logic --- .../detection_sensor/detection_sensor.hpp | 9 +++------ .../detection_sensor/detection_sensor.cpp | 2 +- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index 67fb5bd3025..0aff66be710 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -40,10 +40,7 @@ class DetectionSensorBase explicit DetectionSensorBase( const double current_simulation_time, const simulation_api_schema::DetectionSensorConfiguration & configuration) - : previous_simulation_time_(current_simulation_time), - configuration_(configuration), - ego_plane_opt_(std::nullopt), - ego_plane_pose_opt_(std::nullopt) + : previous_simulation_time_(current_simulation_time), configuration_(configuration) { } @@ -66,8 +63,8 @@ class DetectionSensorBase const std::vector & lidar_detected_entities) = 0; private: - std::optional ego_plane_opt_; - std::optional ego_plane_pose_opt_; + std::optional ego_plane_opt_{std::nullopt}; + std::optional ego_plane_pose_opt_{std::nullopt}; }; template diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index c5f180d70a2..cf5925688ff 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -97,7 +97,7 @@ auto DetectionSensorBase::isOnOrAboveEgoPlane( // update ego plane if needed geometry_msgs::msg::Pose ego_pose_ros; simulation_interface::toMsg(ego_pose, ego_pose_ros); - if (!ego_plane_opt_ || ego_plane_pose_opt_ || hasEgoOrientationChanged(ego_pose_ros)) { + if (!ego_plane_opt_ || !ego_plane_pose_opt_ || hasEgoOrientationChanged(ego_pose_ros)) { ego_plane_opt_.emplace( ego_pose_ros.position, math::geometry::getNormalVector(ego_pose_ros.orientation)); ego_plane_pose_opt_ = ego_pose_ros; From c4e331de00773761f931ac62d6a3386c51110853 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 11 Dec 2024 13:31:51 +0900 Subject: [PATCH 095/149] Cleanup `FieldOperatorApplication::spinSome` Signed-off-by: yamacir-kit --- .../src/field_operator_application.cpp | 36 +++++++++---------- 1 file changed, 17 insertions(+), 19 deletions(-) diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 6d154b97b46..bce620f25e4 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -37,28 +37,26 @@ auto FieldOperatorApplication::spinSome() -> void { if (rclcpp::ok() and not isStopRequested()) { if (process_id != 0) { - int wstatus = 0; - int ret = waitpid(process_id, &wstatus, WNOHANG); - if (ret == 0) { - return; - } else if (ret < 0) { - if (errno == ECHILD) { + auto status = 0; + if (const auto id = waitpid(process_id, &status, WNOHANG); id < 0) { + switch (errno) { + case ECHILD: + is_autoware_exited = true; + throw common::AutowareError("Autoware process is already terminated"); + default: + AUTOWARE_SYSTEM_ERROR("waitpid"); + std::exit(EXIT_FAILURE); + } + } else if (0 < id) { + if (WIFEXITED(status)) { + is_autoware_exited = true; + throw common::AutowareError( + "Autoware process is unintentionally exited. exit code: ", WEXITSTATUS(status)); + } else if (WIFSIGNALED(status)) { is_autoware_exited = true; - throw common::AutowareError("Autoware process is already terminated"); - } else { - AUTOWARE_SYSTEM_ERROR("waitpid"); - std::exit(EXIT_FAILURE); + throw common::AutowareError("Autoware process is killed. signal is ", WTERMSIG(status)); } } - - if (WIFEXITED(wstatus)) { - is_autoware_exited = true; - throw common::AutowareError( - "Autoware process is unintentionally exited. exit code: ", WEXITSTATUS(wstatus)); - } else if (WIFSIGNALED(wstatus)) { - is_autoware_exited = true; - throw common::AutowareError("Autoware process is killed. signal is ", WTERMSIG(wstatus)); - } } rclcpp::spin_some(get_node_base_interface()); } From d57b87cc9bdf10cb1f6096958ff6a4a6e7e9ab72 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 11 Dec 2024 13:32:22 +0900 Subject: [PATCH 096/149] Remove parameter `random_offset` from scenario `sample.yaml` Signed-off-by: yamacir-kit --- .../scenario_test_runner/scenario/sample.yaml | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/test_runner/scenario_test_runner/scenario/sample.yaml b/test_runner/scenario_test_runner/scenario/sample.yaml index d465b19c270..37d6440f158 100644 --- a/test_runner/scenario_test_runner/scenario/sample.yaml +++ b/test_runner/scenario_test_runner/scenario/sample.yaml @@ -10,16 +10,7 @@ OpenSCENARIO: revMajor: 1 revMinor: 0 ParameterDeclarations: - ParameterDeclaration: - - name: random_offset - parameterType: double - value: $(ros2 run openscenario_interpreter_example uniform_distribution -1.0 1.0) - ConstraintGroup: - - ValueConstraint: - - rule: lessOrEqual - value: 1.0 - - rule: greaterOrEqual - value: -1.0 + ParameterDeclaration: [] CatalogLocations: VehicleCatalog: Directory: @@ -54,7 +45,7 @@ OpenSCENARIO: roadId: '' laneId: LANE_ID s: 1 - offset: $random_offset + offset: 0 Orientation: type: relative h: 0 From 3cf51694a0c51152da4ce7760a1f7df354e16818 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 11 Dec 2024 13:58:32 +0900 Subject: [PATCH 097/149] Remove virtual function `sendSIGINT` Signed-off-by: yamacir-kit --- .../include/concealer/field_operator_application.hpp | 4 ---- .../field_operator_application_for_autoware_universe.hpp | 1 - external/concealer/src/field_operator_application.cpp | 2 +- .../src/field_operator_application_for_autoware_universe.cpp | 5 ----- 4 files changed, 1 insertion(+), 11 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 2d12c82470c..b2bf66cd7a2 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -71,10 +71,6 @@ class FieldOperatorApplication : public rclcpp::Node auto isStopRequested() const noexcept -> bool; - // this method is purely virtual because different Autoware types are killed differently - // currently, we are not sure why this is the case so detailed investigation is needed - virtual auto sendSIGINT() -> void = 0; - // method called in destructor of a derived class // because it is difficult to differentiate shutting down behavior in destructor of a base class auto shutdownAutoware() -> void; diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 2948ae427bd..27116446176 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -128,7 +128,6 @@ class FieldOperatorApplicationFor #undef CASE } - auto sendSIGINT() -> void override; public: SubscriberWrapper getPathWithLaneId; diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index bce620f25e4..e0632650fff 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -91,7 +91,7 @@ auto FieldOperatorApplication::shutdownAutoware() -> void return timeout; }(); - if (sendSIGINT(); sigtimedwait(&sigset, nullptr, &timeout) < 0) { + if (::kill(process_id, SIGINT); sigtimedwait(&sigset, nullptr, &timeout) < 0) { switch (errno) { case EINTR: /* diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index ca2bdb483d0..af93164e287 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -427,11 +427,6 @@ auto FieldOperatorApplicationFor::getMinimumRiskManeuverStateN return minimum_risk_maneuver_state; } -auto FieldOperatorApplicationFor::sendSIGINT() -> void // -{ - ::kill(process_id, SIGINT); -} - auto FieldOperatorApplicationFor::setVelocityLimit(double velocity_limit) -> void { task_queue.delay([this, velocity_limit]() { From e0f40f21da77b44c1cb0adff0aeb25dc94ed7575 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 11 Dec 2024 14:15:08 +0900 Subject: [PATCH 098/149] Remove member function `isStopRequested` and `stopRequest` Signed-off-by: yamacir-kit --- .../include/concealer/field_operator_application.hpp | 8 +------- .../include/concealer/transition_assertion.hpp | 2 +- external/concealer/src/field_operator_application.cpp | 11 ++--------- 3 files changed, 4 insertions(+), 17 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index b2bf66cd7a2..14bdd30cc22 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -54,28 +54,22 @@ class FieldOperatorApplicationFor; * initialize, plan, and engage. * * -------------------------------------------------------------------------- */ -class FieldOperatorApplication : public rclcpp::Node +struct FieldOperatorApplication : public rclcpp::Node { std::atomic is_stop_requested = false; bool is_autoware_exited = false; -protected: const pid_t process_id = 0; TaskQueue task_queue; bool initialize_was_called = false; - auto stopRequest() noexcept -> void; - - auto isStopRequested() const noexcept -> bool; - // method called in destructor of a derived class // because it is difficult to differentiate shutting down behavior in destructor of a base class auto shutdownAutoware() -> void; -public: CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t = 0); template diff --git a/external/concealer/include/concealer/transition_assertion.hpp b/external/concealer/include/concealer/transition_assertion.hpp index 7850899362e..39121175319 100644 --- a/external/concealer/include/concealer/transition_assertion.hpp +++ b/external/concealer/include/concealer/transition_assertion.hpp @@ -45,7 +45,7 @@ struct TransitionAssertion auto waitForAutowareStateToBe##STATE( \ Thunk && thunk = [] {}, Interval interval = std::chrono::seconds(1)) \ { \ - for (thunk(); not static_cast(*this).isStopRequested() and \ + for (thunk(); not static_cast(*this).is_stop_requested.load() and \ not static_cast(*this).is##STATE(); \ rclcpp::GenericRate(interval).sleep()) { \ if ( \ diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index e0632650fff..ef0d86d66a6 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -26,16 +26,9 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) { } -auto FieldOperatorApplication::stopRequest() noexcept -> void { is_stop_requested.store(true); } - -auto FieldOperatorApplication::isStopRequested() const noexcept -> bool -{ - return is_stop_requested.load(); -} - auto FieldOperatorApplication::spinSome() -> void { - if (rclcpp::ok() and not isStopRequested()) { + if (rclcpp::ok() and not is_stop_requested.load()) { if (process_id != 0) { auto status = 0; if (const auto id = waitpid(process_id, &status, WNOHANG); id < 0) { @@ -64,7 +57,7 @@ auto FieldOperatorApplication::spinSome() -> void auto FieldOperatorApplication::shutdownAutoware() -> void { - if (stopRequest(); process_id != 0 && not std::exchange(is_autoware_exited, true)) { + if (is_stop_requested.store(true); process_id != 0 && not std::exchange(is_autoware_exited, true)) { const auto sigset = [this]() { if (auto signal_set = sigset_t(); sigemptyset(&signal_set) or sigaddset(&signal_set, SIGCHLD)) { From 724d705c6b203e205d9b18e7b0a1a6c074773d2e Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 11 Dec 2024 14:48:34 +0900 Subject: [PATCH 099/149] Update `~FieldOperatorApplication()` to be non-virtual Signed-off-by: yamacir-kit --- .../include/concealer/field_operator_application.hpp | 2 +- .../field_operator_application_for_autoware_universe.hpp | 2 -- external/concealer/src/field_operator_application.cpp | 7 +++++++ .../field_operator_application_for_autoware_universe.cpp | 7 ------- 4 files changed, 8 insertions(+), 10 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 14bdd30cc22..e38a8caaccd 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -78,7 +78,7 @@ struct FieldOperatorApplication : public rclcpp::Node { } - ~FieldOperatorApplication() override = default; + ~FieldOperatorApplication(); auto spinSome() -> void; diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 27116446176..3b92a56043a 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -162,8 +162,6 @@ class FieldOperatorApplicationFor { } - ~FieldOperatorApplicationFor() override; - auto engage() -> void override; auto engageable() const -> bool override; diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index ef0d86d66a6..08fdab83b85 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -26,6 +26,13 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) { } +FieldOperatorApplication::~FieldOperatorApplication() +{ + shutdownAutoware(); + // All tasks should be complete before the services used in them will be deinitialized. + task_queue.stopAndJoin(); +} + auto FieldOperatorApplication::spinSome() -> void { if (rclcpp::ok() and not is_stop_requested.load()) { diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index af93164e287..327fd1cb3ee 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -20,13 +20,6 @@ namespace concealer { -FieldOperatorApplicationFor::~FieldOperatorApplicationFor() -{ - shutdownAutoware(); - // All tasks should be complete before the services used in them will be deinitialized. - task_queue.stopAndJoin(); -} - template struct lister { From 3f4016054ed78211b63370c88b293dcdf3df12a8 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 11 Dec 2024 15:59:36 +0900 Subject: [PATCH 100/149] Remove macro `CONCEALER_ISOLATE_STANDARD_OUTPUT` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 2 -- external/concealer/include/concealer/launch.hpp | 15 --------------- 2 files changed, 17 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index e38a8caaccd..26d4b387feb 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -15,8 +15,6 @@ #ifndef CONCEALER__AUTOWARE_USER_HPP_ #define CONCEALER__AUTOWARE_USER_HPP_ -// #define CONCEALER_ISOLATE_STANDARD_OUTPUT - #include #include diff --git a/external/concealer/include/concealer/launch.hpp b/external/concealer/include/concealer/launch.hpp index 54f1459f1b3..55f4da068f3 100644 --- a/external/concealer/include/concealer/launch.hpp +++ b/external/concealer/include/concealer/launch.hpp @@ -15,13 +15,6 @@ #ifndef CONCEALER__LAUNCH_HPP_ #define CONCEALER__LAUNCH_HPP_ -#ifdef CONCEALER_ISOLATE_STANDARD_OUTPUT -#include -#include -#include -#include -#endif - #include #include #include @@ -37,14 +30,6 @@ 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"; - const auto fd = ::open(log_filename.c_str(), O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); - ::dup2(fd, STDOUT_FILENO); - ::dup2(fd, STDERR_FILENO); - ::close(fd); -#endif - const auto argv = [&]() { auto argv = std::vector(); From f0c86a40ee3b2daed448d1d2735a4889eee206d3 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 11 Dec 2024 17:08:00 +0900 Subject: [PATCH 101/149] Update all members of `FieldOperatorApplicationFor<...>` to be public Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 31 ++----------------- ...ator_application_for_autoware_universe.hpp | 14 +++------ 2 files changed, 6 insertions(+), 39 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 26d4b387feb..d92a66321ad 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -17,19 +17,15 @@ #include -#include #include #include -#include #include #include #include #include #include -#include #include #include -#include #include #include #include @@ -64,10 +60,6 @@ struct FieldOperatorApplication : public rclcpp::Node bool initialize_was_called = false; - // method called in destructor of a derived class - // because it is difficult to differentiate shutting down behavior in destructor of a base class - auto shutdownAutoware() -> void; - CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t = 0); template @@ -80,35 +72,16 @@ struct FieldOperatorApplication : public rclcpp::Node auto spinSome() -> void; - /* ---- NOTE ------------------------------------------------------------------- - * - * Send an engagement request to Autoware. If Autoware does not have an - * engagement equivalent, this operation can be nop (No operation). - * - * -------------------------------------------------------------------------- */ + auto shutdownAutoware() -> void; + virtual auto engage() -> void = 0; virtual auto engageable() const -> bool = 0; virtual auto engaged() const -> bool = 0; - /* ---- NOTE ------------------------------------------------------------------- - * - * Send initial_pose to Autoware. - * - * -------------------------------------------------------------------------- */ virtual auto initialize(const geometry_msgs::msg::Pose &) -> void = 0; - /* ---- NOTE ------------------------------------------------------------------- - * - * Send the destination and route constraint points to Autoware. The argument - * route is guaranteed to be size 1 or greater, and its last element is the - * destination. When the size of a route is 2 or greater, the non-last element - * is the route constraint. That is, Autoware must go through the element - * points on the given'route' starting at index 0 and stop at index - * route.size() - 1. - * - * -------------------------------------------------------------------------- */ virtual auto plan(const std::vector &) -> void = 0; virtual auto clearRoute() -> void = 0; diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 3b92a56043a..1f365964eb3 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -46,12 +46,10 @@ namespace concealer { template <> -class FieldOperatorApplicationFor +struct FieldOperatorApplicationFor : public FieldOperatorApplication, public TransitionAssertion> { - friend struct TransitionAssertion>; - // clang-format off SubscriberWrapper getCommand; SubscriberWrapper getAutowareState; @@ -61,6 +59,7 @@ class FieldOperatorApplicationFor SubscriberWrapper getLocalizationState; #endif SubscriberWrapper getMrmState; + SubscriberWrapper getPathWithLaneId; SubscriberWrapper getTrajectory; SubscriberWrapper getTurnIndicatorsCommandImpl; @@ -105,7 +104,6 @@ class FieldOperatorApplicationFor #undef DEFINE_STATE_PREDICATE -protected: template auto getAutowareStateString(std::uint8_t state) const -> char const * { @@ -129,10 +127,6 @@ class FieldOperatorApplicationFor #undef CASE } -public: - SubscriberWrapper getPathWithLaneId; - -public: template CONCEALER_PUBLIC explicit FieldOperatorApplicationFor(Ts &&... xs) : FieldOperatorApplication(std::forward(xs)...), @@ -146,6 +140,7 @@ class FieldOperatorApplicationFor getLocalizationState("/api/localization/initialization_state", rclcpp::QoS(1).transient_local(), *this), #endif getMrmState("/api/fail_safe/mrm_state", rclcpp::QoS(1), *this, [this](const auto & v) { receiveMrmState(v); }), + getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), *this), getTrajectory("/api/iv_msgs/planning/scenario_planning/trajectory", rclcpp::QoS(1), *this), getTurnIndicatorsCommandImpl("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), requestClearRoute("/api/routing/clear_route", *this), @@ -156,8 +151,7 @@ class FieldOperatorApplicationFor requestSetRoutePoints("/api/routing/set_route_points", *this, std::chrono::seconds(10)), requestSetRtcAutoMode("/api/external/set/rtc_auto_mode", *this), requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this), - requestEnableAutowareControl("/api/operation_mode/enable_autoware_control", *this), - getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), *this) + requestEnableAutowareControl("/api/operation_mode/enable_autoware_control", *this) // clang-format on { } From b84c41fe58253e0ee7e94a329fb133afdbecb429 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 11 Dec 2024 17:11:59 +0900 Subject: [PATCH 102/149] Move `getAutowareState` into `FieldOperatorApplication` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 25 ++++++++++ ...ator_application_for_autoware_universe.hpp | 49 ------------------- .../src/field_operator_application.cpp | 31 +++++++++++- 3 files changed, 54 insertions(+), 51 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index d92a66321ad..7cd4795cdeb 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -18,9 +18,11 @@ #include #include +#include #include #include #include +#include #include #include #include @@ -60,6 +62,10 @@ struct FieldOperatorApplication : public rclcpp::Node bool initialize_was_called = false; + std::string autoware_state; + + SubscriberWrapper getAutowareState; + CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t = 0); template @@ -70,6 +76,25 @@ struct FieldOperatorApplication : public rclcpp::Node ~FieldOperatorApplication(); + /* + NOTE: This predicate should not take the state being compared as an + argument or template parameter. Otherwise, code using this class would + need to have knowledge of the Autoware state type. + */ +#define DEFINE_STATE_PREDICATE(NAME, VALUE) \ + auto is##NAME() const noexcept { return autoware_state == #VALUE; } \ + static_assert(true, "") + + DEFINE_STATE_PREDICATE(Initializing, INITIALIZING_VEHICLE); + DEFINE_STATE_PREDICATE(WaitingForRoute, WAITING_FOR_ROUTE); + DEFINE_STATE_PREDICATE(Planning, PLANNING); + DEFINE_STATE_PREDICATE(WaitingForEngage, WAITING_FOR_ENGAGE); + DEFINE_STATE_PREDICATE(Driving, DRIVING); + DEFINE_STATE_PREDICATE(ArrivedGoal, ARRIVAL_GOAL); + DEFINE_STATE_PREDICATE(Finalizing, FINALIZING); + +#undef DEFINE_STATE_PREDICATE + auto spinSome() -> void; auto shutdownAutoware() -> void; diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 1f365964eb3..6e45dc01ceb 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -25,13 +25,11 @@ #include #include #include -#include #include #include #include #include #include -#include #include #include #include @@ -52,7 +50,6 @@ struct FieldOperatorApplicationFor { // clang-format off SubscriberWrapper getCommand; - SubscriberWrapper getAutowareState; SubscriberWrapper getCooperateStatusArray; SubscriberWrapper getEmergencyState; #if __has_include() @@ -75,8 +72,6 @@ struct FieldOperatorApplicationFor tier4_rtc_msgs::msg::CooperateStatusArray latest_cooperate_status_array; - std::string autoware_state; - std::string minimum_risk_maneuver_state; std::string minimum_risk_maneuver_behavior; @@ -85,55 +80,11 @@ struct FieldOperatorApplicationFor auto receiveEmergencyState(const tier4_external_api_msgs::msg::Emergency & msg) -> void; - /* - NOTE: This predicate should not take the state being compared as an - argument or template parameter. Otherwise, code using this class would - need to have knowledge of the Autoware state type. - */ -#define DEFINE_STATE_PREDICATE(NAME, VALUE) \ - auto is##NAME() const noexcept { return autoware_state == #VALUE; } \ - static_assert(true, "") - - DEFINE_STATE_PREDICATE(Initializing, INITIALIZING_VEHICLE); - DEFINE_STATE_PREDICATE(WaitingForRoute, WAITING_FOR_ROUTE); - DEFINE_STATE_PREDICATE(Planning, PLANNING); - DEFINE_STATE_PREDICATE(WaitingForEngage, WAITING_FOR_ENGAGE); - DEFINE_STATE_PREDICATE(Driving, DRIVING); - DEFINE_STATE_PREDICATE(ArrivedGoal, ARRIVAL_GOAL); - DEFINE_STATE_PREDICATE(Finalizing, FINALIZING); - -#undef DEFINE_STATE_PREDICATE - - template - auto getAutowareStateString(std::uint8_t state) const -> char const * - { -#define CASE(IDENTIFIER) \ - case T::IDENTIFIER: \ - return #IDENTIFIER - - switch (state) { - CASE(INITIALIZING); - CASE(WAITING_FOR_ROUTE); - CASE(PLANNING); - CASE(WAITING_FOR_ENGAGE); - CASE(DRIVING); - CASE(ARRIVED_GOAL); - CASE(FINALIZING); - - default: - return ""; - } - -#undef CASE - } - template CONCEALER_PUBLIC explicit FieldOperatorApplicationFor(Ts &&... xs) : FieldOperatorApplication(std::forward(xs)...), // clang-format off getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), - getAutowareState("/autoware/state", rclcpp::QoS(1), *this, [this](const auto & v) { - autoware_state = getAutowareStateString(v.state); }), getCooperateStatusArray("/api/external/get/rtc_status", rclcpp::QoS(1), *this, [this](const auto & v) { latest_cooperate_status_array = v; }), getEmergencyState("/api/external/get/emergency", rclcpp::QoS(1), *this, [this](const auto & v) { receiveEmergencyState(v); }), #if __has_include() diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 08fdab83b85..490346ff106 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -20,9 +20,35 @@ namespace concealer { +template +auto toAutowareStateString(std::uint8_t state) -> char const * +{ +#define CASE(IDENTIFIER) \ + case T::IDENTIFIER: \ + return #IDENTIFIER + + switch (state) { + CASE(INITIALIZING); + CASE(WAITING_FOR_ROUTE); + CASE(PLANNING); + CASE(WAITING_FOR_ENGAGE); + CASE(DRIVING); + CASE(ARRIVED_GOAL); + CASE(FINALIZING); + + default: + return ""; + } + +#undef CASE +} + FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) : rclcpp::Node("concealer_user", "simulation", rclcpp::NodeOptions().use_global_arguments(false)), - process_id(pid) + process_id(pid), + getAutowareState("/autoware/state", rclcpp::QoS(1), *this, [this](const auto & v) { + autoware_state = toAutowareStateString(v.state); + }) { } @@ -64,7 +90,8 @@ auto FieldOperatorApplication::spinSome() -> void auto FieldOperatorApplication::shutdownAutoware() -> void { - if (is_stop_requested.store(true); process_id != 0 && not std::exchange(is_autoware_exited, true)) { + if (is_stop_requested.store(true); + process_id != 0 && not std::exchange(is_autoware_exited, true)) { const auto sigset = [this]() { if (auto signal_set = sigset_t(); sigemptyset(&signal_set) or sigaddset(&signal_set, SIGCHLD)) { From a2cae3a2e05473c1a748975f269380616260093f Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 11 Dec 2024 17:27:41 +0900 Subject: [PATCH 103/149] Move base class `TransitionAssertion` to `FieldOperatorApplication` Signed-off-by: yamacir-kit --- .../include/concealer/field_operator_application.hpp | 3 ++- .../field_operator_application_for_autoware_universe.hpp | 4 +--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 7cd4795cdeb..ab45b515c61 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -50,7 +50,8 @@ class FieldOperatorApplicationFor; * initialize, plan, and engage. * * -------------------------------------------------------------------------- */ -struct FieldOperatorApplication : public rclcpp::Node +struct FieldOperatorApplication : public rclcpp::Node, + public TransitionAssertion { std::atomic is_stop_requested = false; diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 6e45dc01ceb..01b3afbce48 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -44,9 +44,7 @@ namespace concealer { template <> -struct FieldOperatorApplicationFor -: public FieldOperatorApplication, - public TransitionAssertion> +struct FieldOperatorApplicationFor : public FieldOperatorApplication { // clang-format off SubscriberWrapper getCommand; From 9c568e97222a8ba6fa29fbde1a2b732e6710ebbf Mon Sep 17 00:00:00 2001 From: Release Bot Date: Wed, 11 Dec 2024 09:17:20 +0000 Subject: [PATCH 104/149] Bump version of scenario_simulator_v2 from version 7.0.0 to version 7.0.1 --- common/math/arithmetic/CHANGELOG.rst | 6 ++++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 6 ++++++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 6 ++++++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 6 ++++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 6 ++++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 6 ++++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 6 ++++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 6 ++++++ map/kashiwanoha_map/package.xml | 2 +- map/simple_cross_map/CHANGELOG.rst | 6 ++++++ map/simple_cross_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 6 ++++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../CHANGELOG.rst | 6 ++++++ .../openscenario_experimental_catalog/package.xml | 2 +- .../openscenario_interpreter/CHANGELOG.rst | 14 ++++++++++++++ openscenario/openscenario_interpreter/package.xml | 2 +- .../openscenario_interpreter_example/CHANGELOG.rst | 6 ++++++ .../openscenario_interpreter_example/package.xml | 2 +- .../openscenario_interpreter_msgs/CHANGELOG.rst | 6 ++++++ .../openscenario_interpreter_msgs/package.xml | 2 +- .../openscenario_preprocessor/CHANGELOG.rst | 6 ++++++ openscenario/openscenario_preprocessor/package.xml | 2 +- .../openscenario_preprocessor_msgs/CHANGELOG.rst | 6 ++++++ .../openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 6 ++++++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 6 ++++++ openscenario/openscenario_validator/package.xml | 2 +- .../openscenario_visualization/CHANGELOG.rst | 6 ++++++ .../openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 6 ++++++ .../package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 6 ++++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 6 ++++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 6 ++++++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 6 ++++++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 6 ++++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 6 ++++++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 6 ++++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 6 ++++++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 6 ++++++ test_runner/scenario_test_runner/package.xml | 2 +- 58 files changed, 211 insertions(+), 29 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index ffbec95e648..11a6fa0e40e 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index f6e3214ac47..5707c36b6fd 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 7.0.0 + 7.0.1 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index c5936ea71e6..4dc5aaf01b7 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index a9b603c7e7f..57b9100a3eb 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 7.0.0 + 7.0.1 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 1151b1ff95c..11093af028b 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 74d9979120a..4bf670df97d 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 7.0.0 + 7.0.1 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index f753416d42b..0e2676c8a60 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 18a6bd5e5b8..2e2e8f96fe1 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 7.0.0 + 7.0.1 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index d047520783c..89b829384f6 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 46aa5e5e1ed..2884cbb75f9 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 7.0.0 + 7.0.1 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index bfee9cd9f14..c826ef2e84b 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge pull request `#1454 `_ from tier4/RJD-736/autoware_msgs_support diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 0803d4c4cfb..06550716cb6 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 7.0.0 + 7.0.1 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index d8076f4cae3..fb4759552b2 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,12 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index e9ea9025c11..3f2966bfff6 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 7.0.0 + 7.0.1 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index cd5eb9f6491..893a9858a22 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 1f90ffe02a6..785151a0a68 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 7.0.0 + 7.0.1 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 2609e18e217..ecfba00ef7f 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,12 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index 21100000215..46c2aacd5fb 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 7.0.0 + 7.0.1 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 7324aafffb5..ebf606436c3 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge pull request `#1454 `_ from tier4/RJD-736/autoware_msgs_support diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 83b675be3d8..c131f71f45c 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 7.0.0 + 7.0.1 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 0e6f94cf1b9..d50a2494615 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index b7bc016fa2c..e58170312db 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 7.0.0 + 7.0.1 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index d9efb5ec129..91633085118 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,20 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge pull request `#1462 `_ from tier4/feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Change default value definition +* Update schema comment +* Use always_true in Event +* Fix StopTrigger behavior +* Fix version +* Specify version on comment +* Set default value for Start/StopTrigger in Act +* Contributors: Kotaro Yoshimoto, f0reachARR, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge pull request `#1454 `_ from tier4/RJD-736/autoware_msgs_support diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 189fc370650..a436602f709 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 7.0.0 + 7.0.1 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 58277e25f72..8f55fa25ee3 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge pull request `#1454 `_ from tier4/RJD-736/autoware_msgs_support diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 7b79d5418f6..b9642b3aca0 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 7.0.0 + 7.0.1 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index 3a0dbbbd85b..882a29dc4f4 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index a2b749e70d0..6ab00575b6c 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 7.0.0 + 7.0.1 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index 45aa124d2ab..44086f88ca5 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 8e1cf8e1c32..1304d3a2b95 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 7.0.0 + 7.0.1 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index cd4d1c1d339..ac63b6c22ec 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index f944de3e947..bbd9db7f3b7 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 7.0.0 + 7.0.1 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 1fc991b6a01..143d4971857 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,12 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index baf21f54724..76e182f4fed 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 7.0.0 + 7.0.1 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index cec132f84ec..71229bc2469 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,12 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index fc8cccc8550..7fc225093da 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 7.0.0 + 7.0.1 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 92bcf0fc007..80e709cb026 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index f400f471349..d5803df72e6 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 7.0.0 + 7.0.1 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 1a482062317..f7f2d79f3c3 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,12 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support 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 65fceb3047c..f30ad193f67 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 - 7.0.0 + 7.0.1 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 266932b7f88..d40187b2501 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 0845fa1e7eb..8a892894a0d 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 7.0.0 + 7.0.1 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index b699f100bb5..9f04c12db37 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 862137bf157..09b64c905fe 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 7.0.0 + 7.0.1 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index b54d16b55e4..cfc854c265c 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 01ad3e54707..e9938b62acb 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 7.0.0 + 7.0.1 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index ca1c8360096..977a08d8ecc 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge pull request `#1454 `_ from tier4/RJD-736/autoware_msgs_support diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 30d0cc939da..e3dfe78d048 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 7.0.0 + 7.0.1 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 773d73013ec..b1d510a9578 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge pull request `#1454 `_ from tier4/RJD-736/autoware_msgs_support diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 9cbff0ec53e..04d18defa97 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 7.0.0 + 7.0.1 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index ebe8a1143ae..f61e96c4a38 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge pull request `#1454 `_ from tier4/RJD-736/autoware_msgs_support diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 8113d65ce1e..7427a58b2ad 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 7.0.0 + 7.0.1 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index cf8105964a2..054c854cc40 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index e7c090973f2..ecc189edecd 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 7.0.0 + 7.0.1 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 91e98512908..dc2296feba0 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge pull request `#1454 `_ from tier4/RJD-736/autoware_msgs_support diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 4438911c445..a4d4cdfd2b8 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 7.0.0 + 7.0.1 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index e2d016367e9..e3c9a70f4d4 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,12 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.1 (2024-12-11) +------------------ +* Merge branch 'master' into feature/act-starttrigger-optional +* Merge branch 'master' into feature/act-starttrigger-optional +* Contributors: Kotaro Yoshimoto, ぐるぐる + 7.0.0 (2024-12-10) ------------------ * Merge pull request `#1454 `_ from tier4/RJD-736/autoware_msgs_support diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 58ba79dc752..9fa9dac0716 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 7.0.0 + 7.0.1 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From b6ecb69f5ee327536640e75aae6e896635975ea9 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 11 Dec 2024 18:19:29 +0900 Subject: [PATCH 105/149] Remove member function `receiveEmergencyState` Signed-off-by: yamacir-kit --- .../include/concealer/field_operator_application.hpp | 2 ++ .../field_operator_application_for_autoware_universe.hpp | 8 +++++--- .../field_operator_application_for_autoware_universe.cpp | 8 -------- 3 files changed, 7 insertions(+), 11 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index ab45b515c61..0b572c31665 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -65,7 +65,9 @@ struct FieldOperatorApplication : public rclcpp::Node, std::string autoware_state; + // clang-format off SubscriberWrapper getAutowareState; + // clang-format on CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t = 0); diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 01b3afbce48..e412dcd5d4a 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -76,15 +76,17 @@ struct FieldOperatorApplicationFor : public FieldOperatorAppli auto receiveMrmState(const autoware_adapi_v1_msgs::msg::MrmState & msg) -> void; - auto receiveEmergencyState(const tier4_external_api_msgs::msg::Emergency & msg) -> void; - template CONCEALER_PUBLIC explicit FieldOperatorApplicationFor(Ts &&... xs) : FieldOperatorApplication(std::forward(xs)...), // clang-format off getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), getCooperateStatusArray("/api/external/get/rtc_status", rclcpp::QoS(1), *this, [this](const auto & v) { latest_cooperate_status_array = v; }), - getEmergencyState("/api/external/get/emergency", rclcpp::QoS(1), *this, [this](const auto & v) { receiveEmergencyState(v); }), + getEmergencyState("/api/external/get/emergency", rclcpp::QoS(1), *this, [this](const auto & message) { + if (message.emergency) { + throw common::Error("Emergency state received"); + } + }), #if __has_include() getLocalizationState("/api/localization/initialization_state", rclcpp::QoS(1).transient_local(), *this), #endif diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index 327fd1cb3ee..5654b823763 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -460,14 +460,6 @@ auto FieldOperatorApplicationFor::enableAutowareControl() -> v }); } -auto FieldOperatorApplicationFor::receiveEmergencyState( - const tier4_external_api_msgs::msg::Emergency & message) -> void -{ - if (message.emergency) { - throw common::Error("Emergency state received"); - } -} - template auto toMinimumRiskManeuverBehaviorString(const std::uint8_t & behavior_number) -> std::string { From 7c9d6d616ea1e1acdf7888342ada3cb4755da460 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 11 Dec 2024 18:46:56 +0900 Subject: [PATCH 106/149] Remove member function `receiveMrmState` Signed-off-by: yamacir-kit --- ...ator_application_for_autoware_universe.hpp | 97 ++++++++++++++++- ...ator_application_for_autoware_universe.cpp | 100 ------------------ 2 files changed, 94 insertions(+), 103 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index e412dcd5d4a..952df6a67da 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -43,6 +43,48 @@ namespace concealer { +#define DEFINE_STATIC_DATA_MEMBER_DETECTOR(NAME) \ + template \ + struct HasStatic##NAME : public std::false_type \ + { \ + }; \ + \ + template \ + struct HasStatic##NAME> : public std::true_type \ + { \ + } + +DEFINE_STATIC_DATA_MEMBER_DETECTOR(NONE); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(LANE_CHANGE_LEFT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(LANE_CHANGE_RIGHT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_LEFT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_RIGHT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(GOAL_PLANNER); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(START_PLANNER); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(PULL_OUT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(TRAFFIC_LIGHT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(INTERSECTION); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(INTERSECTION_OCCLUSION); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(CROSSWALK); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(BLIND_SPOT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(DETECTION_AREA); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(NO_STOPPING_AREA); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(OCCLUSION_SPOT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_LEFT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_RIGHT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_BY_LC_LEFT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_BY_LC_RIGHT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(NO_DRIVABLE_LANE); + +// For MrmState::behavior +DEFINE_STATIC_DATA_MEMBER_DETECTOR(COMFORTABLE_STOP); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(EMERGENCY_STOP); +// DEFINE_STATIC_DATA_MEMBER_DETECTOR(NONE); // NOTE: This is defined above. +DEFINE_STATIC_DATA_MEMBER_DETECTOR(UNKNOWN); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(PULL_OVER); + +#undef DEFINE_STATIC_DATA_MEMBER_DETECTOR + template <> struct FieldOperatorApplicationFor : public FieldOperatorApplication { @@ -74,8 +116,6 @@ struct FieldOperatorApplicationFor : public FieldOperatorAppli std::string minimum_risk_maneuver_behavior; - auto receiveMrmState(const autoware_adapi_v1_msgs::msg::MrmState & msg) -> void; - template CONCEALER_PUBLIC explicit FieldOperatorApplicationFor(Ts &&... xs) : FieldOperatorApplication(std::forward(xs)...), @@ -90,7 +130,58 @@ struct FieldOperatorApplicationFor : public FieldOperatorAppli #if __has_include() getLocalizationState("/api/localization/initialization_state", rclcpp::QoS(1).transient_local(), *this), #endif - getMrmState("/api/fail_safe/mrm_state", rclcpp::QoS(1), *this, [this](const auto & v) { receiveMrmState(v); }), + getMrmState("/api/fail_safe/mrm_state", rclcpp::QoS(1), *this, [this](const auto & message) { + auto state_name_of = [](auto state) constexpr { + switch (state) { + case autoware_adapi_v1_msgs::msg::MrmState::MRM_FAILED: + return "MRM_FAILED"; + case autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING: + return "MRM_OPERATING"; + case autoware_adapi_v1_msgs::msg::MrmState::MRM_SUCCEEDED: + return "MRM_SUCCEEDED"; + case autoware_adapi_v1_msgs::msg::MrmState::NORMAL: + return "NORMAL"; + case autoware_adapi_v1_msgs::msg::MrmState::UNKNOWN: + return "UNKNOWN"; + default: + throw common::Error( + "Unexpected autoware_adapi_v1_msgs::msg::MrmState::state, number: ", state); + } + }; + + auto behavior_name_of = [](auto behavior) constexpr { + if constexpr (HasStaticCOMFORTABLE_STOP::value) { + if (behavior == autoware_adapi_v1_msgs::msg::MrmState::COMFORTABLE_STOP) { + return "COMFORTABLE_STOP"; + } + } + if constexpr (HasStaticEMERGENCY_STOP::value) { + if (behavior == autoware_adapi_v1_msgs::msg::MrmState::EMERGENCY_STOP) { + return "EMERGENCY_STOP"; + } + } + if constexpr (HasStaticNONE::value) { + if (behavior == autoware_adapi_v1_msgs::msg::MrmState::NONE) { + return "NONE"; + } + } + if constexpr (HasStaticUNKNOWN::value) { + if (behavior == autoware_adapi_v1_msgs::msg::MrmState::UNKNOWN) { + return "UNKNOWN"; + } + } + if constexpr (HasStaticPULL_OVER::value) { + if (behavior == autoware_adapi_v1_msgs::msg::MrmState::PULL_OVER) { + return "PULL_OVER"; + } + } + throw common::Error( + "Unexpected autoware_adapi_v1_msgs::msg::MrmState::behavior, number: ", behavior); + }; + + minimum_risk_maneuver_state = state_name_of(message.state); + minimum_risk_maneuver_behavior = behavior_name_of(message.behavior); + }), getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), *this), getTrajectory("/api/iv_msgs/planning/scenario_planning/trajectory", rclcpp::QoS(1), *this), getTurnIndicatorsCommandImpl("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index 5654b823763..d15bc6fee15 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -56,48 +56,6 @@ auto listup(const Tuples & tuples) -> lister return lister(tuples); } -#define DEFINE_STATIC_DATA_MEMBER_DETECTOR(NAME) \ - template \ - struct HasStatic##NAME : public std::false_type \ - { \ - }; \ - \ - template \ - struct HasStatic##NAME> : public std::true_type \ - { \ - } - -DEFINE_STATIC_DATA_MEMBER_DETECTOR(NONE); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(LANE_CHANGE_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(LANE_CHANGE_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(GOAL_PLANNER); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(START_PLANNER); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(PULL_OUT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(TRAFFIC_LIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(INTERSECTION); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(INTERSECTION_OCCLUSION); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(CROSSWALK); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(BLIND_SPOT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(DETECTION_AREA); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(NO_STOPPING_AREA); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(OCCLUSION_SPOT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_BY_LC_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_BY_LC_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(NO_DRIVABLE_LANE); - -// For MrmState::behavior -DEFINE_STATIC_DATA_MEMBER_DETECTOR(COMFORTABLE_STOP); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(EMERGENCY_STOP); -// DEFINE_STATIC_DATA_MEMBER_DETECTOR(NONE); // NOTE: This is defined above. -DEFINE_STATIC_DATA_MEMBER_DETECTOR(UNKNOWN); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(PULL_OVER); - -#undef DEFINE_STATIC_DATA_MEMBER_DETECTOR - /** * NOTE: for changes from `distance` to start/finish distance * see https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef @@ -459,62 +417,4 @@ auto FieldOperatorApplicationFor::enableAutowareControl() -> v requestEnableAutowareControl(request); }); } - -template -auto toMinimumRiskManeuverBehaviorString(const std::uint8_t & behavior_number) -> std::string -{ - static const std::unordered_map behavior_string_map = [&]() { - std::unordered_map behavior_string_map; - -#define EMPLACE(IDENTIFIER) \ - if constexpr (HasStatic##IDENTIFIER::value) { \ - behavior_string_map.emplace(T::IDENTIFIER, #IDENTIFIER); \ - } \ - static_assert(true) - - EMPLACE(COMFORTABLE_STOP); - EMPLACE(EMERGENCY_STOP); - EMPLACE(NONE); - EMPLACE(UNKNOWN); - EMPLACE(PULL_OVER); - -#undef EMPLACE - return behavior_string_map; - }(); - - if (const auto behavior = behavior_string_map.find(behavior_number); - behavior == behavior_string_map.end()) { - throw common::Error( - "Unexpected autoware_adapi_v1_msgs::msg::MrmState::behavior, number: ", behavior_number); - } else { - return behavior->second; - } -} - -auto toMinimumRiskManeuverStateString(const std::uint8_t & state_number) -> std::string -{ - static const std::unordered_map state_string_map = { - {autoware_adapi_v1_msgs::msg::MrmState::MRM_FAILED, "MRM_FAILED"}, - {autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING, "MRM_OPERATING"}, - {autoware_adapi_v1_msgs::msg::MrmState::MRM_SUCCEEDED, "MRM_SUCCEEDED"}, - {autoware_adapi_v1_msgs::msg::MrmState::NORMAL, "NORMAL"}, - {autoware_adapi_v1_msgs::msg::MrmState::UNKNOWN, "UNKNOWN"}, - }; - - if (const auto state = state_string_map.find(state_number); state == state_string_map.end()) { - throw common::Error( - "Unexpected autoware_adapi_v1_msgs::msg::MrmState::state, number: ", state_number); - } else { - return state->second; - } -} - -auto FieldOperatorApplicationFor::receiveMrmState( - const autoware_adapi_v1_msgs::msg::MrmState & message) -> void -{ - minimum_risk_maneuver_state = toMinimumRiskManeuverStateString(message.state); - - minimum_risk_maneuver_behavior = - toMinimumRiskManeuverBehaviorString(message.behavior); -} } // namespace concealer From 3bfba93d8815d37dc2fedb8e0ab860af5fad0aaa Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 12 Dec 2024 11:14:12 +0900 Subject: [PATCH 107/149] Move data members into base class `FieldOperatorApplication` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 88 +++++++++ ...ator_application_for_autoware_universe.hpp | 172 +----------------- .../concealer/service_with_validation.hpp | 9 +- .../src/field_operator_application.cpp | 76 +++++++- 4 files changed, 169 insertions(+), 176 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 0b572c31665..4eae6602839 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -17,18 +17,40 @@ #include +#if __has_include() +#include +#endif + +#include +#include +#include +#include +#include #include #include +#include #include #include +#include #include +#include +#include #include #include #include #include #include #include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -37,6 +59,47 @@ namespace concealer template class FieldOperatorApplicationFor; +#define DEFINE_STATIC_DATA_MEMBER_DETECTOR(NAME) \ + template \ + struct HasStatic##NAME : public std::false_type \ + { \ + }; \ + \ + template \ + struct HasStatic##NAME> : public std::true_type \ + { \ + } + +DEFINE_STATIC_DATA_MEMBER_DETECTOR(NONE); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(LANE_CHANGE_LEFT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(LANE_CHANGE_RIGHT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_LEFT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_RIGHT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(GOAL_PLANNER); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(START_PLANNER); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(PULL_OUT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(TRAFFIC_LIGHT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(INTERSECTION); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(INTERSECTION_OCCLUSION); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(CROSSWALK); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(BLIND_SPOT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(DETECTION_AREA); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(NO_STOPPING_AREA); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(OCCLUSION_SPOT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_LEFT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_RIGHT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_BY_LC_LEFT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_BY_LC_RIGHT); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(NO_DRIVABLE_LANE); + +DEFINE_STATIC_DATA_MEMBER_DETECTOR(COMFORTABLE_STOP); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(EMERGENCY_STOP); +// DEFINE_STATIC_DATA_MEMBER_DETECTOR(NONE); // NOTE: This is defined above. +DEFINE_STATIC_DATA_MEMBER_DETECTOR(UNKNOWN); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(PULL_OVER); + +#undef DEFINE_STATIC_DATA_MEMBER_DETECTOR + /* ---- NOTE ------------------------------------------------------------------- * * The magic class 'FieldOperatorApplication' is a class that makes it easy to work with @@ -65,8 +128,33 @@ struct FieldOperatorApplication : public rclcpp::Node, std::string autoware_state; + tier4_rtc_msgs::msg::CooperateStatusArray latest_cooperate_status_array; + + std::string minimum_risk_maneuver_state; + + std::string minimum_risk_maneuver_behavior; + // clang-format off SubscriberWrapper getAutowareState; + SubscriberWrapper getCommand; + SubscriberWrapper getCooperateStatusArray; + SubscriberWrapper getEmergencyState; +#if __has_include() + SubscriberWrapper getLocalizationState; +#endif + SubscriberWrapper getMrmState; + SubscriberWrapper getPathWithLaneId; + SubscriberWrapper getTrajectory; + SubscriberWrapper getTurnIndicatorsCommandImpl; + + ServiceWithValidation requestClearRoute; + ServiceWithValidation requestCooperateCommands; + ServiceWithValidation requestEngage; + ServiceWithValidation requestInitialPose; + ServiceWithValidation requestSetRoutePoints; + ServiceWithValidation requestSetRtcAutoMode; + ServiceWithValidation requestSetVelocityLimit; + ServiceWithValidation requestEnableAutowareControl; // clang-format on CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t = 0); diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 952df6a67da..1d4cc1bdc04 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -15,186 +15,16 @@ #ifndef CONCEALER__AUTOWARE_UNIVERSE_USER_HPP_ #define CONCEALER__AUTOWARE_UNIVERSE_USER_HPP_ -#if __has_include() -#include -#endif - -#include -#include -#include -#include -#include -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include namespace concealer { -#define DEFINE_STATIC_DATA_MEMBER_DETECTOR(NAME) \ - template \ - struct HasStatic##NAME : public std::false_type \ - { \ - }; \ - \ - template \ - struct HasStatic##NAME> : public std::true_type \ - { \ - } - -DEFINE_STATIC_DATA_MEMBER_DETECTOR(NONE); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(LANE_CHANGE_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(LANE_CHANGE_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(GOAL_PLANNER); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(START_PLANNER); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(PULL_OUT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(TRAFFIC_LIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(INTERSECTION); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(INTERSECTION_OCCLUSION); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(CROSSWALK); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(BLIND_SPOT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(DETECTION_AREA); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(NO_STOPPING_AREA); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(OCCLUSION_SPOT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_BY_LC_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_BY_LC_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(NO_DRIVABLE_LANE); - -// For MrmState::behavior -DEFINE_STATIC_DATA_MEMBER_DETECTOR(COMFORTABLE_STOP); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(EMERGENCY_STOP); -// DEFINE_STATIC_DATA_MEMBER_DETECTOR(NONE); // NOTE: This is defined above. -DEFINE_STATIC_DATA_MEMBER_DETECTOR(UNKNOWN); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(PULL_OVER); - -#undef DEFINE_STATIC_DATA_MEMBER_DETECTOR - template <> struct FieldOperatorApplicationFor : public FieldOperatorApplication { - // clang-format off - SubscriberWrapper getCommand; - SubscriberWrapper getCooperateStatusArray; - SubscriberWrapper getEmergencyState; -#if __has_include() - SubscriberWrapper getLocalizationState; -#endif - SubscriberWrapper getMrmState; - SubscriberWrapper getPathWithLaneId; - SubscriberWrapper getTrajectory; - SubscriberWrapper getTurnIndicatorsCommandImpl; - - ServiceWithValidation requestClearRoute; - ServiceWithValidation requestCooperateCommands; - ServiceWithValidation requestEngage; - ServiceWithValidation requestInitialPose; - ServiceWithValidation requestSetRoutePoints; - ServiceWithValidation requestSetRtcAutoMode; - ServiceWithValidation requestSetVelocityLimit; - ServiceWithValidation requestEnableAutowareControl; - // clang-format on - - tier4_rtc_msgs::msg::CooperateStatusArray latest_cooperate_status_array; - - std::string minimum_risk_maneuver_state; - - std::string minimum_risk_maneuver_behavior; - template CONCEALER_PUBLIC explicit FieldOperatorApplicationFor(Ts &&... xs) - : FieldOperatorApplication(std::forward(xs)...), - // clang-format off - getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), - getCooperateStatusArray("/api/external/get/rtc_status", rclcpp::QoS(1), *this, [this](const auto & v) { latest_cooperate_status_array = v; }), - getEmergencyState("/api/external/get/emergency", rclcpp::QoS(1), *this, [this](const auto & message) { - if (message.emergency) { - throw common::Error("Emergency state received"); - } - }), -#if __has_include() - getLocalizationState("/api/localization/initialization_state", rclcpp::QoS(1).transient_local(), *this), -#endif - getMrmState("/api/fail_safe/mrm_state", rclcpp::QoS(1), *this, [this](const auto & message) { - auto state_name_of = [](auto state) constexpr { - switch (state) { - case autoware_adapi_v1_msgs::msg::MrmState::MRM_FAILED: - return "MRM_FAILED"; - case autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING: - return "MRM_OPERATING"; - case autoware_adapi_v1_msgs::msg::MrmState::MRM_SUCCEEDED: - return "MRM_SUCCEEDED"; - case autoware_adapi_v1_msgs::msg::MrmState::NORMAL: - return "NORMAL"; - case autoware_adapi_v1_msgs::msg::MrmState::UNKNOWN: - return "UNKNOWN"; - default: - throw common::Error( - "Unexpected autoware_adapi_v1_msgs::msg::MrmState::state, number: ", state); - } - }; - - auto behavior_name_of = [](auto behavior) constexpr { - if constexpr (HasStaticCOMFORTABLE_STOP::value) { - if (behavior == autoware_adapi_v1_msgs::msg::MrmState::COMFORTABLE_STOP) { - return "COMFORTABLE_STOP"; - } - } - if constexpr (HasStaticEMERGENCY_STOP::value) { - if (behavior == autoware_adapi_v1_msgs::msg::MrmState::EMERGENCY_STOP) { - return "EMERGENCY_STOP"; - } - } - if constexpr (HasStaticNONE::value) { - if (behavior == autoware_adapi_v1_msgs::msg::MrmState::NONE) { - return "NONE"; - } - } - if constexpr (HasStaticUNKNOWN::value) { - if (behavior == autoware_adapi_v1_msgs::msg::MrmState::UNKNOWN) { - return "UNKNOWN"; - } - } - if constexpr (HasStaticPULL_OVER::value) { - if (behavior == autoware_adapi_v1_msgs::msg::MrmState::PULL_OVER) { - return "PULL_OVER"; - } - } - throw common::Error( - "Unexpected autoware_adapi_v1_msgs::msg::MrmState::behavior, number: ", behavior); - }; - - minimum_risk_maneuver_state = state_name_of(message.state); - minimum_risk_maneuver_behavior = behavior_name_of(message.behavior); - }), - getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), *this), - getTrajectory("/api/iv_msgs/planning/scenario_planning/trajectory", rclcpp::QoS(1), *this), - getTurnIndicatorsCommandImpl("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), - requestClearRoute("/api/routing/clear_route", *this), - requestCooperateCommands("/api/external/set/rtc_commands", *this), - requestEngage("/api/external/set/engage", *this), - requestInitialPose("/api/localization/initialize", *this), - // NOTE: /api/routing/set_route_points takes a long time to return. But the specified duration is not decided by any technical reasons. - requestSetRoutePoints("/api/routing/set_route_points", *this, std::chrono::seconds(10)), - requestSetRtcAutoMode("/api/external/set/rtc_auto_mode", *this), - requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this), - requestEnableAutowareControl("/api/operation_mode/enable_autoware_control", *this) - // clang-format on + : FieldOperatorApplication(std::forward(xs)...) { } diff --git a/external/concealer/include/concealer/service_with_validation.hpp b/external/concealer/include/concealer/service_with_validation.hpp index 62421299c85..1b01488224e 100644 --- a/external/concealer/include/concealer/service_with_validation.hpp +++ b/external/concealer/include/concealer/service_with_validation.hpp @@ -17,8 +17,8 @@ #include #include -#include #include +#include #include #include #include @@ -58,21 +58,22 @@ template class ServiceWithValidation { public: + template explicit ServiceWithValidation( const std::string & service_name, FieldOperatorApplication & autoware, const std::chrono::nanoseconds validation_interval = std::chrono::seconds(1)) : service_name(service_name), logger(autoware.get_logger()), - client(autoware.create_client(service_name, rmw_qos_profile_default)), + client(autoware.template create_client(service_name, rmw_qos_profile_default)), validation_rate(validation_interval) { } - class TimeoutError : public common::Error + class TimeoutError : public common::scenario_simulator_exception::Error { public: template - explicit TimeoutError(Ts &&... xs) : common::Error(std::forward(xs)...) + explicit TimeoutError(Ts &&... xs) : common::scenario_simulator_exception::Error(std::forward(xs)...) { } }; diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 490346ff106..71af9a2ae8a 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -48,7 +48,81 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) process_id(pid), getAutowareState("/autoware/state", rclcpp::QoS(1), *this, [this](const auto & v) { autoware_state = toAutowareStateString(v.state); - }) + }), + getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), + getCooperateStatusArray("/api/external/get/rtc_status", rclcpp::QoS(1), *this, [this](const auto & v) { latest_cooperate_status_array = v; }), + getEmergencyState("/api/external/get/emergency", rclcpp::QoS(1), *this, [this](const auto & message) { + if (message.emergency) { + throw common::Error("Emergency state received"); + } + }), +#if __has_include() + getLocalizationState("/api/localization/initialization_state", rclcpp::QoS(1).transient_local(), *this), +#endif + getMrmState("/api/fail_safe/mrm_state", rclcpp::QoS(1), *this, [this](const auto & message) { + auto state_name_of = [](auto state) constexpr { + switch (state) { + case autoware_adapi_v1_msgs::msg::MrmState::MRM_FAILED: + return "MRM_FAILED"; + case autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING: + return "MRM_OPERATING"; + case autoware_adapi_v1_msgs::msg::MrmState::MRM_SUCCEEDED: + return "MRM_SUCCEEDED"; + case autoware_adapi_v1_msgs::msg::MrmState::NORMAL: + return "NORMAL"; + case autoware_adapi_v1_msgs::msg::MrmState::UNKNOWN: + return "UNKNOWN"; + default: + throw common::Error( + "Unexpected autoware_adapi_v1_msgs::msg::MrmState::state, number: ", state); + } + }; + + auto behavior_name_of = [](auto behavior) constexpr { + if constexpr (HasStaticCOMFORTABLE_STOP::value) { + if (behavior == autoware_adapi_v1_msgs::msg::MrmState::COMFORTABLE_STOP) { + return "COMFORTABLE_STOP"; + } + } + if constexpr (HasStaticEMERGENCY_STOP::value) { + if (behavior == autoware_adapi_v1_msgs::msg::MrmState::EMERGENCY_STOP) { + return "EMERGENCY_STOP"; + } + } + if constexpr (HasStaticNONE::value) { + if (behavior == autoware_adapi_v1_msgs::msg::MrmState::NONE) { + return "NONE"; + } + } + if constexpr (HasStaticUNKNOWN::value) { + if (behavior == autoware_adapi_v1_msgs::msg::MrmState::UNKNOWN) { + return "UNKNOWN"; + } + } + if constexpr (HasStaticPULL_OVER::value) { + if (behavior == autoware_adapi_v1_msgs::msg::MrmState::PULL_OVER) { + return "PULL_OVER"; + } + } + throw common::Error( + "Unexpected autoware_adapi_v1_msgs::msg::MrmState::behavior, number: ", behavior); + }; + + minimum_risk_maneuver_state = state_name_of(message.state); + minimum_risk_maneuver_behavior = behavior_name_of(message.behavior); + }), + getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), *this), + getTrajectory("/api/iv_msgs/planning/scenario_planning/trajectory", rclcpp::QoS(1), *this), + getTurnIndicatorsCommandImpl("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), + requestClearRoute("/api/routing/clear_route", *this), + requestCooperateCommands("/api/external/set/rtc_commands", *this), + requestEngage("/api/external/set/engage", *this), + requestInitialPose("/api/localization/initialize", *this), + // NOTE: /api/routing/set_route_points takes a long time to return. But the specified duration is not decided by any technical reasons. + requestSetRoutePoints("/api/routing/set_route_points", *this, std::chrono::seconds(10)), + requestSetRtcAutoMode("/api/external/set/rtc_auto_mode", *this), + requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this), + requestEnableAutowareControl("/api/operation_mode/enable_autoware_control", *this) { } From b2a1b1e715950c69594b9d8cae40944a6c27ab8b Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 12 Dec 2024 12:00:03 +0900 Subject: [PATCH 108/149] Remove static member function `evaluateTimeToCollisionCondition` Signed-off-by: yamacir-kit --- .../simulator_core.hpp | 145 ------------------ .../syntax/time_to_collision_condition.hpp | 33 ++-- ...ityCondition.TimeToCollisionCondition.yaml | 48 ------ 3 files changed, 12 insertions(+), 214 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 8bf5f664b3b..5252176abfc 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -620,151 +620,6 @@ class SimulatorCore return std::numeric_limits::quiet_NaN(); } } - - static auto evaluateTimeToCollisionCondition(const Entity & from, const Entity & to) - { - /* - This function uses the Separating Axis Theorem (SAT) to find the - Time-To-Collision (TTC) of two entities in the world coordinate - system. - - First, the expected time of collision is obtained independently for - each separating axis. Then, we check whether the polygons of the - entities actually collide after that time. If no collision occurs, the - expected collision time is rejected. The lowest value of the expected - collision times obtained for each separate axis is then returned as - the TTC. - */ - - if (const auto entity_a = core->getEntity(from.name())) { - if (const auto entity_b = core->getEntity(to.name())) { - struct OrientedBoundingBox - { - Eigen::Vector3d position; - - Eigen::Matrix3d rotation; - - Eigen::Vector3d size; // [width, height, depth] - - explicit OrientedBoundingBox( - const geometry_msgs::msg::Pose & pose, - const traffic_simulator_msgs::msg::BoundingBox & box) - : position( - pose.position.x + box.center.x, pose.position.y + box.center.y, - pose.position.z + box.center.z), - rotation(math::geometry::getRotationMatrix(pose.orientation)), - size(box.dimensions.y / 2, box.dimensions.z / 2, box.dimensions.x / 2) - { - } - - auto axis(std::size_t index) const -> Eigen::Vector3d { return rotation.col(index); } - }; - - const Eigen::Vector3d v = evaluateRelativeSpeed(from, to); - - const auto a = OrientedBoundingBox(entity_a->getMapPose(), entity_a->getBoundingBox()); - const auto b = OrientedBoundingBox(entity_b->getMapPose(), entity_b->getBoundingBox()); - - const auto axes = std::array{ - a.axis(0), - a.axis(1), - a.axis(2), - - b.axis(0), - b.axis(1), - b.axis(2), - - a.axis(0).cross(b.axis(0)), - a.axis(0).cross(b.axis(1)), - a.axis(0).cross(b.axis(2)), - - a.axis(1).cross(b.axis(0)), - a.axis(1).cross(b.axis(1)), - a.axis(1).cross(b.axis(2)), - - a.axis(2).cross(b.axis(0)), - a.axis(2).cross(b.axis(1)), - a.axis(2).cross(b.axis(2)), - }; - - Eigen::Vector3d d = b.position - a.position; - - auto t_min = std::numeric_limits::infinity(); - - auto projection = [&](const auto & box, const auto & axis) { - return std::abs(axis.dot(box.axis(0)) * box.size(0)) + - std::abs(axis.dot(box.axis(1)) * box.size(1)) + - std::abs(axis.dot(box.axis(2)) * box.size(2)); - }; - - for (const auto & axis : axes) { - /* - The variable `separation` means how far apart two Oriented - Bounding Boxes (OBBs) are on the separation axis. When it is - positive, it means they are separated by `separation`, i.e. the - OBBs do not overlap on the separation axis. When it is less than - zero, it means the OBBs overlap on the separation axis. - - d.dot(axis) is the projection of the center distance between the - two OBBs onto the separation axis, and projection(a, axis) and - projection(b, axis) are half the projected range of the boxes - when the two OBBs are projected onto the separation axis. - */ - if (const auto separation = - std::abs(d.dot(axis)) - projection(a, axis) - projection(b, axis); - 0 < separation) { - /* - v.dot(axis) is the projection of the relative distance onto - the separation axis. In other words, we want to find the TTC - for each separation axis. The relative velocity is the - velocity vector of the observed object measured in the - observer's rest frame, so a negative value means that the two - OBBs are approaching each other. - */ - if (const auto relative_speed = v.dot(axis); relative_speed < 0) { - /* - Since this t is only a Time-To-Collision on this separation - axis, it is necessary to check that a and b are not - separated on all separation axes after t seconds. - */ - const auto t = separation / -relative_speed; - - const auto a_t = a; - - const auto b_t = [&]() { - /* - Since t is the exact time when contact occurs, it may not - be possible to say that a and b are in collision after t - seconds. Therefore, 0.01 seconds is added to t so that - collision can be expected to occur with certainty. This - value "0.01" has no technical basis and is an arbitrary - value. - */ - constexpr auto tolerance = 0.01; - auto copy = b; - copy.position += v * (t + tolerance); - return copy; - }(); - - Eigen::Vector3d d_t = b_t.position - a_t.position; - - if (std::all_of(axes.begin(), axes.end(), [&](const auto & axis) { - const auto separation_t = - std::abs(d_t.dot(axis)) - projection(a_t, axis) - projection(b_t, axis); - return separation_t <= 0; - })) { - t_min = std::min(t_min, t); - } - } - } - } - - return t_min; - } - } - - return std::numeric_limits::quiet_NaN(); - } }; class NonStandardOperation : private CoordinateSystemConversion diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index 17a34204c03..ff290830df5 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -141,19 +141,18 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio } }; - auto directional_dimension = [&]() { - switch (relative_distance_type) { - case RelativeDistanceType::longitudinal: - return std::optional(DirectionalDimension::longitudinal); - case RelativeDistanceType::lateral: - return std::optional(DirectionalDimension::lateral); - default: - case RelativeDistanceType::euclidianDistance: - return std::optional(std::nullopt); - }; - }; - auto speed = [&]() { + auto directional_dimension = [&]() { + switch (relative_distance_type) { + case RelativeDistanceType::longitudinal: + return std::optional(DirectionalDimension::longitudinal); + case RelativeDistanceType::lateral: + return std::optional(DirectionalDimension::lateral); + default: + case RelativeDistanceType::euclidianDistance: + return std::optional(std::nullopt); + }; + }; if (time_to_collision_condition_target.is()) { return RelativeSpeedCondition::evaluate( entities, triggering_entity, time_to_collision_condition_target.as(), @@ -163,15 +162,7 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio } }; - if ( - time_to_collision_condition_target.is() and - coordinate_system == CoordinateSystem::entity and - relative_distance_type == RelativeDistanceType::euclidianDistance and freespace) { - return evaluateTimeToCollisionCondition( - triggering_entity, time_to_collision_condition_target.as()); - } else { - return distance() / std::max(speed(), 0.0); - } + return distance() / std::max(speed(), 0.0); } auto evaluate() diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml index 555aad18bad..213ac429acd 100644 --- a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml +++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml @@ -25,13 +25,6 @@ OpenSCENARIO: name: '' Properties: Property: [] - - name: vehicle_02 - CatalogReference: { catalogName: sample_vehicle, entryName: sample_vehicle } - ObjectController: - Controller: - name: '' - Properties: - Property: [] Storyboard: Init: Actions: @@ -72,28 +65,6 @@ OpenSCENARIO: Property: - name: maxSpeed value: '0.5' - - entityRef: vehicle_02 - PrivateAction: - - TeleportAction: - Position: - LanePosition: - roadId: '' - laneId: '34576' - s: 0 - offset: 0 - Orientation: - type: relative - h: 0 - p: 0 - r: 0 - - ControllerAction: - AssignControllerAction: - Controller: - name: '' - Properties: - Property: - - name: maxSpeed - value: '2' # If 2.5, collision will occur. Story: - name: '' Act: @@ -245,25 +216,6 @@ OpenSCENARIO: TimeToCollisionConditionTarget: EntityRef: entityRef: vehicle_01 - - Condition: - - name: '' - delay: 0 - conditionEdge: none - ByEntityCondition: - TriggeringEntities: - triggeringEntitiesRule: any - EntityRef: [ entityRef: ego ] - EntityCondition: - TimeToCollisionCondition: - freespace: true - rule: lessThan - value: 0 - relativeDistanceType: cartesianDistance - coordinateSystem: entity - routingAlgorithm: undefined - TimeToCollisionConditionTarget: - EntityRef: - entityRef: vehicle_02 Action: - name: '' UserDefinedAction: From 081643a1213c83ad9da54f47c136121a6a64f340 Mon Sep 17 00:00:00 2001 From: Release Bot Date: Thu, 12 Dec 2024 04:11:27 +0000 Subject: [PATCH 109/149] Bump version of scenario_simulator_v2 from version 7.0.1 to version 7.0.2 --- common/math/arithmetic/CHANGELOG.rst | 8 ++++++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 13 +++++++++++++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 8 ++++++++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 8 ++++++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 8 ++++++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 8 ++++++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 8 ++++++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 8 ++++++++ map/kashiwanoha_map/package.xml | 2 +- map/simple_cross_map/CHANGELOG.rst | 8 ++++++++ map/simple_cross_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 8 ++++++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../openscenario_experimental_catalog/CHANGELOG.rst | 8 ++++++++ .../openscenario_experimental_catalog/package.xml | 2 +- openscenario/openscenario_interpreter/CHANGELOG.rst | 8 ++++++++ openscenario/openscenario_interpreter/package.xml | 2 +- .../openscenario_interpreter_example/CHANGELOG.rst | 8 ++++++++ .../openscenario_interpreter_example/package.xml | 2 +- .../openscenario_interpreter_msgs/CHANGELOG.rst | 8 ++++++++ .../openscenario_interpreter_msgs/package.xml | 2 +- .../openscenario_preprocessor/CHANGELOG.rst | 8 ++++++++ openscenario/openscenario_preprocessor/package.xml | 2 +- .../openscenario_preprocessor_msgs/CHANGELOG.rst | 8 ++++++++ .../openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 8 ++++++++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 8 ++++++++ openscenario/openscenario_validator/package.xml | 2 +- .../openscenario_visualization/CHANGELOG.rst | 12 ++++++++++++ rviz_plugins/openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 8 ++++++++ .../package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 8 ++++++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 8 ++++++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 8 ++++++++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 8 ++++++++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 8 ++++++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 12 ++++++++++++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 8 ++++++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 8 ++++++++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 8 ++++++++ test_runner/scenario_test_runner/package.xml | 2 +- 58 files changed, 274 insertions(+), 29 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 11a6fa0e40e..b664724b675 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 5707c36b6fd..908b76bdea3 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 7.0.1 + 7.0.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 4dc5aaf01b7..398d518f7aa 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,19 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge pull request `#1470 `_ from tier4/fix/snor-cloud-issue-8-1 + Fix/sonor cloud issue 8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Fixed bugs and added comments. +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Remove the commented out code. + Replace this declaration by a structured binding declaration. +* Contributors: Masaya Kataoka, Taiga, Taiga Takano + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 57b9100a3eb..5116ae32538 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 7.0.1 + 7.0.2 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 11093af028b..cf3bc3adf29 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 4bf670df97d..a005e5748d6 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 7.0.1 + 7.0.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 0e2676c8a60..4a6b3ee0902 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 2e2e8f96fe1..25a635b3db9 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 7.0.1 + 7.0.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 89b829384f6..8fe30da0cf0 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 2884cbb75f9..59d9d6502a2 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 7.0.1 + 7.0.2 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index c826ef2e84b..c9ec6fde662 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 06550716cb6..a5971def2e4 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 7.0.1 + 7.0.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 fb4759552b2..6661d8bf53c 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 3f2966bfff6..8acfa0e4a14 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 7.0.1 + 7.0.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 893a9858a22..db775da252e 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 785151a0a68..540a6e44ea6 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 7.0.1 + 7.0.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 ecfba00ef7f..9114c7740ce 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,14 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index 46c2aacd5fb..0303b2c1d0e 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 7.0.1 + 7.0.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 ebf606436c3..f493b68bfa7 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index c131f71f45c..175546f8e77 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 7.0.1 + 7.0.2 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index d50a2494615..4d0dcabb2da 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index e58170312db..31f6f183bb3 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 7.0.1 + 7.0.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 91633085118..813f25e053e 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,14 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge pull request `#1462 `_ from tier4/feature/act-starttrigger-optional diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index a436602f709..c7d497d8741 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 7.0.1 + 7.0.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 8f55fa25ee3..a600ece6a67 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index b9642b3aca0..68ff65fc757 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 7.0.1 + 7.0.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 882a29dc4f4..47881a2a88a 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 6ab00575b6c..057014f0ee9 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 7.0.1 + 7.0.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 44086f88ca5..a1014e65fde 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 1304d3a2b95..834978f29f0 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 7.0.1 + 7.0.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 ac63b6c22ec..1f31a52ab4f 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index bbd9db7f3b7..d94c408de3e 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 7.0.1 + 7.0.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 143d4971857..b7d93c751f1 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 76e182f4fed..b4f51b923e4 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 7.0.1 + 7.0.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 71229bc2469..c58b60badaa 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,14 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 7fc225093da..a5dda80e4f9 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 7.0.1 + 7.0.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 80e709cb026..608a87963bf 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge pull request `#1470 `_ from tier4/fix/snor-cloud-issue-8-1 + Fix/sonor cloud issue 8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Remove the commented out code. + Replace this declaration by a structured binding declaration. +* Contributors: Masaya Kataoka, Taiga, Taiga Takano + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index d5803df72e6..2c596393415 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 7.0.1 + 7.0.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 f7f2d79f3c3..7bc21f4ad87 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,14 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional 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 f30ad193f67..8f0243c3744 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 - 7.0.1 + 7.0.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 d40187b2501..84fdea0f5e7 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 8a892894a0d..35edc1a1a53 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 7.0.1 + 7.0.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 9f04c12db37..06d8350c0db 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 09b64c905fe..9be02b547d4 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 7.0.1 + 7.0.2 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index cfc854c265c..57a85dbd380 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index e9938b62acb..9fc2c66d90f 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 7.0.1 + 7.0.2 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 977a08d8ecc..d30b0cc1d6f 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index e3dfe78d048..bb558e2d07a 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 7.0.1 + 7.0.2 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index b1d510a9578..695f680fc6e 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 04d18defa97..21bee384cc4 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 7.0.1 + 7.0.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 f61e96c4a38..525db7453f5 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge pull request `#1470 `_ from tier4/fix/snor-cloud-issue-8-1 + Fix/sonor cloud issue 8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Remove the commented out code. + Replace this declaration by a structured binding declaration. +* Contributors: Masaya Kataoka, Taiga, Taiga Takano + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 7427a58b2ad..19e650a4e2d 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 7.0.1 + 7.0.2 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 054c854cc40..63e1f6fcc89 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index ecc189edecd..d2c17555369 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 7.0.1 + 7.0.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 dc2296feba0..4340d7352b4 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index a4d4cdfd2b8..8f1a0332630 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 7.0.1 + 7.0.2 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index e3c9a70f4d4..39c8efa3a9e 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,14 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.2 (2024-12-12) +------------------ +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Merge branch 'master' into fix/snor-cloud-issue-8-1 +* Contributors: Masaya Kataoka, Taiga + 7.0.1 (2024-12-11) ------------------ * Merge branch 'master' into feature/act-starttrigger-optional diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 9fa9dac0716..f95ecbcf47a 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 7.0.1 + 7.0.2 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 82b72ce35fc57a451b8c0cdb0f57d03f96f6ce2f Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 12 Dec 2024 15:42:03 +0900 Subject: [PATCH 110/149] Add new enumeration `Compatibility` Signed-off-by: yamacir-kit --- .../compatibility.hpp | 30 +++++++++++++++ .../src/compatibility.cpp | 38 +++++++++++++++++++ .../launch/scenario_test_runner.launch.py | 2 +- 3 files changed, 69 insertions(+), 1 deletion(-) create mode 100644 openscenario/openscenario_interpreter/include/openscenario_interpreter/compatibility.hpp create mode 100644 openscenario/openscenario_interpreter/src/compatibility.cpp diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/compatibility.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/compatibility.hpp new file mode 100644 index 00000000000..683ba6f2377 --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/compatibility.hpp @@ -0,0 +1,30 @@ +// 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 OPENSCENARIO_INTERPRETER__COMPATIBILITY_HPP_ +#define OPENSCENARIO_INTERPRETER__COMPATIBILITY_HPP_ + +#include + +namespace openscenario_interpreter +{ +enum class Compatibility { + legacy, + standard, +}; + +auto operator>>(std::istream &, Compatibility &) -> std::istream &; +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__COMPATIBILITY_HPP_ diff --git a/openscenario/openscenario_interpreter/src/compatibility.cpp b/openscenario/openscenario_interpreter/src/compatibility.cpp new file mode 100644 index 00000000000..617a5bcc08b --- /dev/null +++ b/openscenario/openscenario_interpreter/src/compatibility.cpp @@ -0,0 +1,38 @@ +// 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 + +namespace openscenario_interpreter +{ +auto operator>>(std::istream & input, Compatibility & compatiblity) -> std::istream & +{ + if (auto token = std::string(); input >> token) { + if (token == "legacy") { + compatiblity = Compatibility::legacy; + return input; + } else if (token == "standard") { + compatiblity = Compatibility::standard; + return input; + } else { + throw Error( + "Unknown compatiblity ", std::quoted(token), + " was specified. It must be \"legacy\" or \"standard\"."); + } + } else { + compatiblity = Compatibility::legacy; + return input; + } +} +} // namespace openscenario_interpreter 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 98897b9badf..ba38750a462 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 @@ -173,8 +173,8 @@ def collect_prefixed_parameters(): DeclareLaunchArgument("global_timeout", default_value=global_timeout ), DeclareLaunchArgument("launch_autoware", default_value=launch_autoware ), DeclareLaunchArgument("launch_rviz", default_value=launch_rviz ), - DeclareLaunchArgument("publish_empty_context", default_value=publish_empty_context ), DeclareLaunchArgument("output_directory", default_value=output_directory ), + DeclareLaunchArgument("publish_empty_context", default_value=publish_empty_context ), DeclareLaunchArgument("rviz_config", default_value=rviz_config ), DeclareLaunchArgument("scenario", default_value=scenario ), DeclareLaunchArgument("sensor_model", default_value=sensor_model ), From 078581360b692e4ddc72d80b50d159ec26130707 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 12 Dec 2024 16:19:24 +0900 Subject: [PATCH 111/149] Add new parameter `speed_condition` to switch compatibility Signed-off-by: yamacir-kit --- .../scenario_simulator_exception/concatenate.hpp | 6 +----- .../openscenario_interpreter/CMakeLists.txt | 3 ++- .../syntax/speed_condition.hpp | 6 +++++- .../src/openscenario_interpreter.cpp | 5 +++++ .../src/syntax/speed_condition.cpp | 13 ++++++++++--- .../launch/scenario_test_runner.launch.py | 4 ++++ 6 files changed, 27 insertions(+), 10 deletions(-) diff --git a/common/scenario_simulator_exception/include/scenario_simulator_exception/concatenate.hpp b/common/scenario_simulator_exception/include/scenario_simulator_exception/concatenate.hpp index 0e967d66821..efb9139b7e2 100644 --- a/common/scenario_simulator_exception/include/scenario_simulator_exception/concatenate.hpp +++ b/common/scenario_simulator_exception/include/scenario_simulator_exception/concatenate.hpp @@ -25,12 +25,8 @@ namespace common inline namespace scenario_simulator_exception { inline auto concatenate = [](auto &&... xs) { - auto write = [](auto && os, auto && x) { - os.get() << std::forward(x); - return std::forward(os); - }; std::stringstream result; - fold_left(write, std::ref(result), std::forward(xs)...); + (result << ... << std::forward(xs)); return result.str(); }; } // namespace scenario_simulator_exception diff --git a/openscenario/openscenario_interpreter/CMakeLists.txt b/openscenario/openscenario_interpreter/CMakeLists.txt index 72ac634e062..a1d95ea6350 100644 --- a/openscenario/openscenario_interpreter/CMakeLists.txt +++ b/openscenario/openscenario_interpreter/CMakeLists.txt @@ -59,8 +59,9 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_POSIX_SOURCES} ${${PROJECT_NAME}_SYNTAX_SOURCES} ${${PROJECT_NAME}_UTILITY_SOURCES} - src/object.cpp + src/compatibility.cpp src/evaluate.cpp + src/object.cpp src/openscenario_interpreter.cpp src/record.cpp src/scope.cpp) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp index 9c2bf1ffb92..f64c7499051 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp @@ -15,6 +15,7 @@ #ifndef OPENSCENARIO_INTERPRETER__SYNTAX__SPEED_CONDITION_HPP_ #define OPENSCENARIO_INTERPRETER__SYNTAX__SPEED_CONDITION_HPP_ +#include #include #include #include @@ -63,6 +64,8 @@ struct SpeedCondition : private Scope, private SimulatorCore::ConditionEvaluatio std::vector> results; // for description + static inline auto compatibility = Compatibility::legacy; + explicit SpeedCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); auto description() const -> String; @@ -70,7 +73,8 @@ struct SpeedCondition : private Scope, private SimulatorCore::ConditionEvaluatio static auto evaluate(const Entities *, const Entity &) -> Eigen::Vector3d; static auto evaluate( - const Entities *, const Entity &, const std::optional &) -> double; + const Entities *, const Entity &, const std::optional &, + const Compatibility = Compatibility::legacy) -> double; auto evaluate() -> Object; }; diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index d2f7c910cc0..824c83280f3 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -49,6 +50,10 @@ Interpreter::Interpreter(const rclcpp::NodeOptions & options) DECLARE_PARAMETER(output_directory); DECLARE_PARAMETER(publish_empty_context); DECLARE_PARAMETER(record); + + declare_parameter("speed_condition", "legacy"); + SpeedCondition::compatibility = + boost::lexical_cast(get_parameter("speed_condition").as_string()); } Interpreter::~Interpreter() {} diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp index cc8d64b0c41..5e2f8e34038 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp @@ -58,7 +58,8 @@ auto SpeedCondition::evaluate(const Entities * entities, const Entity & triggeri auto SpeedCondition::evaluate( const Entities * entities, const Entity & triggering_entity, - const std::optional & direction) -> double + const std::optional & direction, const Compatibility compatibility) + -> double { if (const Eigen::Vector3d v = evaluate(entities, triggering_entity); direction) { switch (*direction) { @@ -71,7 +72,13 @@ auto SpeedCondition::evaluate( return v.z(); } } else { - return v.norm(); + switch (compatibility) { + default: + case Compatibility::legacy: + return v.x(); + case Compatibility::standard: + return v.norm(); + } } } @@ -81,7 +88,7 @@ auto SpeedCondition::evaluate() -> Object return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) { results.push_back(triggering_entity.apply([&](const auto & triggering_entity) { - return evaluate(global().entities, triggering_entity, direction); + return evaluate(global().entities, triggering_entity, direction, compatibility); })); return not results.back().size() or std::invoke(rule, results.back(), value).min(); })); 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 ba38750a462..fa1492c172c 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 @@ -87,6 +87,7 @@ def launch_setup(context, *args, **kwargs): scenario = LaunchConfiguration("scenario", default=Path("/dev/null")) sensor_model = LaunchConfiguration("sensor_model", default="") sigterm_timeout = LaunchConfiguration("sigterm_timeout", default=8) + speed_condition = LaunchConfiguration("speed_condition", default="legacy") use_sim_time = LaunchConfiguration("use_sim_time", default=False) vehicle_model = LaunchConfiguration("vehicle_model", default="") # fmt: on @@ -111,6 +112,7 @@ def launch_setup(context, *args, **kwargs): print(f"scenario := {scenario.perform(context)}") print(f"sensor_model := {sensor_model.perform(context)}") print(f"sigterm_timeout := {sigterm_timeout.perform(context)}") + print(f"speed_condition := {speed_condition.perform(context)}") print(f"use_sim_time := {use_sim_time.perform(context)}") print(f"vehicle_model := {vehicle_model.perform(context)}") @@ -135,6 +137,7 @@ def make_parameters(): {"rviz_config": rviz_config}, {"sensor_model": sensor_model}, {"sigterm_timeout": sigterm_timeout}, + {"speed_condition": speed_condition}, {"use_sim_time": use_sim_time}, {"vehicle_model": vehicle_model}, ] @@ -179,6 +182,7 @@ def collect_prefixed_parameters(): DeclareLaunchArgument("scenario", default_value=scenario ), DeclareLaunchArgument("sensor_model", default_value=sensor_model ), DeclareLaunchArgument("sigterm_timeout", default_value=sigterm_timeout ), + DeclareLaunchArgument("speed_condition", default_value=speed_condition ), DeclareLaunchArgument("use_sim_time", default_value=use_sim_time ), DeclareLaunchArgument("vehicle_model", default_value=vehicle_model ), # fmt: on From 7c4e369d2b1af0e009332ef80237eb09fcf5c01b Mon Sep 17 00:00:00 2001 From: Release Bot Date: Fri, 13 Dec 2024 01:03:45 +0000 Subject: [PATCH 112/149] Bump version of scenario_simulator_v2 from version 7.0.2 to version 7.0.3 --- common/math/arithmetic/CHANGELOG.rst | 8 ++++++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 8 ++++++++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 8 ++++++++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 8 ++++++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 8 ++++++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 10 ++++++++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 8 ++++++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 8 ++++++++ map/kashiwanoha_map/package.xml | 2 +- map/simple_cross_map/CHANGELOG.rst | 8 ++++++++ map/simple_cross_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 8 ++++++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../openscenario_experimental_catalog/CHANGELOG.rst | 8 ++++++++ .../openscenario_experimental_catalog/package.xml | 2 +- openscenario/openscenario_interpreter/CHANGELOG.rst | 8 ++++++++ openscenario/openscenario_interpreter/package.xml | 2 +- .../openscenario_interpreter_example/CHANGELOG.rst | 8 ++++++++ .../openscenario_interpreter_example/package.xml | 2 +- .../openscenario_interpreter_msgs/CHANGELOG.rst | 8 ++++++++ openscenario/openscenario_interpreter_msgs/package.xml | 2 +- openscenario/openscenario_preprocessor/CHANGELOG.rst | 8 ++++++++ openscenario/openscenario_preprocessor/package.xml | 2 +- .../openscenario_preprocessor_msgs/CHANGELOG.rst | 8 ++++++++ .../openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 8 ++++++++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 8 ++++++++ openscenario/openscenario_validator/package.xml | 2 +- rviz_plugins/openscenario_visualization/CHANGELOG.rst | 8 ++++++++ rviz_plugins/openscenario_visualization/package.xml | 2 +- .../real_time_factor_control_rviz_plugin/CHANGELOG.rst | 8 ++++++++ .../real_time_factor_control_rviz_plugin/package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 8 ++++++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 8 ++++++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 8 ++++++++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 8 ++++++++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 8 ++++++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 10 ++++++++++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 8 ++++++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 8 ++++++++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 8 ++++++++ test_runner/scenario_test_runner/package.xml | 2 +- 58 files changed, 265 insertions(+), 29 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index b664724b675..b170351db24 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 908b76bdea3..18994179744 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 7.0.2 + 7.0.3 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index 398d518f7aa..be0bafab44c 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge pull request `#1470 `_ from tier4/fix/snor-cloud-issue-8-1 diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 5116ae32538..b46f86bb4e6 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 7.0.2 + 7.0.3 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index cf3bc3adf29..a6808384409 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index a005e5748d6..7daa737d657 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 7.0.2 + 7.0.3 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index 4a6b3ee0902..5356eb816b0 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 25a635b3db9..c364b08669c 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 7.0.2 + 7.0.3 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index 8fe30da0cf0..13ea3091a5c 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 59d9d6502a2..0f936dcaaad 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 7.0.2 + 7.0.3 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index c9ec6fde662..522e1395c67 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,16 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge pull request `#1465 `_ from tier4/fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* fix(concealer): increase max time to request enable autoware control +* Contributors: Kotaro Yoshimoto, satoshi-ota + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/external/concealer/package.xml b/external/concealer/package.xml index a5971def2e4..6807ea82e2a 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 7.0.2 + 7.0.3 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 6661d8bf53c..db9fffa20db 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 8acfa0e4a14..ac7bbc4d659 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 7.0.2 + 7.0.3 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index db775da252e..9c17f9e8a8b 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 540a6e44ea6..0696ea4827d 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 7.0.2 + 7.0.3 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 9114c7740ce..112a45e7580 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,14 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index 0303b2c1d0e..484d8714faa 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 7.0.2 + 7.0.3 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 f493b68bfa7..c66a4cc76cf 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 175546f8e77..92f9ba96f88 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 7.0.2 + 7.0.3 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 4d0dcabb2da..3b56d58375c 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 31f6f183bb3..4c9ed5482c3 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 7.0.2 + 7.0.3 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 813f25e053e..36d4f266144 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,14 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index c7d497d8741..e9fbe1d3598 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 7.0.2 + 7.0.3 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index a600ece6a67..3038b9e4678 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 68ff65fc757..4a31ce56fd8 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 7.0.2 + 7.0.3 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index 47881a2a88a..f45a5099e19 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 057014f0ee9..56a95035ba2 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 7.0.2 + 7.0.3 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index a1014e65fde..674191360dc 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 834978f29f0..e91d6109b6e 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 7.0.2 + 7.0.3 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 1f31a52ab4f..4d4702d540a 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index d94c408de3e..d044251414e 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 7.0.2 + 7.0.3 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index b7d93c751f1..06c7e90f7dc 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index b4f51b923e4..b8c23e528f9 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 7.0.2 + 7.0.3 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index c58b60badaa..68c811fe173 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,14 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index a5dda80e4f9..19840b64144 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 7.0.2 + 7.0.3 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 608a87963bf..f53adfee34f 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge pull request `#1470 `_ from tier4/fix/snor-cloud-issue-8-1 diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 2c596393415..a02db4e9fbd 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 7.0.2 + 7.0.3 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 7bc21f4ad87..fabf001a247 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,14 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 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 8f0243c3744..bce21f3ee2e 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 - 7.0.2 + 7.0.3 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 84fdea0f5e7..bace7285cff 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 35edc1a1a53..54211af4af7 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 7.0.2 + 7.0.3 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 06d8350c0db..c03dd28c38d 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 9be02b547d4..640aea992d0 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 7.0.2 + 7.0.3 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 57a85dbd380..a2df026aa25 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 9fc2c66d90f..937e3a5441e 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 7.0.2 + 7.0.3 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index d30b0cc1d6f..6727d05b83f 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index bb558e2d07a..1e1d5e3cb84 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 7.0.2 + 7.0.3 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 695f680fc6e..26d57436298 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 21bee384cc4..046efa14c6a 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 7.0.2 + 7.0.3 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index 525db7453f5..a3e40444e82 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,16 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge pull request `#1465 `_ from tier4/fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* fix(api): request enable autoware control +* Contributors: Kotaro Yoshimoto, satoshi-ota + 7.0.2 (2024-12-12) ------------------ * Merge pull request `#1470 `_ from tier4/fix/snor-cloud-issue-8-1 diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 19e650a4e2d..d3ceb5ed4f8 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 7.0.2 + 7.0.3 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 63e1f6fcc89..11b944a5910 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index d2c17555369..a872b522734 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 7.0.2 + 7.0.3 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 4340d7352b4..bb5ba5043d1 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,14 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 8f1a0332630..0d8796c11b3 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 7.0.2 + 7.0.3 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 39c8efa3a9e..150fe57fb3b 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,14 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index f95ecbcf47a..419a1b7fc01 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 7.0.2 + 7.0.3 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 3255cef0b23139e44e7ceeb19da7bd8b96ee8038 Mon Sep 17 00:00:00 2001 From: Release Bot Date: Fri, 13 Dec 2024 03:27:58 +0000 Subject: [PATCH 113/149] Bump version of scenario_simulator_v2 from version 7.0.3 to version 7.0.4 --- common/math/arithmetic/CHANGELOG.rst | 6 ++++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 6 ++++++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 9 +++++++++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 6 ++++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 6 ++++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 6 ++++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 6 ++++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 6 ++++++ map/kashiwanoha_map/package.xml | 2 +- map/simple_cross_map/CHANGELOG.rst | 6 ++++++ map/simple_cross_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 6 ++++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../openscenario_experimental_catalog/CHANGELOG.rst | 6 ++++++ .../openscenario_experimental_catalog/package.xml | 2 +- openscenario/openscenario_interpreter/CHANGELOG.rst | 10 ++++++++++ openscenario/openscenario_interpreter/package.xml | 2 +- .../openscenario_interpreter_example/CHANGELOG.rst | 6 ++++++ .../openscenario_interpreter_example/package.xml | 2 +- .../openscenario_interpreter_msgs/CHANGELOG.rst | 6 ++++++ openscenario/openscenario_interpreter_msgs/package.xml | 2 +- openscenario/openscenario_preprocessor/CHANGELOG.rst | 6 ++++++ openscenario/openscenario_preprocessor/package.xml | 2 +- .../openscenario_preprocessor_msgs/CHANGELOG.rst | 6 ++++++ .../openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 6 ++++++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 6 ++++++ openscenario/openscenario_validator/package.xml | 2 +- rviz_plugins/openscenario_visualization/CHANGELOG.rst | 6 ++++++ rviz_plugins/openscenario_visualization/package.xml | 2 +- .../real_time_factor_control_rviz_plugin/CHANGELOG.rst | 6 ++++++ .../real_time_factor_control_rviz_plugin/package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 6 ++++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 6 ++++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 6 ++++++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 6 ++++++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 6 ++++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 6 ++++++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 6 ++++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 6 ++++++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 10 ++++++++++ test_runner/scenario_test_runner/package.xml | 2 +- 58 files changed, 214 insertions(+), 29 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index b170351db24..4bb19449115 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 18994179744..15abfa02a91 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 7.0.3 + 7.0.4 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index be0bafab44c..7978b669400 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index b46f86bb4e6..49467cbde14 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 7.0.3 + 7.0.4 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index a6808384409..fb9d82f2305 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge pull request `#1486 `_ from tier4/fix/speed-condition/backward-compatibility + Fix/speed condition/backward compatibility +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Add new parameter `speed_condition` to switch compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 7daa737d657..d82aec6de9a 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 7.0.3 + 7.0.4 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index 5356eb816b0..b460aa61a8e 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index c364b08669c..0453c2ae9b0 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 7.0.3 + 7.0.4 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index 13ea3091a5c..d83fa605d55 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 0f936dcaaad..30e322eb56e 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 7.0.3 + 7.0.4 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 522e1395c67..d1cd7fcce35 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge pull request `#1465 `_ from tier4/fix/request-enable-autoware-control diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 6807ea82e2a..703a5b23184 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 7.0.3 + 7.0.4 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index db9fffa20db..1f1f5bc5223 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,12 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index ac7bbc4d659..f68ffcd128d 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 7.0.3 + 7.0.4 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 9c17f9e8a8b..852f27c951a 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 0696ea4827d..c111cd34b59 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 7.0.3 + 7.0.4 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 112a45e7580..2316cf6c77b 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,12 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index 484d8714faa..2f55a6f9426 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 7.0.3 + 7.0.4 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 c66a4cc76cf..a2e4a96afae 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 92f9ba96f88..78f8a2a45fd 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 7.0.3 + 7.0.4 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 3b56d58375c..874579d5052 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 4c9ed5482c3..773bfe663d2 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 7.0.3 + 7.0.4 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 36d4f266144..5adaf81ff87 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,16 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge pull request `#1486 `_ from tier4/fix/speed-condition/backward-compatibility + Fix/speed condition/backward compatibility +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Add new parameter `speed_condition` to switch compatibility +* Add new enumeration `Compatibility` +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index e9fbe1d3598..0826de6a98e 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 7.0.3 + 7.0.4 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 3038b9e4678..c59120080dc 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 4a31ce56fd8..6ef4c36cf6b 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 7.0.3 + 7.0.4 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index f45a5099e19..b7582bd5a37 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 56a95035ba2..3fc90d56bd8 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 7.0.3 + 7.0.4 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index 674191360dc..6b36a4fa0ad 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index e91d6109b6e..1bf43109ae3 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 7.0.3 + 7.0.4 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 4d4702d540a..9717e5fea04 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index d044251414e..db4ca60757b 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 7.0.3 + 7.0.4 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 06c7e90f7dc..5cec18c57ca 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,12 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index b8c23e528f9..234952f2623 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 7.0.3 + 7.0.4 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index 68c811fe173..8c73f295169 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,12 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 19840b64144..5a538f788b6 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 7.0.3 + 7.0.4 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index f53adfee34f..cb44766923a 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index a02db4e9fbd..d479f37b4e2 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 7.0.3 + 7.0.4 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index fabf001a247..75789efcfb8 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,12 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control 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 bce21f3ee2e..d61120a1ad3 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 - 7.0.3 + 7.0.4 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index bace7285cff..84967884d80 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 54211af4af7..55e95745d6b 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 7.0.3 + 7.0.4 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index c03dd28c38d..4c1534e2e20 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 640aea992d0..954652386e3 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 7.0.3 + 7.0.4 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index a2df026aa25..9c4c1b21250 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 937e3a5441e..fde03aea3b1 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 7.0.3 + 7.0.4 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 6727d05b83f..5332076f875 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 1e1d5e3cb84..2c3a32bfbb6 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 7.0.3 + 7.0.4 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 26d57436298..4cf0429c43a 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 046efa14c6a..a75f30edfd2 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 7.0.3 + 7.0.4 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index a3e40444e82..e19f2963a66 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge pull request `#1465 `_ from tier4/fix/request-enable-autoware-control diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index d3ceb5ed4f8..3830901f7ca 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 7.0.3 + 7.0.4 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 11b944a5910..0d1035446d0 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index a872b522734..c4fb5beafc4 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 7.0.3 + 7.0.4 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index bb5ba5043d1..c8e5351fe86 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,12 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 0d8796c11b3..5c01ea5269a 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 7.0.3 + 7.0.4 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 150fe57fb3b..7ea86322917 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,16 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.0.4 (2024-12-13) +------------------ +* Merge pull request `#1486 `_ from tier4/fix/speed-condition/backward-compatibility + Fix/speed condition/backward compatibility +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Add new parameter `speed_condition` to switch compatibility +* Add new enumeration `Compatibility` +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.3 (2024-12-13) ------------------ * Merge branch 'master' into fix/request-enable-autoware-control diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 419a1b7fc01..633d1aeaf30 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 7.0.3 + 7.0.4 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 1717315cebb72362ba915681735b595956c1c1b5 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 13 Dec 2024 14:14:46 +0900 Subject: [PATCH 114/149] Update `TimeToCollisionCondition` to call `SpeedCondition` in standard compatible mode Signed-off-by: yamacir-kit --- .../syntax/time_to_collision_condition.hpp | 3 ++- .../scenario_test_runner/scenario/sample.yaml | 13 ++----------- 2 files changed, 4 insertions(+), 12 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index ff290830df5..3dc68ba3cd9 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -158,7 +158,8 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio entities, triggering_entity, time_to_collision_condition_target.as(), directional_dimension()); } else { - return SpeedCondition::evaluate(entities, triggering_entity, directional_dimension()); + return SpeedCondition::evaluate( + entities, triggering_entity, directional_dimension(), Compatibility::standard); } }; diff --git a/test_runner/scenario_test_runner/scenario/sample.yaml b/test_runner/scenario_test_runner/scenario/sample.yaml index d465b19c270..37d6440f158 100644 --- a/test_runner/scenario_test_runner/scenario/sample.yaml +++ b/test_runner/scenario_test_runner/scenario/sample.yaml @@ -10,16 +10,7 @@ OpenSCENARIO: revMajor: 1 revMinor: 0 ParameterDeclarations: - ParameterDeclaration: - - name: random_offset - parameterType: double - value: $(ros2 run openscenario_interpreter_example uniform_distribution -1.0 1.0) - ConstraintGroup: - - ValueConstraint: - - rule: lessOrEqual - value: 1.0 - - rule: greaterOrEqual - value: -1.0 + ParameterDeclaration: [] CatalogLocations: VehicleCatalog: Directory: @@ -54,7 +45,7 @@ OpenSCENARIO: roadId: '' laneId: LANE_ID s: 1 - offset: $random_offset + offset: 0 Orientation: type: relative h: 0 From a9d81db67b0ccfed554953717a18fe595acd7962 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 13 Dec 2024 16:07:38 +0900 Subject: [PATCH 115/149] Update some member functions of `FieldOperatorApplication` to be non-virtual Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 13 ++++----- ...ator_application_for_autoware_universe.hpp | 17 +---------- .../concealer/service_with_validation.hpp | 3 +- .../src/field_operator_application.cpp | 4 ++- ...ator_application_for_autoware_universe.cpp | 28 ------------------- 5 files changed, 11 insertions(+), 54 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 4eae6602839..73723d838d9 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -145,7 +145,7 @@ struct FieldOperatorApplication : public rclcpp::Node, SubscriberWrapper getMrmState; SubscriberWrapper getPathWithLaneId; SubscriberWrapper getTrajectory; - SubscriberWrapper getTurnIndicatorsCommandImpl; + SubscriberWrapper getTurnIndicatorsCommand; ServiceWithValidation requestClearRoute; ServiceWithValidation requestCooperateCommands; @@ -202,13 +202,13 @@ struct FieldOperatorApplication : public rclcpp::Node, virtual auto clearRoute() -> void = 0; - virtual auto getAutowareStateName() const -> std::string = 0; + auto getAutowareStateName() const { return autoware_state; } - virtual auto getMinimumRiskManeuverBehaviorName() const -> std::string = 0; + auto getMinimumRiskManeuverBehaviorName() const { return minimum_risk_maneuver_behavior; } - virtual auto getMinimumRiskManeuverStateName() const -> std::string = 0; + auto getMinimumRiskManeuverStateName() const { return minimum_risk_maneuver_state; } - virtual auto getEmergencyStateName() const -> std::string = 0; + auto getEmergencyStateName() const { return minimum_risk_maneuver_state; } virtual auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray = 0; @@ -216,9 +216,6 @@ struct FieldOperatorApplication : public rclcpp::Node, virtual auto requestAutoModeForCooperation(const std::string &, bool) -> void = 0; - virtual auto getTurnIndicatorsCommand() const - -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand = 0; - virtual auto rethrow() const noexcept(false) -> void; virtual auto sendCooperateCommand(const std::string &, const std::string &) -> void = 0; diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 1d4cc1bdc04..08332cfbd80 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -22,11 +22,7 @@ namespace concealer template <> struct FieldOperatorApplicationFor : public FieldOperatorApplication { - template - CONCEALER_PUBLIC explicit FieldOperatorApplicationFor(Ts &&... xs) - : FieldOperatorApplication(std::forward(xs)...) - { - } + using FieldOperatorApplication::FieldOperatorApplication; auto engage() -> void override; @@ -34,19 +30,8 @@ struct FieldOperatorApplicationFor : public FieldOperatorAppli auto engaged() const -> bool override; - auto getAutowareStateName() const -> std::string override; - auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray override; - auto getTurnIndicatorsCommand() const - -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand override; - - auto getEmergencyStateName() const -> std::string override; - - auto getMinimumRiskManeuverBehaviorName() const -> std::string override; - - auto getMinimumRiskManeuverStateName() const -> std::string override; - auto initialize(const geometry_msgs::msg::Pose &) -> void override; auto plan(const std::vector &) -> void override; diff --git a/external/concealer/include/concealer/service_with_validation.hpp b/external/concealer/include/concealer/service_with_validation.hpp index 1b01488224e..256cf30f062 100644 --- a/external/concealer/include/concealer/service_with_validation.hpp +++ b/external/concealer/include/concealer/service_with_validation.hpp @@ -73,7 +73,8 @@ class ServiceWithValidation { public: template - explicit TimeoutError(Ts &&... xs) : common::scenario_simulator_exception::Error(std::forward(xs)...) + explicit TimeoutError(Ts &&... xs) + : common::scenario_simulator_exception::Error(std::forward(xs)...) { } }; diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 71af9a2ae8a..3d17e961456 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -43,6 +43,7 @@ auto toAutowareStateString(std::uint8_t state) -> char const * #undef CASE } +// clang-format off FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) : rclcpp::Node("concealer_user", "simulation", rclcpp::NodeOptions().use_global_arguments(false)), process_id(pid), @@ -113,7 +114,7 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) }), getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), *this), getTrajectory("/api/iv_msgs/planning/scenario_planning/trajectory", rclcpp::QoS(1), *this), - getTurnIndicatorsCommandImpl("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), + getTurnIndicatorsCommand("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), requestClearRoute("/api/routing/clear_route", *this), requestCooperateCommands("/api/external/set/rtc_commands", *this), requestEngage("/api/external/set/engage", *this), @@ -125,6 +126,7 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) requestEnableAutowareControl("/api/operation_mode/enable_autoware_control", *this) { } +// clang-format on FieldOperatorApplication::~FieldOperatorApplication() { diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index ae5707fc62f..13a4404b1d6 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -350,34 +350,6 @@ auto FieldOperatorApplicationFor::getWaypoints() const return waypoints; } -auto FieldOperatorApplicationFor::getTurnIndicatorsCommand() const - -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand -{ - return getTurnIndicatorsCommandImpl(); -} - -auto FieldOperatorApplicationFor::getAutowareStateName() const -> std::string -{ - return autoware_state; -} - -auto FieldOperatorApplicationFor::getEmergencyStateName() const -> std::string -{ - return minimum_risk_maneuver_state; -} - -auto FieldOperatorApplicationFor::getMinimumRiskManeuverBehaviorName() const - -> std::string -{ - return minimum_risk_maneuver_behavior; -} - -auto FieldOperatorApplicationFor::getMinimumRiskManeuverStateName() const - -> std::string -{ - return minimum_risk_maneuver_state; -} - auto FieldOperatorApplicationFor::setVelocityLimit(double velocity_limit) -> void { task_queue.delay([this, velocity_limit]() { From 0b16184ad760af2d30812a8d67f1ed5038d73248 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 13 Dec 2024 16:38:10 +0900 Subject: [PATCH 116/149] Update all member function of `FieldOperatorApplication` to be non-virtual Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 26 +++++++++---------- ...ator_application_for_autoware_universe.hpp | 22 ---------------- .../src/field_operator_application.cpp | 2 -- ...ator_application_for_autoware_universe.cpp | 26 +++++++++---------- 4 files changed, 25 insertions(+), 51 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 73723d838d9..34c291fd24e 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -190,17 +190,17 @@ struct FieldOperatorApplication : public rclcpp::Node, auto shutdownAutoware() -> void; - virtual auto engage() -> void = 0; + auto engage() -> void; - virtual auto engageable() const -> bool = 0; + auto engageable() const -> bool; - virtual auto engaged() const -> bool = 0; + auto engaged() const -> bool; - virtual auto initialize(const geometry_msgs::msg::Pose &) -> void = 0; + auto initialize(const geometry_msgs::msg::Pose &) -> void; - virtual auto plan(const std::vector &) -> void = 0; + auto plan(const std::vector &) -> void; - virtual auto clearRoute() -> void = 0; + auto clearRoute() -> void; auto getAutowareStateName() const { return autoware_state; } @@ -210,19 +210,19 @@ struct FieldOperatorApplication : public rclcpp::Node, auto getEmergencyStateName() const { return minimum_risk_maneuver_state; } - virtual auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray = 0; + auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray; - /* */ auto initialized() const noexcept { return initialize_was_called; } + auto initialized() const noexcept { return initialize_was_called; } - virtual auto requestAutoModeForCooperation(const std::string &, bool) -> void = 0; + auto requestAutoModeForCooperation(const std::string &, bool) -> void; - virtual auto rethrow() const noexcept(false) -> void; + auto rethrow() const { task_queue.rethrow(); } - virtual auto sendCooperateCommand(const std::string &, const std::string &) -> void = 0; + auto sendCooperateCommand(const std::string &, const std::string &) -> void; - virtual auto setVelocityLimit(double) -> void = 0; + auto setVelocityLimit(double) -> void; - virtual auto enableAutowareControl() -> void = 0; + auto enableAutowareControl() -> void; }; } // namespace concealer diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 08332cfbd80..ec69999a5be 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -23,28 +23,6 @@ template <> struct FieldOperatorApplicationFor : public FieldOperatorApplication { using FieldOperatorApplication::FieldOperatorApplication; - - auto engage() -> void override; - - auto engageable() const -> bool override; - - auto engaged() const -> bool override; - - auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray override; - - auto initialize(const geometry_msgs::msg::Pose &) -> void override; - - auto plan(const std::vector &) -> void override; - - auto clearRoute() -> void override; - - auto requestAutoModeForCooperation(const std::string &, bool) -> void override; - - auto sendCooperateCommand(const std::string &, const std::string &) -> void override; - - auto setVelocityLimit(double) -> void override; - - auto enableAutowareControl() -> void override; }; } // namespace concealer diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 3d17e961456..ad8d9ccfe6d 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -241,6 +241,4 @@ auto FieldOperatorApplication::shutdownAutoware() -> void } } } - -auto FieldOperatorApplication::rethrow() const -> void { task_queue.rethrow(); } } // namespace concealer diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index 13a4404b1d6..913f2acbda4 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -152,7 +152,7 @@ bool isValidCooperateStatus( } } -auto FieldOperatorApplicationFor::sendCooperateCommand( +auto FieldOperatorApplication::sendCooperateCommand( const std::string & module_name, const std::string & command) -> void { auto to_command_type = [](const auto & command) { @@ -221,8 +221,7 @@ auto FieldOperatorApplicationFor::sendCooperateCommand( } } -auto FieldOperatorApplicationFor::initialize( - const geometry_msgs::msg::Pose & initial_pose) -> void +auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initial_pose) -> void { if (not std::exchange(initialize_was_called, true)) { task_queue.delay([this, initial_pose]() { @@ -254,8 +253,8 @@ auto FieldOperatorApplicationFor::initialize( } } -auto FieldOperatorApplicationFor::plan( - const std::vector & route) -> void +auto FieldOperatorApplication::plan(const std::vector & route) + -> void { assert(not route.empty()); @@ -298,7 +297,7 @@ auto FieldOperatorApplicationFor::plan( }); } -auto FieldOperatorApplicationFor::clearRoute() -> void +auto FieldOperatorApplication::clearRoute() -> void { task_queue.delay([this] { /* @@ -310,7 +309,7 @@ auto FieldOperatorApplicationFor::clearRoute() -> void }); } -auto FieldOperatorApplicationFor::engage() -> void +auto FieldOperatorApplication::engage() -> void { task_queue.delay([this]() { waitForAutowareStateToBeDriving([this]() { @@ -326,20 +325,19 @@ auto FieldOperatorApplicationFor::engage() -> void }); } -auto FieldOperatorApplicationFor::engageable() const -> bool +auto FieldOperatorApplication::engageable() const -> bool { rethrow(); return task_queue.exhausted() and isWaitingForEngage(); } -auto FieldOperatorApplicationFor::engaged() const -> bool +auto FieldOperatorApplication::engaged() const -> bool { rethrow(); return task_queue.exhausted() and isDriving(); } -auto FieldOperatorApplicationFor::getWaypoints() const - -> traffic_simulator_msgs::msg::WaypointsArray +auto FieldOperatorApplication::getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray { traffic_simulator_msgs::msg::WaypointsArray waypoints; @@ -350,7 +348,7 @@ auto FieldOperatorApplicationFor::getWaypoints() const return waypoints; } -auto FieldOperatorApplicationFor::setVelocityLimit(double velocity_limit) -> void +auto FieldOperatorApplication::setVelocityLimit(double velocity_limit) -> void { task_queue.delay([this, velocity_limit]() { auto request = std::make_shared(); @@ -361,7 +359,7 @@ auto FieldOperatorApplicationFor::setVelocityLimit(double velo }); } -auto FieldOperatorApplicationFor::requestAutoModeForCooperation( +auto FieldOperatorApplication::requestAutoModeForCooperation( const std::string & module_name, bool enable) -> void { // Note: The implementation of this function will not work properly @@ -382,7 +380,7 @@ auto FieldOperatorApplicationFor::requestAutoModeForCooperatio } } -auto FieldOperatorApplicationFor::enableAutowareControl() -> void +auto FieldOperatorApplication::enableAutowareControl() -> void { task_queue.delay([this]() { auto request = std::make_shared(); From 9fefd58b1e3314c9c3adf1f781b2fb24365e44af Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 13 Dec 2024 18:50:07 +0900 Subject: [PATCH 117/149] Remove class template `FieldOperatorApplicationFor` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 3 -- ...ator_application_for_autoware_universe.hpp | 29 ------------------- ...ator_application_for_autoware_universe.cpp | 6 ++-- .../src/entity/ego_entity.cpp | 12 +++----- 4 files changed, 7 insertions(+), 43 deletions(-) delete mode 100644 external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 34c291fd24e..65b4b9a0956 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -56,9 +56,6 @@ namespace concealer { -template -class FieldOperatorApplicationFor; - #define DEFINE_STATIC_DATA_MEMBER_DETECTOR(NAME) \ template \ struct HasStatic##NAME : public std::false_type \ diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp deleted file mode 100644 index ec69999a5be..00000000000 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CONCEALER__AUTOWARE_UNIVERSE_USER_HPP_ -#define CONCEALER__AUTOWARE_UNIVERSE_USER_HPP_ - -#include - -namespace concealer -{ -template <> -struct FieldOperatorApplicationFor : public FieldOperatorApplication -{ - using FieldOperatorApplication::FieldOperatorApplication; -}; -} // namespace concealer - -#endif // CONCEALER__AUTOWARE_UNIVERSE_USER_HPP_ diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index 913f2acbda4..7c146e52fc4 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include #include @@ -375,8 +375,8 @@ auto FieldOperatorApplication::requestAutoModeForCooperation( }); } else { throw common::Error( - "FieldOperatorApplicationFor::requestAutoModeForCooperation is not " - "supported in this environment, because rtc_auto_mode_manager is present."); + "FieldOperatorApplication::requestAutoModeForCooperation is not supported in this " + "environment, because rtc_auto_mode_manager is present."); } } diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index ee67a46d9ce..e4f9560c180 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -13,8 +13,7 @@ // limitations under the License. #include -#include -#include +#include #include #include #include @@ -58,12 +57,10 @@ auto EgoEntity::makeFieldOperatorApplication( // clang-format on return getParameter(node_parameters, "launch_autoware", true) - ? std::make_unique< - concealer::FieldOperatorApplicationFor>( + ? std::make_unique( getParameter(node_parameters, "autoware_launch_package"), getParameter(node_parameters, "autoware_launch_file"), parameters) - : std::make_unique< - concealer::FieldOperatorApplicationFor>(); + : std::make_unique(); } else { throw common::SemanticError( "Unexpected architecture_type ", std::quoted(architecture_type), " was given."); @@ -117,8 +114,7 @@ auto EgoEntity::getRouteLanelets(double /*unused horizon*/) -> lanelet::Ids lanelet::Ids ids{}; if (const auto universe = - dynamic_cast *>( - field_operator_application.get()); + dynamic_cast(field_operator_application.get()); universe) { for (const auto & point : universe->getPathWithLaneId().points) { ids += point.lane_ids; From eaaa87307b7a50c48caef02386f9bc86e9e62e31 Mon Sep 17 00:00:00 2001 From: Release Bot Date: Mon, 16 Dec 2024 01:51:33 +0000 Subject: [PATCH 118/149] Bump version of scenario_simulator_v2 from version 7.0.4 to version 7.1.0 --- common/math/arithmetic/CHANGELOG.rst | 40 +++++++++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 42 ++++++++++++ common/math/geometry/package.xml | 2 +- .../CHANGELOG.rst | 40 +++++++++++ .../scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 40 +++++++++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 40 +++++++++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 40 +++++++++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 40 +++++++++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 40 +++++++++++ map/kashiwanoha_map/package.xml | 2 +- map/simple_cross_map/CHANGELOG.rst | 36 ++++++++++ map/simple_cross_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 40 +++++++++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../CHANGELOG.rst | 40 +++++++++++ .../package.xml | 2 +- .../openscenario_interpreter/CHANGELOG.rst | 66 +++++++++++++++++++ .../openscenario_interpreter/package.xml | 2 +- .../CHANGELOG.rst | 40 +++++++++++ .../package.xml | 2 +- .../CHANGELOG.rst | 40 +++++++++++ .../openscenario_interpreter_msgs/package.xml | 2 +- .../openscenario_preprocessor/CHANGELOG.rst | 40 +++++++++++ .../openscenario_preprocessor/package.xml | 2 +- .../CHANGELOG.rst | 40 +++++++++++ .../package.xml | 2 +- .../openscenario_utility/CHANGELOG.rst | 40 +++++++++++ openscenario/openscenario_utility/package.xml | 2 +- .../openscenario_validator/CHANGELOG.rst | 40 +++++++++++ .../openscenario_validator/package.xml | 2 +- .../openscenario_visualization/CHANGELOG.rst | 40 +++++++++++ .../openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 40 +++++++++++ .../package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 40 +++++++++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 40 +++++++++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 40 +++++++++++ simulation/do_nothing_plugin/package.xml | 2 +- .../simple_sensor_simulator/CHANGELOG.rst | 41 ++++++++++++ .../simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 41 ++++++++++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 41 ++++++++++++ simulation/traffic_simulator/package.xml | 2 +- .../traffic_simulator_msgs/CHANGELOG.rst | 40 +++++++++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 40 +++++++++++ test_runner/random_test_runner/package.xml | 2 +- .../scenario_test_runner/CHANGELOG.rst | 49 ++++++++++++++ test_runner/scenario_test_runner/package.xml | 2 +- 58 files changed, 1225 insertions(+), 29 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 4bb19449115..355630f0aa1 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 15abfa02a91..0f4f5822ab2 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 7.0.4 + 7.1.0 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index 7978b669400..12bb5ca7850 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,48 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Cleanup +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Add new static member function `RelativeSpeedCondition::evaluate` +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 49467cbde14..1e4ac8f3174 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 7.0.4 + 7.1.0 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index fb9d82f2305..534aa3a16e2 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge pull request `#1486 `_ from tier4/fix/speed-condition/backward-compatibility diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index d82aec6de9a..5700054a3f0 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 7.0.4 + 7.1.0 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index b460aa61a8e..f4413aa3c34 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 0453c2ae9b0..8c42e8e9e09 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 7.0.4 + 7.1.0 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index d83fa605d55..790c6f6d386 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 30e322eb56e..86a54b73946 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 7.0.4 + 7.1.0 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index d1cd7fcce35..4fac7e94197 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 703a5b23184..79bd0769e2b 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 7.0.4 + 7.1.0 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 1f1f5bc5223..86d104903de 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,46 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index f68ffcd128d..2636408c83b 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 7.0.4 + 7.1.0 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 852f27c951a..e50bf4e39bc 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index c111cd34b59..4a000c33aae 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 7.0.4 + 7.1.0 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 2316cf6c77b..a3cc2c3c282 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,42 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index 2f55a6f9426..c3c6ef75696 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 7.0.4 + 7.1.0 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 a2e4a96afae..93dd7c74d96 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 78f8a2a45fd..c0f4a5c6f09 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 7.0.4 + 7.1.0 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 874579d5052..6a1f395a39a 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 773bfe663d2..dcb2b66398f 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 7.0.4 + 7.1.0 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 5adaf81ff87..331e3c798ab 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,72 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge pull request `#1258 `_ from tier4/feature/time-to-collision-condition + Feature/time to collision condition +* Update `TimeToCollisionCondition` to call `SpeedCondition` in standard compatible mode +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Remove static member function `evaluateTimeToCollisionCondition` +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Cleanup +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Fix `evaluateTimeToCollisionCondition` to not return meaningless value when collisions cannot occur +* Fix `TimeToCollisionCondition` to return inf if relative speed < zero +* Cleanup static member function `TimeToCollisionCondition::evaluate` +* Add new member function `evaluateCartesianTimeToCollisionCondition` +* Split `(Relative)?DistanceCondition::evaluate` into two overloads +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Add new static member function `TimeToCollisionCondition::evaluate` +* Update `unordered_map` of the `Entities` base class to private +* Move function `hypot` into new header `cmath/hypot.hpp` +* Add support for `DirectionalDimension` to `SpeedCondition` +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Rename `(Relative)?DistanceCondition::distance` to `evaluate` +* Update member function `CoordinateSystem::distance` to be static member +* Add `const Position &` to the argument of `DistanceCondition::distance` +* Remove data member `DistanceCondition::consider_z` +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Add new static member function `RelativeSpeedCondition::evaluate` +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Update `RelativeDistanceCondition::distance` to static member function +* Move entity existence check into `distance` from speceialized `distance` +* Add static member function `ConditionEvaluation::evaluateRelativeSpeed` +* Add new structs `RelativeSpeedCondition` and `DirectionalDimension` +* Add new struct `TimeToCollisionConditionTarget` +* Add new struct `TimeToCollisionCondition` +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge pull request `#1486 `_ from tier4/fix/speed-condition/backward-compatibility diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 0826de6a98e..1259b887e80 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 7.0.4 + 7.1.0 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index c59120080dc..fff205ca57e 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 6ef4c36cf6b..3588d2cf659 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 7.0.4 + 7.1.0 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index b7582bd5a37..b91ae12d68a 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 3fc90d56bd8..d9f8bef9dd8 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 7.0.4 + 7.1.0 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index 6b36a4fa0ad..fb82d84754f 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 1bf43109ae3..e6c39298a41 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 7.0.4 + 7.1.0 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 9717e5fea04..87336ec580a 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index db4ca60757b..ad8f0ee9415 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 7.0.4 + 7.1.0 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 5cec18c57ca..c6540820fcf 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,46 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 234952f2623..67a482c5a1f 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 7.0.4 + 7.1.0 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index 8c73f295169..ef065925903 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,46 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 5a538f788b6..843a4d451fc 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 7.0.4 + 7.1.0 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index cb44766923a..ff94ea238a5 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index d479f37b4e2..e4667786798 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 7.0.4 + 7.1.0 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 75789efcfb8..2e532270d06 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,46 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility 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 d61120a1ad3..df48d20cc04 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 - 7.0.4 + 7.1.0 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 84967884d80..d51161d48c2 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 55e95745d6b..3e386ff0784 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 7.0.4 + 7.1.0 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 4c1534e2e20..bddb30514b1 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 954652386e3..86af7944595 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 7.0.4 + 7.1.0 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 9c4c1b21250..36249354679 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index fde03aea3b1..25819c44f65 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 7.0.4 + 7.1.0 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 5332076f875..6e96e2f25fa 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,47 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 2c3a32bfbb6..1ab93e383ff 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 7.0.4 + 7.1.0 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 4cf0429c43a..f306ba75dff 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,47 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index a75f30edfd2..7a2bb0820a2 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 7.0.4 + 7.1.0 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index e19f2963a66..ed7cebbb41f 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,47 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 3830901f7ca..98e152d4b78 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 7.0.4 + 7.1.0 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 0d1035446d0..bf7f1b1871c 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index c4fb5beafc4..4cd1e20519e 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 7.0.4 + 7.1.0 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index c8e5351fe86..74287ab308c 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,46 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge branch 'master' into fix/speed-condition/backward-compatibility diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 5c01ea5269a..6bf18a22f48 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 7.0.4 + 7.1.0 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 7ea86322917..ce6500b2e13 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,55 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.1.0 (2024-12-16) +------------------ +* Merge pull request `#1258 `_ from tier4/feature/time-to-collision-condition + Feature/time to collision condition +* Update `TimeToCollisionCondition` to call `SpeedCondition` in standard compatible mode +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Remove static member function `evaluateTimeToCollisionCondition` +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Fix `evaluateTimeToCollisionCondition` to not return meaningless value when collisions cannot occur +* Fix `TimeToCollisionCondition` to return inf if relative speed < zero +* Cleanup static member function `TimeToCollisionCondition::evaluate` +* Add new test scenario `...EntityCondition.TimeToCollisionCondition.yaml` +* Add new member function `evaluateCartesianTimeToCollisionCondition` +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.0.4 (2024-12-13) ------------------ * Merge pull request `#1486 `_ from tier4/fix/speed-condition/backward-compatibility diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 633d1aeaf30..5edd5d15bc3 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 7.0.4 + 7.1.0 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 51c8951e047b43a4cd5aa27df577674f20cf74c4 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 16 Dec 2024 11:31:22 +0900 Subject: [PATCH 119/149] Add `/localization/pose_estimator/pose_with_covariance` to `Communication.md` Signed-off-by: yamacir-kit --- docs/developer_guide/Communication.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index 48d22522178..1000197d55e 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -23,8 +23,9 @@ |------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------| | `/clock` | [`rcl_interfaces/msg/Clock`](https://github.com/ros2/rcl_interfaces/blob/master/rosgraph_msgs/msg/Clock.msg) | | | | | `/initialpose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseWithCovarianceStamped.msg) | | -| `/localization/kinematic_state` | [`nav_msgs/msg/Odometry`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/Odometry.msg) | | -| `/localization/acceleration` | [`geometry_msgs::msg::AccelWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/AccelWithCovarianceStamped.msg) | | +| `/localization/kinematic_state` | [`nav_msgs/msg/Odometry`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/Odometry.msg) | Available if option `simulate_localization` is `true` (default is `true`). | +| `/localization/acceleration` | [`geometry_msgs::msg::AccelWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/AccelWithCovarianceStamped.msg) | Available if option `simulate_localization` is `true` (default is `true`). | +| `/localization/pose_estimator/pose_with_covariance` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseWithCovarianceStamped.msg) | Available if option `simulate_localization` is `false` (default is `true`). | | `/planning/mission_planning/checkpoint` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | | `/planning/mission_planning/goal` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | | `/vehicle/status/control_mode` | [`autoware_vehicle_msgs/msg/ControlModeReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/ControlModeReport.msg) | | From 32a8189b80b6a076c455068995bf5b083da0e4a2 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 16 Dec 2024 11:41:52 +0900 Subject: [PATCH 120/149] Reformat tables in `Communication.md` Signed-off-by: yamacir-kit --- docs/developer_guide/Communication.md | 58 +++++++++++++-------------- 1 file changed, 29 insertions(+), 29 deletions(-) diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index 1000197d55e..12b686d824c 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -11,39 +11,39 @@ | `/control/command/turn_indicators_cmd` | [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.msg) | Used in UserDefinedValueCondition : `currentTurnIndicatorsState` | | `/parameter_events` | [`rcl_interfaces/msg/ParameterEvent`](https://github.com/ros2/rcl_interfaces/blob/master/rcl_interfaces/msg/ParameterEvent.msg) | | | `/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id` | [`tier4_planning_msgs/msg/PathWithLaneId`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/PathWithLaneId.msg) | | -| `/api/iv_msgs/planning/scenario_planning/trajectory` | [`tier4_planning_msgs/msg/Trajectory`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/Trajectory.msg) | | -| `/api/external/get/emergency` | [`tier4_external_api_msgs/msg/Emergency`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/msg/Emergency.msg) | Used in UserDefinedValueCondition : `currentEmergencyState` | +| `/api/iv_msgs/planning/scenario_planning/trajectory` | [`tier4_planning_msgs/msg/Trajectory`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/Trajectory.msg) | | +| `/api/external/get/emergency` | [`tier4_external_api_msgs/msg/Emergency`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/msg/Emergency.msg) | Used in UserDefinedValueCondition : `currentEmergencyState` | | `/api/fail_safe/mrm_state` | [`autoware_adapi_v1_msgs/msg/MrmState`](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/system/msg/MrmState.msg) | Used in UserDefinedValueCondition : `currentMinimumRiskManeuverState` | -| `/planning/scenario_planning/motion_velocity_optimizer/closest_jerk` | [`tier4_debug_msgs/msg/Float32Stamped`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_debug_msgs/msg/Float32Stamped.msg) | Used in /simulation/openscenario_interpreter | -| `/api/external/get/rtc_status` | [`tier4_rtc_msgs::msg::CooperateStatusArray`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/msg/CooperateStatusArray.msg) | | +| `/planning/scenario_planning/motion_velocity_optimizer/closest_jerk` | [`tier4_debug_msgs/msg/Float32Stamped`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_debug_msgs/msg/Float32Stamped.msg) | Used in `/simulation/openscenario_interpreter` | +| `/api/external/get/rtc_status` | [`tier4_rtc_msgs/msg/CooperateStatusArray`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/msg/CooperateStatusArray.msg) | | ### Publishers -| topic | type | note | -|------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| `/clock` | [`rcl_interfaces/msg/Clock`](https://github.com/ros2/rcl_interfaces/blob/master/rosgraph_msgs/msg/Clock.msg) | | | | -| `/initialpose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseWithCovarianceStamped.msg) | | -| `/localization/kinematic_state` | [`nav_msgs/msg/Odometry`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/Odometry.msg) | Available if option `simulate_localization` is `true` (default is `true`). | -| `/localization/acceleration` | [`geometry_msgs::msg::AccelWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/AccelWithCovarianceStamped.msg) | Available if option `simulate_localization` is `true` (default is `true`). | -| `/localization/pose_estimator/pose_with_covariance` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseWithCovarianceStamped.msg) | Available if option `simulate_localization` is `false` (default is `true`). | -| `/planning/mission_planning/checkpoint` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | -| `/planning/mission_planning/goal` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | -| `/vehicle/status/control_mode` | [`autoware_vehicle_msgs/msg/ControlModeReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/ControlModeReport.msg) | | -| `/vehicle/status/gear_status` | [`autoware_vehicle_msgs/msg/GearReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/GearReport.msg) | | -| `/vehicle/status/steering_status` | [`autoware_vehicle_msgs/msg/SteeringReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/SteeringReport.msg) | | -| `/vehicle/status/turn_indicators_status` | [`autoware_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/TurnIndicatorsReport.msg) | | -| `/vehicle/status/velocity_status` | [`autoware_vehicle_msgs/msg/VelocityReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/VelocityReport.msg) | | -| `/perception/object_recognition/detection/objects` | [`autoware_perception_msgs/msg/DetectedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/DetectedObjects.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | -| `/perception/object_recognition/ground_truth/objects` | [`autoware_perception_msgs/msg/TrackedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrackedObjects.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | -| `/perception/obstacle_segmentation/pointcloud` | [`sensor_msgs/msg/PointCloud2`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/PointCloud2.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#lidar-simulation) | -| `/perception/occupancy_grid_map/map` | [`nav_msgs/msg/OccupancyGrid`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/OccupancyGrid.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#occupancy-grid-sensor-simulation) | -| `/sensing/imu/imu_data` | [`sensor_msgs/msg/Imu`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/Imu.msg) | | -| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20230906` | -| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20240605` | -| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | -| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | -| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | -| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | +| topic | type | note | +|------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| `/clock` | [`rcl_interfaces/msg/Clock`](https://github.com/ros2/rcl_interfaces/blob/master/rosgraph_msgs/msg/Clock.msg) | | | | +| `/initialpose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseWithCovarianceStamped.msg) | | +| `/localization/kinematic_state` | [`nav_msgs/msg/Odometry`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/Odometry.msg) | Available if option `simulate_localization` is `true` (default is `true`). | +| `/localization/acceleration` | [`geometry_msgs/msg/AccelWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/AccelWithCovarianceStamped.msg) | Available if option `simulate_localization` is `true` (default is `true`). | +| `/localization/pose_estimator/pose_with_covariance` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseWithCovarianceStamped.msg) | Available if option `simulate_localization` is `false` (default is `true`). | +| `/planning/mission_planning/checkpoint` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | +| `/planning/mission_planning/goal` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | +| `/vehicle/status/control_mode` | [`autoware_vehicle_msgs/msg/ControlModeReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/ControlModeReport.msg) | | +| `/vehicle/status/gear_status` | [`autoware_vehicle_msgs/msg/GearReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/GearReport.msg) | | +| `/vehicle/status/steering_status` | [`autoware_vehicle_msgs/msg/SteeringReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/SteeringReport.msg) | | +| `/vehicle/status/turn_indicators_status` | [`autoware_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/TurnIndicatorsReport.msg) | | +| `/vehicle/status/velocity_status` | [`autoware_vehicle_msgs/msg/VelocityReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/VelocityReport.msg) | | +| `/perception/object_recognition/detection/objects` | [`autoware_perception_msgs/msg/DetectedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/DetectedObjects.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | +| `/perception/object_recognition/ground_truth/objects` | [`autoware_perception_msgs/msg/TrackedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrackedObjects.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | +| `/perception/obstacle_segmentation/pointcloud` | [`sensor_msgs/msg/PointCloud2`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/PointCloud2.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#lidar-simulation) | +| `/perception/occupancy_grid_map/map` | [`nav_msgs/msg/OccupancyGrid`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/OccupancyGrid.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#occupancy-grid-sensor-simulation) | +| `/sensing/imu/imu_data` | [`sensor_msgs/msg/Imu`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/Imu.msg) | | +| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20230906` | +| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20240605` | +| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | +| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | +| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | +| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | [//]: # (| /rosout | rcl_interfaces/msg/Log | | |) [//]: # (| /tf | tf2_msgs/msg/TFMessage | | |) From 916ba5bee50836b84d71c3e3076b8408524e884a Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 16 Dec 2024 11:48:17 +0900 Subject: [PATCH 121/149] Sort the table lexicographically by topic name Signed-off-by: yamacir-kit --- docs/developer_guide/Communication.md | 38 +++++++++++++-------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index 12b686d824c..1927ae5ee8a 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -5,17 +5,17 @@ | topic | type | note | |--------------------------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------| -| `/autoware/state` | [`autoware_system_msgs/msg/AutowareState`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_system_msgs/msg/AutowareState.msg) | Used in UserDefinedValueCondition : `currentAutowareState` | +| `/api/external/get/emergency` | [`tier4_external_api_msgs/msg/Emergency`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/msg/Emergency.msg) | Used in UserDefinedValueCondition `currentEmergencyState` | +| `/api/external/get/rtc_status` | [`tier4_rtc_msgs/msg/CooperateStatusArray`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/msg/CooperateStatusArray.msg) | | +| `/api/fail_safe/mrm_state` | [`autoware_adapi_v1_msgs/msg/MrmState`](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/system/msg/MrmState.msg) | Used in UserDefinedValueCondition `currentMinimumRiskManeuverState` | +| `/api/iv_msgs/planning/scenario_planning/trajectory` | [`tier4_planning_msgs/msg/Trajectory`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/Trajectory.msg) | | +| `/autoware/state` | [`autoware_system_msgs/msg/AutowareState`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_system_msgs/msg/AutowareState.msg) | Used in UserDefinedValueCondition `currentAutowareState` | | `/control/command/control_cmd` | [`autoware_control_msgs/msg/Control`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_control_msgs/msg/Control.msg) | | | `/control/command/gear_cmd` | [`autoware_vehicle_msgs/msg/GearCommand`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/GearCommand.msg) | | -| `/control/command/turn_indicators_cmd` | [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.msg) | Used in UserDefinedValueCondition : `currentTurnIndicatorsState` | +| `/control/command/turn_indicators_cmd` | [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.msg) | Used in UserDefinedValueCondition `currentTurnIndicatorsState` | | `/parameter_events` | [`rcl_interfaces/msg/ParameterEvent`](https://github.com/ros2/rcl_interfaces/blob/master/rcl_interfaces/msg/ParameterEvent.msg) | | | `/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id` | [`tier4_planning_msgs/msg/PathWithLaneId`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/PathWithLaneId.msg) | | -| `/api/iv_msgs/planning/scenario_planning/trajectory` | [`tier4_planning_msgs/msg/Trajectory`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/Trajectory.msg) | | -| `/api/external/get/emergency` | [`tier4_external_api_msgs/msg/Emergency`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/msg/Emergency.msg) | Used in UserDefinedValueCondition : `currentEmergencyState` | -| `/api/fail_safe/mrm_state` | [`autoware_adapi_v1_msgs/msg/MrmState`](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/system/msg/MrmState.msg) | Used in UserDefinedValueCondition : `currentMinimumRiskManeuverState` | | `/planning/scenario_planning/motion_velocity_optimizer/closest_jerk` | [`tier4_debug_msgs/msg/Float32Stamped`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_debug_msgs/msg/Float32Stamped.msg) | Used in `/simulation/openscenario_interpreter` | -| `/api/external/get/rtc_status` | [`tier4_rtc_msgs/msg/CooperateStatusArray`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/msg/CooperateStatusArray.msg) | | ### Publishers @@ -23,27 +23,27 @@ |------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------| | `/clock` | [`rcl_interfaces/msg/Clock`](https://github.com/ros2/rcl_interfaces/blob/master/rosgraph_msgs/msg/Clock.msg) | | | | | `/initialpose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseWithCovarianceStamped.msg) | | -| `/localization/kinematic_state` | [`nav_msgs/msg/Odometry`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/Odometry.msg) | Available if option `simulate_localization` is `true` (default is `true`). | | `/localization/acceleration` | [`geometry_msgs/msg/AccelWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/AccelWithCovarianceStamped.msg) | Available if option `simulate_localization` is `true` (default is `true`). | +| `/localization/kinematic_state` | [`nav_msgs/msg/Odometry`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/Odometry.msg) | Available if option `simulate_localization` is `true` (default is `true`). | | `/localization/pose_estimator/pose_with_covariance` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseWithCovarianceStamped.msg) | Available if option `simulate_localization` is `false` (default is `true`). | +| `/perception/object_recognition/detection/objects` | [`autoware_perception_msgs/msg/DetectedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/DetectedObjects.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | +| `/perception/object_recognition/ground_truth/objects` | [`autoware_perception_msgs/msg/TrackedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrackedObjects.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | +| `/perception/obstacle_segmentation/pointcloud` | [`sensor_msgs/msg/PointCloud2`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/PointCloud2.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#lidar-simulation) | +| `/perception/occupancy_grid_map/map` | [`nav_msgs/msg/OccupancyGrid`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/OccupancyGrid.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#occupancy-grid-sensor-simulation) | +| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | +| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | +| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20240605` | +| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20230906` | | `/planning/mission_planning/checkpoint` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | | `/planning/mission_planning/goal` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | +| `/sensing/imu/imu_data` | [`sensor_msgs/msg/Imu`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/Imu.msg) | | +| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | +| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | | `/vehicle/status/control_mode` | [`autoware_vehicle_msgs/msg/ControlModeReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/ControlModeReport.msg) | | | `/vehicle/status/gear_status` | [`autoware_vehicle_msgs/msg/GearReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/GearReport.msg) | | | `/vehicle/status/steering_status` | [`autoware_vehicle_msgs/msg/SteeringReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/SteeringReport.msg) | | | `/vehicle/status/turn_indicators_status` | [`autoware_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/TurnIndicatorsReport.msg) | | | `/vehicle/status/velocity_status` | [`autoware_vehicle_msgs/msg/VelocityReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/VelocityReport.msg) | | -| `/perception/object_recognition/detection/objects` | [`autoware_perception_msgs/msg/DetectedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/DetectedObjects.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | -| `/perception/object_recognition/ground_truth/objects` | [`autoware_perception_msgs/msg/TrackedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrackedObjects.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | -| `/perception/obstacle_segmentation/pointcloud` | [`sensor_msgs/msg/PointCloud2`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/PointCloud2.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#lidar-simulation) | -| `/perception/occupancy_grid_map/map` | [`nav_msgs/msg/OccupancyGrid`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/OccupancyGrid.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#occupancy-grid-sensor-simulation) | -| `/sensing/imu/imu_data` | [`sensor_msgs/msg/Imu`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/Imu.msg) | | -| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20230906` | -| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20240605` | -| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | -| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | -| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | -| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | [//]: # (| /rosout | rcl_interfaces/msg/Log | | |) [//]: # (| /tf | tf2_msgs/msg/TFMessage | | |) @@ -55,8 +55,8 @@ |-----------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|------| | `/api/autoware/set/velocity_limit` | [`tier4_external_api_msgs/srv/SetVelocityLimit`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/srv/SetVelocityLimit.srv) | | | `/api/external/set/engage` | [`tier4_external_api_msgs/srv/Engage`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/srv/Engage.srv) | | -| `/api/external/set/rtc_commands` | [`tier4_rtc_msgs/srv/CooperateCommands`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/srv/CooperateCommands.srv) | | | `/api/external/set/rtc_auto_mode` | [`tier4_rtc_msgs/srv/AutoModeWithModule`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/srv/AutoModeWithModule.srv) | | +| `/api/external/set/rtc_commands` | [`tier4_rtc_msgs/srv/CooperateCommands`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/srv/CooperateCommands.srv) | | | `/api/operation_mode/enable_autoware_control` | [`autoware_adapi_v1_msgs/srv/ChangeOperationMode`](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/operation_mode/srv/ChangeOperationMode.srv) | | ### Service Servers From ce9295291acc11a9187be7b9538c4276eb5d56d3 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 16 Dec 2024 11:50:15 +0900 Subject: [PATCH 122/149] Remove comments in `Communication.md` Signed-off-by: yamacir-kit --- docs/developer_guide/Communication.md | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index 1927ae5ee8a..1da73ed82f0 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -45,10 +45,6 @@ | `/vehicle/status/turn_indicators_status` | [`autoware_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/TurnIndicatorsReport.msg) | | | `/vehicle/status/velocity_status` | [`autoware_vehicle_msgs/msg/VelocityReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/VelocityReport.msg) | | -[//]: # (| /rosout | rcl_interfaces/msg/Log | | |) -[//]: # (| /tf | tf2_msgs/msg/TFMessage | | |) -[//]: # (| /parameter_events | rcl_interfaces/msg/ParameterEvent | | |) - ### Service Clients | service name | type | note | @@ -64,15 +60,3 @@ | service name | type | note | |---------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------------------------------------------| | `/control/control_mode_request` | [`autoware_auto_vehicle_msgs/srv/ControlModeCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/srv/ControlModeCommand.srv) | Simulated by `simple_sensor_simulator` for a manual override | - -[//]: # (/simulation/openscenario_visualizer) - -[//]: # (Subscribers:) - -[//]: # (/simulation/entity/status: traffic_simulator_msgs/msg/EntityStatusWithTrajectoryArray) - -[//]: # (Publishers:) - -[//]: # (/simulation/entity/marker: visualization_msgs/msg/MarkerArray) - -[//]: # () From 0b755020ecf5928d570cb293b7bbd0e90d52926c Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 16 Dec 2024 13:30:16 +0900 Subject: [PATCH 123/149] Remove function template `toAutowareStateString` Signed-off-by: yamacir-kit --- .../src/field_operator_application.cpp | 48 +++++++++---------- 1 file changed, 23 insertions(+), 25 deletions(-) diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index ad8d9ccfe6d..58376c8c17e 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -20,35 +20,33 @@ namespace concealer { -template -auto toAutowareStateString(std::uint8_t state) -> char const * -{ -#define CASE(IDENTIFIER) \ - case T::IDENTIFIER: \ - return #IDENTIFIER - - switch (state) { - CASE(INITIALIZING); - CASE(WAITING_FOR_ROUTE); - CASE(PLANNING); - CASE(WAITING_FOR_ENGAGE); - CASE(DRIVING); - CASE(ARRIVED_GOAL); - CASE(FINALIZING); - - default: - return ""; - } - -#undef CASE -} - // clang-format off FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) : rclcpp::Node("concealer_user", "simulation", rclcpp::NodeOptions().use_global_arguments(false)), process_id(pid), - getAutowareState("/autoware/state", rclcpp::QoS(1), *this, [this](const auto & v) { - autoware_state = toAutowareStateString(v.state); + getAutowareState("/autoware/state", rclcpp::QoS(1), *this, [this](const auto & message) { + auto state_name_of = [](auto state) constexpr { + switch (state) { + case autoware_system_msgs::msg::AutowareState::INITIALIZING: + return "INITIALIZING"; + case autoware_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE: + return "WAITING_FOR_ROUTE"; + case autoware_system_msgs::msg::AutowareState::PLANNING: + return "PLANNING"; + case autoware_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE: + return "WAITING_FOR_ENGAGE"; + case autoware_system_msgs::msg::AutowareState::DRIVING: + return "DRIVING"; + case autoware_system_msgs::msg::AutowareState::ARRIVED_GOAL: + return "ARRIVED_GOAL"; + case autoware_system_msgs::msg::AutowareState::FINALIZING: + return "FINALIZING"; + default: + return ""; + } + }; + + autoware_state = state_name_of(message.state); }), getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), getCooperateStatusArray("/api/external/get/rtc_status", rclcpp::QoS(1), *this, [this](const auto & v) { latest_cooperate_status_array = v; }), From 0e4277f87783e1e4d47bd16ee543b75b4739331f Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 16 Dec 2024 13:49:41 +0900 Subject: [PATCH 124/149] Remove function template `listup` Signed-off-by: yamacir-kit --- ...ator_application_for_autoware_universe.cpp | 65 ++++--------------- 1 file changed, 14 insertions(+), 51 deletions(-) diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index 7c146e52fc4..6d59b1d9360 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -20,46 +20,10 @@ namespace concealer { -template -struct lister -{ - std::reference_wrapper tuples; - - explicit lister(const Tuples & tuples) : tuples(std::cref(tuples)) {} -}; - -template -auto operator<<(std::ostream & ostream, const lister & lister) -> std::ostream & -{ - for (auto iterator = std::begin(lister.tuples.get()); iterator != std::end(lister.tuples.get()); - ++iterator) { - switch (std::distance(iterator, std::end(lister.tuples.get()))) { - case 1: - return ostream << std::get(*iterator); - - case 2: - ostream << std::get(*iterator) << " and "; - break; - - default: - ostream << std::get(*iterator) << ", "; - break; - } - } - - return ostream; -} - -template -auto listup(const Tuples & tuples) -> lister -{ - return lister(tuples); -} - /** - * NOTE: for changes from `distance` to start/finish distance - * see https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef - */ + NOTE: For changes from `distance` to start/finish distance. See + https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef +*/ template struct HasDistance : public std::false_type { @@ -117,8 +81,7 @@ auto toModuleType(const std::string & module_name) if (const auto module_type = module_type_map.find(module_name); module_type == module_type_map.end()) { throw common::Error( - "Unexpected module name for tier4_rtc_msgs::msg::Module: ", module_name, - ". One of the following module names is expected: ", listup<0>(module_type_map), "."); + "Unexpected module name for tier4_rtc_msgs::msg::Module: ", module_name, "."); } else { return module_type->second; } @@ -162,20 +125,20 @@ auto FieldOperatorApplication::sendCooperateCommand( }; if (const auto command_type = command_type_map.find(command); command_type == command_type_map.end()) { - throw common::Error( - "Unexpected command for tier4_rtc_msgs::msg::Command: ", command, - ", One of the following commands is expected: ", listup<0>(command_type_map), "."); + throw common::Error("Unexpected command for tier4_rtc_msgs::msg::Command: ", command, "."); } else { return command_type->second; } }; - /** - * NOTE: Used cooperate statuses will be deleted correctly in Autoware side and provided via topic update. - * But, their update rate (typ. 10Hz) is lower than the one of scenario_simulator_v2. - * So, we need to check cooperate statuses if they are used or not in scenario_simulator_v2 side - * to avoid sending the same cooperate command when sending multiple commands between updates of cooperate statuses. - */ + /* + NOTE: Used cooperate statuses will be deleted correctly in Autoware side + and provided via topic update. But, their update rate (typ. 10Hz) is lower + than the one of scenario_simulator_v2. So, we need to check cooperate + statuses if they are used or not in scenario_simulator_v2 side to avoid + sending the same cooperate command when sending multiple commands between + updates of cooperate statuses. + */ static std::vector used_cooperate_statuses; auto is_used_cooperate_status = [](const auto & cooperate_status) { return std::find_if( @@ -202,7 +165,7 @@ auto FieldOperatorApplication::sendCooperateCommand( std::stringstream what; what << "Failed to send a cooperate command: Cannot find a valid request to cooperate for module " - << std::quoted(module_name) << " and command " << std::quoted(command) << "." + << std::quoted(module_name) << " and command " << std::quoted(command) << ". " << "Please check if the situation is such that the request occurs when sending."; throw common::Error(what.str()); } else { From 3a22042e9f50e23994a328196d4b634f0be31664 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 16 Dec 2024 14:19:49 +0900 Subject: [PATCH 125/149] Move member function implementations into `field_operator_application.cpp` Signed-off-by: yamacir-kit --- external/concealer/CMakeLists.txt | 1 - .../src/field_operator_application.cpp | 339 +++++++++++++++++ ...ator_application_for_autoware_universe.cpp | 353 ------------------ 3 files changed, 339 insertions(+), 354 deletions(-) delete mode 100644 external/concealer/src/field_operator_application_for_autoware_universe.cpp diff --git a/external/concealer/CMakeLists.txt b/external/concealer/CMakeLists.txt index 2fac64c1f9a..0c05bfd2e30 100644 --- a/external/concealer/CMakeLists.txt +++ b/external/concealer/CMakeLists.txt @@ -17,7 +17,6 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/autoware_universe.cpp src/execute.cpp src/field_operator_application.cpp - src/field_operator_application_for_autoware_universe.cpp src/is_package_exists.cpp src/task_queue.cpp) diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 58376c8c17e..71ec0519047 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -12,7 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include +#include +#include +#include #include #include #include @@ -20,6 +24,101 @@ namespace concealer { +/** + NOTE: For changes from `distance` to start/finish distance. See + https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef +*/ +template +struct HasDistance : public std::false_type +{ +}; + +template +struct HasDistance().distance)>> : public std::true_type +{ +}; + +template +auto toModuleType(const std::string & module_name) +{ + static const std::unordered_map module_type_map = [&]() { + std::unordered_map module_type_map; + +#define EMPLACE(IDENTIFIER) \ + if constexpr (HasStatic##IDENTIFIER::value) { \ + module_type_map.emplace(#IDENTIFIER, T::IDENTIFIER); \ + } \ + static_assert(true) + + /* + The following elements are in order of definition in the + tier4_rtc_msgs/msg/Module.msg file. Of course, unordered_map doesn't + preserve the insertion order, so the order itself doesn't matter. + */ + EMPLACE(NONE); + EMPLACE(LANE_CHANGE_LEFT); + EMPLACE(LANE_CHANGE_RIGHT); + EMPLACE(AVOIDANCE_LEFT); + EMPLACE(AVOIDANCE_RIGHT); + EMPLACE(GOAL_PLANNER); + EMPLACE(START_PLANNER); + EMPLACE(PULL_OUT); + EMPLACE(TRAFFIC_LIGHT); + EMPLACE(INTERSECTION); + EMPLACE(INTERSECTION_OCCLUSION); + EMPLACE(CROSSWALK); + EMPLACE(BLIND_SPOT); + EMPLACE(DETECTION_AREA); + EMPLACE(NO_STOPPING_AREA); + EMPLACE(OCCLUSION_SPOT); + EMPLACE(EXT_REQUEST_LANE_CHANGE_LEFT); + EMPLACE(EXT_REQUEST_LANE_CHANGE_RIGHT); + EMPLACE(AVOIDANCE_BY_LC_LEFT); + EMPLACE(AVOIDANCE_BY_LC_RIGHT); + EMPLACE(NO_DRIVABLE_LANE); + +#undef EMPLACE + + return module_type_map; + }(); + + if (const auto module_type = module_type_map.find(module_name); + module_type == module_type_map.end()) { + throw common::Error( + "Unexpected module name for tier4_rtc_msgs::msg::Module: ", module_name, "."); + } else { + return module_type->second; + } +} + +template +bool isValidCooperateStatus( + const CooperateStatusType & cooperate_status, std::uint8_t command_type, std::uint8_t module_type) +{ + /** + * NOTE1: the finish_distance filter is set to over -20.0, + * because some valid rtc statuses has negative finish_distance due to the errors of + * localization or numerical calculation. This threshold is advised by a member of TIER IV + * planning and control team. + */ + + /** + * NOTE2: The difference in the variable referred as a distance is the impact of the + * message specification changes in the following URL. + * This was also decided after consulting with a member of TIER IV planning and control team. + * ref: https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef + */ + if constexpr (HasDistance::value) { + return cooperate_status.module.type == module_type && + command_type != cooperate_status.command_status.type && + cooperate_status.distance >= -20.0; + } else { + return cooperate_status.module.type == module_type && + command_type != cooperate_status.command_status.type && + cooperate_status.finish_distance >= -20.0; + } +} + // clang-format off FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) : rclcpp::Node("concealer_user", "simulation", rclcpp::NodeOptions().use_global_arguments(false)), @@ -133,6 +232,246 @@ FieldOperatorApplication::~FieldOperatorApplication() task_queue.stopAndJoin(); } +auto FieldOperatorApplication::clearRoute() -> void +{ + task_queue.delay([this] { + /* + Since this service tends to be available long after the launch of + Autoware, set the attempts_count to a high value. There is no technical + basis for the number 30. + */ + requestClearRoute(std::make_shared(), 30); + }); +} + +auto FieldOperatorApplication::enableAutowareControl() -> void +{ + task_queue.delay([this]() { + auto request = std::make_shared(); + requestEnableAutowareControl(request, 30); + }); +} + +auto FieldOperatorApplication::engage() -> void +{ + task_queue.delay([this]() { + waitForAutowareStateToBeDriving([this]() { + auto request = std::make_shared(); + request->engage = true; + try { + return requestEngage(request); + } catch (const decltype(requestEngage)::TimeoutError &) { + // ignore timeout error because this service is validated by Autoware state transition. + return; + } + }); + }); +} + +auto FieldOperatorApplication::engageable() const -> bool +{ + rethrow(); + return task_queue.exhausted() and isWaitingForEngage(); +} + +auto FieldOperatorApplication::engaged() const -> bool +{ + rethrow(); + return task_queue.exhausted() and isDriving(); +} + +auto FieldOperatorApplication::getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray +{ + traffic_simulator_msgs::msg::WaypointsArray waypoints; + + for (const auto & point : getTrajectory().points) { + waypoints.waypoints.emplace_back(point.pose.position); + } + + return waypoints; +} + +auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initial_pose) -> void +{ + if (not std::exchange(initialize_was_called, true)) { + task_queue.delay([this, initial_pose]() { + waitForAutowareStateToBeWaitingForRoute([&]() { +#if __has_include() + if ( + getLocalizationState().state != + autoware_adapi_v1_msgs::msg::LocalizationInitializationState::UNINITIALIZED) { + return; + } +#endif + geometry_msgs::msg::PoseWithCovarianceStamped initial_pose_msg; + initial_pose_msg.header.stamp = get_clock()->now(); + initial_pose_msg.header.frame_id = "map"; + initial_pose_msg.pose.pose = initial_pose; + + auto request = + std::make_shared(); + request->pose.push_back(initial_pose_msg); + try { + return requestInitialPose(request); + } catch (const decltype(requestInitialPose)::TimeoutError &) { + // ignore timeout error because this service is validated by Autoware state transition. + return; + } + }); + }); + } +} + +auto FieldOperatorApplication::plan(const std::vector & route) + -> void +{ + assert(not route.empty()); + + task_queue.delay([this, route] { + waitForAutowareStateToBeWaitingForRoute(); // NOTE: This is assertion. + + auto request = std::make_shared(); + + request->header = route.back().header; + + /* + NOTE: The autoware_adapi_v1_msgs::srv::SetRoutePoints::Request type was + created on 2022/09/05 [1], and the autoware_adapi_v1_msgs::msg::Option + type data member was added to the + autoware_adapi_v1_msgs::srv::SetRoutePoints::Request type on 2023/04/12 + [2]. Therefore, we cannot expect + autoware_adapi_v1_msgs::srv::SetRoutePoints::Request to always have a + data member `option`. + + [1] https://github.com/autowarefoundation/autoware_adapi_msgs/commit/805f8ebd3ca24564844df9889feeaf183101fbef + [2] https://github.com/autowarefoundation/autoware_adapi_msgs/commit/cf310bd038673b6cbef3ae3b61dfe607212de419 + */ + if constexpr ( + has_data_member_option_v and + has_data_member_allow_goal_modification_v< + decltype(std::declval().option)>) { + request->option.allow_goal_modification = + get_parameter("allow_goal_modification").get_value(); + } + + request->goal = route.back().pose; + + for (const auto & each : route | boost::adaptors::sliced(0, route.size() - 1)) { + request->waypoints.push_back(each.pose); + } + + requestSetRoutePoints(request); + + waitForAutowareStateToBeWaitingForEngage(); + }); +} + +auto FieldOperatorApplication::requestAutoModeForCooperation( + const std::string & module_name, bool enable) -> void +{ + /* + The implementation of this function will not work properly if the + `rtc_auto_mode_manager` package is present. + */ + if (not isPackageExists("rtc_auto_mode_manager")) { + task_queue.delay([this, module_name, enable]() { + auto request = std::make_shared(); + request->module.type = toModuleType(module_name); + request->enable = enable; + /* + We attempt to resend the service up to 30 times, but this number of + times was determined by heuristics, not for any technical reason. + */ + requestSetRtcAutoMode(request, 30); + }); + } else { + throw common::Error( + "FieldOperatorApplication::requestAutoModeForCooperation is not supported in this " + "environment, because rtc_auto_mode_manager is present."); + } +} + +auto FieldOperatorApplication::sendCooperateCommand( + const std::string & module_name, const std::string & command) -> void +{ + auto to_command_type = [](const auto & command) { + static const std::unordered_map command_type_map = { + {"ACTIVATE", tier4_rtc_msgs::msg::Command::ACTIVATE}, + {"DEACTIVATE", tier4_rtc_msgs::msg::Command::DEACTIVATE}, + }; + if (const auto command_type = command_type_map.find(command); + command_type == command_type_map.end()) { + throw common::Error("Unexpected command for tier4_rtc_msgs::msg::Command: ", command, "."); + } else { + return command_type->second; + } + }; + + /* + NOTE: Used cooperate statuses will be deleted correctly in Autoware side + and provided via topic update. But, their update rate (typ. 10Hz) is lower + than the one of scenario_simulator_v2. So, we need to check cooperate + statuses if they are used or not in scenario_simulator_v2 side to avoid + sending the same cooperate command when sending multiple commands between + updates of cooperate statuses. + */ + static std::vector used_cooperate_statuses; + + auto is_used_cooperate_status = [](const auto & cooperate_status) { + return std::find_if( + used_cooperate_statuses.begin(), used_cooperate_statuses.end(), + [&cooperate_status](const auto & used_cooperate_status) { + return used_cooperate_status.module == cooperate_status.module && + used_cooperate_status.uuid == cooperate_status.uuid && + used_cooperate_status.command_status.type == + cooperate_status.command_status.type; + }) != used_cooperate_statuses.end(); + }; + + if (const auto cooperate_status = std::find_if( + latest_cooperate_status_array.statuses.begin(), + latest_cooperate_status_array.statuses.end(), + [module_type = toModuleType(module_name), + command_type = to_command_type(command), + is_used_cooperate_status](const auto & cooperate_status) { + return isValidCooperateStatus( + cooperate_status, command_type, module_type) && + not is_used_cooperate_status(cooperate_status); + }); + cooperate_status == latest_cooperate_status_array.statuses.end()) { + std::stringstream what; + what + << "Failed to send a cooperate command: Cannot find a valid request to cooperate for module " + << std::quoted(module_name) << " and command " << std::quoted(command) << ". " + << "Please check if the situation is such that the request occurs when sending."; + throw common::Error(what.str()); + } else { + tier4_rtc_msgs::msg::CooperateCommand cooperate_command; + cooperate_command.module = cooperate_status->module; + cooperate_command.uuid = cooperate_status->uuid; + cooperate_command.command.type = to_command_type(command); + + auto request = std::make_shared(); + request->stamp = latest_cooperate_status_array.stamp; + request->commands.push_back(cooperate_command); + + task_queue.delay([this, request]() { requestCooperateCommands(request); }); + + used_cooperate_statuses.push_back(*cooperate_status); + } +} + +auto FieldOperatorApplication::setVelocityLimit(double velocity_limit) -> void +{ + task_queue.delay([this, velocity_limit]() { + auto request = std::make_shared(); + request->velocity = velocity_limit; + // We attempt to resend the service up to 30 times, but this number of times was determined by + // heuristics, not for any technical reason + requestSetVelocityLimit(request, 30); + }); +} + auto FieldOperatorApplication::spinSome() -> void { if (rclcpp::ok() and not is_stop_requested.load()) { diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp deleted file mode 100644 index 6d59b1d9360..00000000000 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ /dev/null @@ -1,353 +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 - -namespace concealer -{ -/** - NOTE: For changes from `distance` to start/finish distance. See - https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef -*/ -template -struct HasDistance : public std::false_type -{ -}; - -template -struct HasDistance().distance)>> : public std::true_type -{ -}; - -template -auto toModuleType(const std::string & module_name) -{ - static const std::unordered_map module_type_map = [&]() { - std::unordered_map module_type_map; - -#define EMPLACE(IDENTIFIER) \ - if constexpr (HasStatic##IDENTIFIER::value) { \ - module_type_map.emplace(#IDENTIFIER, T::IDENTIFIER); \ - } \ - static_assert(true) - - /* - The following elements are in order of definition in the - tier4_rtc_msgs/msg/Module.msg file. Of course, unordered_map doesn't - preserve the insertion order, so the order itself doesn't matter. - */ - EMPLACE(NONE); - EMPLACE(LANE_CHANGE_LEFT); - EMPLACE(LANE_CHANGE_RIGHT); - EMPLACE(AVOIDANCE_LEFT); - EMPLACE(AVOIDANCE_RIGHT); - EMPLACE(GOAL_PLANNER); - EMPLACE(START_PLANNER); - EMPLACE(PULL_OUT); - EMPLACE(TRAFFIC_LIGHT); - EMPLACE(INTERSECTION); - EMPLACE(INTERSECTION_OCCLUSION); - EMPLACE(CROSSWALK); - EMPLACE(BLIND_SPOT); - EMPLACE(DETECTION_AREA); - EMPLACE(NO_STOPPING_AREA); - EMPLACE(OCCLUSION_SPOT); - EMPLACE(EXT_REQUEST_LANE_CHANGE_LEFT); - EMPLACE(EXT_REQUEST_LANE_CHANGE_RIGHT); - EMPLACE(AVOIDANCE_BY_LC_LEFT); - EMPLACE(AVOIDANCE_BY_LC_RIGHT); - EMPLACE(NO_DRIVABLE_LANE); - -#undef EMPLACE - - return module_type_map; - }(); - - if (const auto module_type = module_type_map.find(module_name); - module_type == module_type_map.end()) { - throw common::Error( - "Unexpected module name for tier4_rtc_msgs::msg::Module: ", module_name, "."); - } else { - return module_type->second; - } -} - -template -bool isValidCooperateStatus( - const CooperateStatusType & cooperate_status, std::uint8_t command_type, std::uint8_t module_type) -{ - /** - * NOTE1: the finish_distance filter is set to over -20.0, - * because some valid rtc statuses has negative finish_distance due to the errors of - * localization or numerical calculation. This threshold is advised by a member of TIER IV - * planning and control team. - */ - - /** - * NOTE2: The difference in the variable referred as a distance is the impact of the - * message specification changes in the following URL. - * This was also decided after consulting with a member of TIER IV planning and control team. - * ref: https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef - */ - if constexpr (HasDistance::value) { - return cooperate_status.module.type == module_type && - command_type != cooperate_status.command_status.type && - cooperate_status.distance >= -20.0; - } else { - return cooperate_status.module.type == module_type && - command_type != cooperate_status.command_status.type && - cooperate_status.finish_distance >= -20.0; - } -} - -auto FieldOperatorApplication::sendCooperateCommand( - const std::string & module_name, const std::string & command) -> void -{ - auto to_command_type = [](const auto & command) { - static const std::unordered_map command_type_map = { - {"ACTIVATE", tier4_rtc_msgs::msg::Command::ACTIVATE}, - {"DEACTIVATE", tier4_rtc_msgs::msg::Command::DEACTIVATE}, - }; - if (const auto command_type = command_type_map.find(command); - command_type == command_type_map.end()) { - throw common::Error("Unexpected command for tier4_rtc_msgs::msg::Command: ", command, "."); - } else { - return command_type->second; - } - }; - - /* - NOTE: Used cooperate statuses will be deleted correctly in Autoware side - and provided via topic update. But, their update rate (typ. 10Hz) is lower - than the one of scenario_simulator_v2. So, we need to check cooperate - statuses if they are used or not in scenario_simulator_v2 side to avoid - sending the same cooperate command when sending multiple commands between - updates of cooperate statuses. - */ - static std::vector used_cooperate_statuses; - auto is_used_cooperate_status = [](const auto & cooperate_status) { - return std::find_if( - used_cooperate_statuses.begin(), used_cooperate_statuses.end(), - [&cooperate_status](const auto & used_cooperate_status) { - return used_cooperate_status.module == cooperate_status.module && - used_cooperate_status.uuid == cooperate_status.uuid && - used_cooperate_status.command_status.type == - cooperate_status.command_status.type; - }) != used_cooperate_statuses.end(); - }; - - if (const auto cooperate_status = std::find_if( - latest_cooperate_status_array.statuses.begin(), - latest_cooperate_status_array.statuses.end(), - [module_type = toModuleType(module_name), - command_type = to_command_type(command), - is_used_cooperate_status](const auto & cooperate_status) { - return isValidCooperateStatus( - cooperate_status, command_type, module_type) && - not is_used_cooperate_status(cooperate_status); - }); - cooperate_status == latest_cooperate_status_array.statuses.end()) { - std::stringstream what; - what - << "Failed to send a cooperate command: Cannot find a valid request to cooperate for module " - << std::quoted(module_name) << " and command " << std::quoted(command) << ". " - << "Please check if the situation is such that the request occurs when sending."; - throw common::Error(what.str()); - } else { - tier4_rtc_msgs::msg::CooperateCommand cooperate_command; - cooperate_command.module = cooperate_status->module; - cooperate_command.uuid = cooperate_status->uuid; - cooperate_command.command.type = to_command_type(command); - - auto request = std::make_shared(); - request->stamp = latest_cooperate_status_array.stamp; - request->commands.push_back(cooperate_command); - - task_queue.delay([this, request]() { requestCooperateCommands(request); }); - - used_cooperate_statuses.push_back(*cooperate_status); - } -} - -auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initial_pose) -> void -{ - if (not std::exchange(initialize_was_called, true)) { - task_queue.delay([this, initial_pose]() { - waitForAutowareStateToBeWaitingForRoute([&]() { - -#if __has_include() - if ( - getLocalizationState().state != - autoware_adapi_v1_msgs::msg::LocalizationInitializationState::UNINITIALIZED) { - return; - } -#endif - geometry_msgs::msg::PoseWithCovarianceStamped initial_pose_msg; - initial_pose_msg.header.stamp = get_clock()->now(); - initial_pose_msg.header.frame_id = "map"; - initial_pose_msg.pose.pose = initial_pose; - - auto request = - std::make_shared(); - request->pose.push_back(initial_pose_msg); - try { - return requestInitialPose(request); - } catch (const decltype(requestInitialPose)::TimeoutError &) { - // ignore timeout error because this service is validated by Autoware state transition. - return; - } - }); - }); - } -} - -auto FieldOperatorApplication::plan(const std::vector & route) - -> void -{ - assert(not route.empty()); - - task_queue.delay([this, route] { - waitForAutowareStateToBeWaitingForRoute(); // NOTE: This is assertion. - - auto request = std::make_shared(); - - request->header = route.back().header; - - /* - NOTE: The autoware_adapi_v1_msgs::srv::SetRoutePoints::Request type was - created on 2022/09/05 [1], and the autoware_adapi_v1_msgs::msg::Option - type data member was added to the - autoware_adapi_v1_msgs::srv::SetRoutePoints::Request type on 2023/04/12 - [2]. Therefore, we cannot expect - autoware_adapi_v1_msgs::srv::SetRoutePoints::Request to always have a - data member `option`. - - [1] https://github.com/autowarefoundation/autoware_adapi_msgs/commit/805f8ebd3ca24564844df9889feeaf183101fbef - [2] https://github.com/autowarefoundation/autoware_adapi_msgs/commit/cf310bd038673b6cbef3ae3b61dfe607212de419 - */ - if constexpr ( - has_data_member_option_v and - has_data_member_allow_goal_modification_v< - decltype(std::declval().option)>) { - request->option.allow_goal_modification = - get_parameter("allow_goal_modification").get_value(); - } - - request->goal = route.back().pose; - - for (const auto & each : route | boost::adaptors::sliced(0, route.size() - 1)) { - request->waypoints.push_back(each.pose); - } - - requestSetRoutePoints(request); - - waitForAutowareStateToBeWaitingForEngage(); - }); -} - -auto FieldOperatorApplication::clearRoute() -> void -{ - task_queue.delay([this] { - /* - Since this service tends to be available long after the launch of - Autoware, set the attempts_count to a high value. There is no technical - basis for the number 30. - */ - requestClearRoute(std::make_shared(), 30); - }); -} - -auto FieldOperatorApplication::engage() -> void -{ - task_queue.delay([this]() { - waitForAutowareStateToBeDriving([this]() { - auto request = std::make_shared(); - request->engage = true; - try { - return requestEngage(request); - } catch (const decltype(requestEngage)::TimeoutError &) { - // ignore timeout error because this service is validated by Autoware state transition. - return; - } - }); - }); -} - -auto FieldOperatorApplication::engageable() const -> bool -{ - rethrow(); - return task_queue.exhausted() and isWaitingForEngage(); -} - -auto FieldOperatorApplication::engaged() const -> bool -{ - rethrow(); - return task_queue.exhausted() and isDriving(); -} - -auto FieldOperatorApplication::getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray -{ - traffic_simulator_msgs::msg::WaypointsArray waypoints; - - for (const auto & point : getTrajectory().points) { - waypoints.waypoints.emplace_back(point.pose.position); - } - - return waypoints; -} - -auto FieldOperatorApplication::setVelocityLimit(double velocity_limit) -> void -{ - task_queue.delay([this, velocity_limit]() { - auto request = std::make_shared(); - request->velocity = velocity_limit; - // We attempt to resend the service up to 30 times, but this number of times was determined by - // heuristics, not for any technical reason - requestSetVelocityLimit(request, 30); - }); -} - -auto FieldOperatorApplication::requestAutoModeForCooperation( - const std::string & module_name, bool enable) -> void -{ - // Note: The implementation of this function will not work properly - // if the `rtc_auto_mode_manager` package is present. - if (not isPackageExists("rtc_auto_mode_manager")) { - task_queue.delay([this, module_name, enable]() { - auto request = std::make_shared(); - request->module.type = toModuleType(module_name); - request->enable = enable; - // We attempt to resend the service up to 30 times, but this number of times was determined by - // heuristics, not for any technical reason - requestSetRtcAutoMode(request, 30); - }); - } else { - throw common::Error( - "FieldOperatorApplication::requestAutoModeForCooperation is not supported in this " - "environment, because rtc_auto_mode_manager is present."); - } -} - -auto FieldOperatorApplication::enableAutowareControl() -> void -{ - task_queue.delay([this]() { - auto request = std::make_shared(); - requestEnableAutowareControl(request, 30); - }); -} -} // namespace concealer From 14c50516e461be1df40147cad35125f5132a6ce3 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 16 Dec 2024 14:37:59 +0900 Subject: [PATCH 126/149] Remove header file `autoware_stream.hpp` Signed-off-by: yamacir-kit --- .../include/concealer/autoware_stream.hpp | 31 ------------------- .../concealer/field_operator_application.hpp | 1 - .../src/field_operator_application.cpp | 3 +- 3 files changed, 2 insertions(+), 33 deletions(-) delete mode 100644 external/concealer/include/concealer/autoware_stream.hpp diff --git a/external/concealer/include/concealer/autoware_stream.hpp b/external/concealer/include/concealer/autoware_stream.hpp deleted file mode 100644 index 5a13a61b276..00000000000 --- a/external/concealer/include/concealer/autoware_stream.hpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CONCEALER__AUTOWARE_STREAM_HPP_ -#define CONCEALER__AUTOWARE_STREAM_HPP_ - -#define AUTOWARE_INFO_STREAM(...) \ - RCLCPP_INFO_STREAM(get_logger(), "\x1b[32m" << __VA_ARGS__ << "\x1b[0m") - -#define AUTOWARE_WARN_STREAM(...) \ - RCLCPP_WARN_STREAM(get_logger(), "\x1b[33m" << __VA_ARGS__ << "\x1b[0m") - -#define AUTOWARE_ERROR_STREAM(...) \ - RCLCPP_ERROR_STREAM(get_logger(), "\x1b[1;31m" << __VA_ARGS__ << "\x1b[0m") - -#define AUTOWARE_SYSTEM_ERROR(FROM) \ - AUTOWARE_ERROR_STREAM( \ - "Error on calling " FROM ": " << std::system_error(errno, std::system_category()).what()) - -#endif // CONCEALER__AUTOWARE_STREAM_HPP_ diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 65b4b9a0956..686a44e37ac 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -30,7 +30,6 @@ #include #include #include -#include #include #include #include diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 71ec0519047..1b3ce86c581 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -483,7 +483,8 @@ auto FieldOperatorApplication::spinSome() -> void is_autoware_exited = true; throw common::AutowareError("Autoware process is already terminated"); default: - AUTOWARE_SYSTEM_ERROR("waitpid"); + RCLCPP_ERROR_STREAM( + get_logger(), std::system_error(errno, std::system_category()).what()); std::exit(EXIT_FAILURE); } } else if (0 < id) { From 6d8ce96ca3a74c06447183df78644c9a94c636f5 Mon Sep 17 00:00:00 2001 From: Release Bot Date: Mon, 16 Dec 2024 05:41:38 +0000 Subject: [PATCH 127/149] Bump version of scenario_simulator_v2 from version 7.1.0 to version 7.2.0 --- common/math/arithmetic/CHANGELOG.rst | 15 +++++++++++++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 15 +++++++++++++++ common/math/geometry/package.xml | 2 +- .../CHANGELOG.rst | 15 +++++++++++++++ .../scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 15 +++++++++++++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 15 +++++++++++++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 19 +++++++++++++++++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 15 +++++++++++++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 15 +++++++++++++++ map/kashiwanoha_map/package.xml | 2 +- map/simple_cross_map/CHANGELOG.rst | 15 +++++++++++++++ map/simple_cross_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 15 +++++++++++++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../CHANGELOG.rst | 15 +++++++++++++++ .../package.xml | 2 +- .../openscenario_interpreter/CHANGELOG.rst | 16 ++++++++++++++++ .../openscenario_interpreter/package.xml | 2 +- .../CHANGELOG.rst | 15 +++++++++++++++ .../package.xml | 2 +- .../CHANGELOG.rst | 15 +++++++++++++++ .../openscenario_interpreter_msgs/package.xml | 2 +- .../openscenario_preprocessor/CHANGELOG.rst | 15 +++++++++++++++ .../openscenario_preprocessor/package.xml | 2 +- .../CHANGELOG.rst | 15 +++++++++++++++ .../package.xml | 2 +- .../openscenario_utility/CHANGELOG.rst | 15 +++++++++++++++ openscenario/openscenario_utility/package.xml | 2 +- .../openscenario_validator/CHANGELOG.rst | 15 +++++++++++++++ .../openscenario_validator/package.xml | 2 +- .../openscenario_visualization/CHANGELOG.rst | 15 +++++++++++++++ .../openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 15 +++++++++++++++ .../package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 15 +++++++++++++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 15 +++++++++++++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 15 +++++++++++++++ simulation/do_nothing_plugin/package.xml | 2 +- .../simple_sensor_simulator/CHANGELOG.rst | 18 ++++++++++++++++++ .../simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 15 +++++++++++++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 18 ++++++++++++++++++ simulation/traffic_simulator/package.xml | 2 +- .../traffic_simulator_msgs/CHANGELOG.rst | 15 +++++++++++++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 15 +++++++++++++++ test_runner/random_test_runner/package.xml | 2 +- .../scenario_test_runner/CHANGELOG.rst | 19 +++++++++++++++++++ test_runner/scenario_test_runner/package.xml | 2 +- 58 files changed, 479 insertions(+), 29 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 355630f0aa1..00157438e0a 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 0f4f5822ab2..eb64a9ff504 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 7.1.0 + 7.2.0 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index 12bb5ca7850..ada9a6569bf 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 1e4ac8f3174..4e6a717ea81 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 7.1.0 + 7.2.0 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 534aa3a16e2..887c77f337e 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 5700054a3f0..53da81744bd 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 7.1.0 + 7.2.0 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index f4413aa3c34..886ac0d4264 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 8c42e8e9e09..022f47d3339 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 7.1.0 + 7.2.0 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index 790c6f6d386..d35ce5574fd 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 86a54b73946..5fc64409eef 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 7.1.0 + 7.2.0 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 4fac7e94197..4cd7cc43996 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,25 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge pull request `#1484 `_ from tier4/RJD-736/autoware_msgs_support_and_localization_sim_mode_support + Rjd 736/autoware msgs support and localization sim mode support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Add the missing semicolon +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Add launch argument `simulate_localization` to `scenario_test_runner` +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 79bd0769e2b..6308636521d 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 7.1.0 + 7.2.0 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 86d104903de..173ae3e0eb5 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,21 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 2636408c83b..cf8c7245b1a 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 7.1.0 + 7.2.0 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index e50bf4e39bc..1663efd54b0 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 4a000c33aae..66c98fb29bd 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 7.1.0 + 7.2.0 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 a3cc2c3c282..b9ea513a641 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,21 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index c3c6ef75696..35b7d07eaf6 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 7.1.0 + 7.2.0 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 93dd7c74d96..d7691f345b9 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index c0f4a5c6f09..640155968db 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 7.1.0 + 7.2.0 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 6a1f395a39a..ef2dfbdd120 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index dcb2b66398f..a834e1e4b59 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 7.1.0 + 7.2.0 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 331e3c798ab..bc48149ebdb 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,22 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge pull request `#1258 `_ from tier4/feature/time-to-collision-condition diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 1259b887e80..5f555daba8b 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 7.1.0 + 7.2.0 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index fff205ca57e..ed1774681db 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 3588d2cf659..b5ac17c1c0b 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 7.1.0 + 7.2.0 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index b91ae12d68a..581c13a2606 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index d9f8bef9dd8..c2a5042dd7c 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 7.1.0 + 7.2.0 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index fb82d84754f..e69a8044f83 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index e6c39298a41..ab58136c7f3 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 7.1.0 + 7.2.0 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 87336ec580a..4c0231e37d7 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index ad8f0ee9415..21b1247bd37 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 7.1.0 + 7.2.0 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index c6540820fcf..317b0449bc7 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,21 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 67a482c5a1f..88fd01322dc 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 7.1.0 + 7.2.0 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index ef065925903..dfcbe41993a 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,21 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 843a4d451fc..d576c36356b 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 7.1.0 + 7.2.0 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index ff94ea238a5..752f5a883ef 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index e4667786798..76f7eefeb60 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 7.1.0 + 7.2.0 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 2e532270d06..5a6c43d77c3 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,21 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition 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 df48d20cc04..c3d5e6ceec6 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 - 7.1.0 + 7.2.0 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index d51161d48c2..1efd3513e82 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 3e386ff0784..cccd49d9046 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 7.1.0 + 7.2.0 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index bddb30514b1..3363674fa38 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 86af7944595..cf39316d865 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 7.1.0 + 7.2.0 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 36249354679..0b9b9b1639f 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 25819c44f65..379e7a20b56 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 7.1.0 + 7.2.0 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 6e96e2f25fa..61926489cbf 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,24 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge pull request `#1484 `_ from tier4/RJD-736/autoware_msgs_support_and_localization_sim_mode_support + Rjd 736/autoware msgs support and localization sim mode support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Add launch argument `simulate_localization` to `scenario_test_runner` +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 1ab93e383ff..eabfb92b2e2 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 7.1.0 + 7.2.0 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index f306ba75dff..74a5e915192 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 7a2bb0820a2..d085b6043c6 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 7.1.0 + 7.2.0 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index ed7cebbb41f..c0362563dad 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,24 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge pull request `#1484 `_ from tier4/RJD-736/autoware_msgs_support_and_localization_sim_mode_support + Rjd 736/autoware msgs support and localization sim mode support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Add launch argument `simulate_localization` to `scenario_test_runner` +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 98e152d4b78..7a505c915ff 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 7.1.0 + 7.2.0 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index bf7f1b1871c..6bee56ba464 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 4cd1e20519e..69b4b354ff6 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 7.1.0 + 7.2.0 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 74287ab308c..007d0a483de 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 6bf18a22f48..c8308bbce73 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 7.1.0 + 7.2.0 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index ce6500b2e13..a92463cda5e 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,25 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.2.0 (2024-12-16) +------------------ +* Merge pull request `#1484 `_ from tier4/RJD-736/autoware_msgs_support_and_localization_sim_mode_support + Rjd 736/autoware msgs support and localization sim mode support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Add launch argument `simulate_localization` to `scenario_test_runner` +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.1.0 (2024-12-16) ------------------ * Merge pull request `#1258 `_ from tier4/feature/time-to-collision-condition diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 5edd5d15bc3..1c72387b39a 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 7.1.0 + 7.2.0 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From cd96887a5dad48ffd21e444485a94bbdf7fa747b Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 16 Dec 2024 16:09:21 +0900 Subject: [PATCH 128/149] Cleanup class template `SubscriberWrapper` Signed-off-by: yamacir-kit --- .../include/concealer/autoware_universe.hpp | 8 +-- .../concealer/field_operator_application.hpp | 2 +- .../include/concealer/subscriber_wrapper.hpp | 51 ++++++++----------- 3 files changed, 27 insertions(+), 34 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 20df13aafac..18a4d15c080 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -56,10 +56,10 @@ class AutowareUniverse : public rclcpp::Node, using TurnIndicatorsReport = autoware_vehicle_msgs::msg::TurnIndicatorsReport; using VelocityReport = autoware_vehicle_msgs::msg::VelocityReport; - SubscriberWrapper getCommand; - SubscriberWrapper getGearCommand; - SubscriberWrapper getTurnIndicatorsCommand; - SubscriberWrapper getPathWithLaneId; + SubscriberWrapper getCommand; + SubscriberWrapper getGearCommand; + SubscriberWrapper getTurnIndicatorsCommand; + SubscriberWrapper getPathWithLaneId; PublisherWrapper setAcceleration; PublisherWrapper setOdometry; diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 686a44e37ac..c0ee1ff7070 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -131,7 +131,7 @@ struct FieldOperatorApplication : public rclcpp::Node, std::string minimum_risk_maneuver_behavior; // clang-format off - SubscriberWrapper getAutowareState; + SubscriberWrapper getAutowareState; SubscriberWrapper getCommand; SubscriberWrapper getCooperateStatusArray; SubscriberWrapper getEmergencyState; diff --git a/external/concealer/include/concealer/subscriber_wrapper.hpp b/external/concealer/include/concealer/subscriber_wrapper.hpp index 8247a872e3a..eb4fde75af0 100644 --- a/external/concealer/include/concealer/subscriber_wrapper.hpp +++ b/external/concealer/include/concealer/subscriber_wrapper.hpp @@ -20,46 +20,39 @@ namespace concealer { -enum class ThreadSafety : bool { unsafe, safe }; - -template +template class SubscriberWrapper { - typename MessageType::ConstSharedPtr current_value = std::make_shared(); + typename Message::ConstSharedPtr current_value = std::make_shared(); - typename rclcpp::Subscription::SharedPtr subscription; + typename rclcpp::Subscription::SharedPtr subscription; public: - auto operator()() const -> decltype(auto) - { - if constexpr (thread_safety == ThreadSafety::unsafe) { - return *current_value; - } else { - return *std::atomic_load(¤t_value); - } - } + auto operator()() const -> const auto & { return *std::atomic_load(¤t_value); } - template - SubscriberWrapper( - const std::string & topic, const rclcpp::QoS & quality_of_service, - NodeInterface & autoware_interface, - const std::function & callback = {}) - : subscription(autoware_interface.template create_subscription( + template + explicit SubscriberWrapper( + const std::string & topic, const rclcpp::QoS & quality_of_service, Autoware & autoware, + const Callback & callback) + : subscription(autoware.template create_subscription( topic, quality_of_service, - [this, callback](const typename MessageType::ConstSharedPtr message) { - if constexpr (thread_safety == ThreadSafety::safe) { - std::atomic_store(¤t_value, message); - if (current_value and callback) { - callback(*std::atomic_load(¤t_value)); - } - } else { - if (current_value = message; current_value and callback) { - callback(*current_value); - } + [this, callback](const typename Message::ConstSharedPtr & message) { + if (std::atomic_store(¤t_value, message); current_value) { + callback(*std::atomic_load(¤t_value)); } })) { } + + template + explicit SubscriberWrapper( + const std::string & topic, const rclcpp::QoS & quality_of_service, Autoware & autoware) + : subscription(autoware.template create_subscription( + topic, quality_of_service, [this](const typename Message::ConstSharedPtr & message) { + std::atomic_store(¤t_value, message); + })) + { + } }; } // namespace concealer From ecde7c01972b06820af6c8a522bdf81ccf5cf10e Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 16 Dec 2024 16:58:06 +0900 Subject: [PATCH 129/149] Add new header file `concealer/member_detector.hpp` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 41 --------- ...as_data_member_allow_goal_modification.hpp | 38 --------- .../concealer/has_data_member_option.hpp | 37 --------- .../include/concealer/member_detector.hpp | 83 +++++++++++++++++++ .../concealer/service_with_validation.hpp | 33 +------- .../src/field_operator_application.cpp | 44 ++++------ 6 files changed, 101 insertions(+), 175 deletions(-) delete mode 100644 external/concealer/include/concealer/has_data_member_allow_goal_modification.hpp delete mode 100644 external/concealer/include/concealer/has_data_member_option.hpp create mode 100644 external/concealer/include/concealer/member_detector.hpp diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index c0ee1ff7070..eaace157b8d 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -55,47 +55,6 @@ namespace concealer { -#define DEFINE_STATIC_DATA_MEMBER_DETECTOR(NAME) \ - template \ - struct HasStatic##NAME : public std::false_type \ - { \ - }; \ - \ - template \ - struct HasStatic##NAME> : public std::true_type \ - { \ - } - -DEFINE_STATIC_DATA_MEMBER_DETECTOR(NONE); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(LANE_CHANGE_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(LANE_CHANGE_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(GOAL_PLANNER); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(START_PLANNER); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(PULL_OUT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(TRAFFIC_LIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(INTERSECTION); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(INTERSECTION_OCCLUSION); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(CROSSWALK); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(BLIND_SPOT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(DETECTION_AREA); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(NO_STOPPING_AREA); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(OCCLUSION_SPOT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_BY_LC_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_BY_LC_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(NO_DRIVABLE_LANE); - -DEFINE_STATIC_DATA_MEMBER_DETECTOR(COMFORTABLE_STOP); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(EMERGENCY_STOP); -// DEFINE_STATIC_DATA_MEMBER_DETECTOR(NONE); // NOTE: This is defined above. -DEFINE_STATIC_DATA_MEMBER_DETECTOR(UNKNOWN); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(PULL_OVER); - -#undef DEFINE_STATIC_DATA_MEMBER_DETECTOR - /* ---- NOTE ------------------------------------------------------------------- * * The magic class 'FieldOperatorApplication' is a class that makes it easy to work with diff --git a/external/concealer/include/concealer/has_data_member_allow_goal_modification.hpp b/external/concealer/include/concealer/has_data_member_allow_goal_modification.hpp deleted file mode 100644 index 56de7a1f9f7..00000000000 --- a/external/concealer/include/concealer/has_data_member_allow_goal_modification.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CONCEALER__HAS_DATA_MEMBER_ALLOW_GOAL_MODIFICATION_HPP_ -#define CONCEALER__HAS_DATA_MEMBER_ALLOW_GOAL_MODIFICATION_HPP_ - -#include - -namespace concealer -{ -template -struct has_data_member_allow_goal_modification : public std::false_type -{ -}; - -template -struct has_data_member_allow_goal_modification< - T, std::void_t().allow_goal_modification)>> : public std::true_type -{ -}; - -template -inline constexpr auto has_data_member_allow_goal_modification_v = - has_data_member_allow_goal_modification::value; -} // namespace concealer - -#endif // CONCEALER__HAS_DATA_MEMBER_ALLOW_GOAL_MODIFICATION_HPP_ diff --git a/external/concealer/include/concealer/has_data_member_option.hpp b/external/concealer/include/concealer/has_data_member_option.hpp deleted file mode 100644 index 959e5fcdf44..00000000000 --- a/external/concealer/include/concealer/has_data_member_option.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CONCEALER__HAS_DATA_MEMBER_OPTION_HPP_ -#define CONCEALER__HAS_DATA_MEMBER_OPTION_HPP_ - -#include - -namespace concealer -{ -template -struct has_data_member_option : public std::false_type -{ -}; - -template -struct has_data_member_option().option)>> -: public std::true_type -{ -}; - -template -inline constexpr auto has_data_member_option_v = has_data_member_option::value; -} // namespace concealer - -#endif // CONCEALER__HAS_DATA_MEMBER_OPTION_HPP_ diff --git a/external/concealer/include/concealer/member_detector.hpp b/external/concealer/include/concealer/member_detector.hpp new file mode 100644 index 00000000000..d618347b0e9 --- /dev/null +++ b/external/concealer/include/concealer/member_detector.hpp @@ -0,0 +1,83 @@ +// 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 CONCEALER__MEMBER_DETECTOR_HPP_ +#define CONCEALER__MEMBER_DETECTOR_HPP_ + +#include + +namespace concealer +{ +#define DEFINE_MEMBER_DETECTOR(IDENTIFIER) \ + template \ + struct DetectMember_##IDENTIFIER : public std::false_type \ + { \ + }; \ + \ + template \ + struct DetectMember_##IDENTIFIER().IDENTIFIER)>> \ + : public std::true_type \ + { \ + } + +DEFINE_MEMBER_DETECTOR(allow_goal_modification); +DEFINE_MEMBER_DETECTOR(distance); +DEFINE_MEMBER_DETECTOR(option); +DEFINE_MEMBER_DETECTOR(status); +DEFINE_MEMBER_DETECTOR(success); + +#undef DEFINE_MEMBER_DETECTOR + +#define DEFINE_STATIC_MEMBER_DETECTOR(IDENTIFIER) \ + template \ + struct DetectStaticMember_##IDENTIFIER : public std::false_type \ + { \ + }; \ + \ + template \ + struct DetectStaticMember_##IDENTIFIER> \ + : public std::true_type \ + { \ + } + +DEFINE_STATIC_MEMBER_DETECTOR(AVOIDANCE_BY_LC_LEFT); +DEFINE_STATIC_MEMBER_DETECTOR(AVOIDANCE_BY_LC_RIGHT); +DEFINE_STATIC_MEMBER_DETECTOR(AVOIDANCE_LEFT); +DEFINE_STATIC_MEMBER_DETECTOR(AVOIDANCE_RIGHT); +DEFINE_STATIC_MEMBER_DETECTOR(BLIND_SPOT); +DEFINE_STATIC_MEMBER_DETECTOR(COMFORTABLE_STOP); +DEFINE_STATIC_MEMBER_DETECTOR(CROSSWALK); +DEFINE_STATIC_MEMBER_DETECTOR(DETECTION_AREA); +DEFINE_STATIC_MEMBER_DETECTOR(EMERGENCY_STOP); +DEFINE_STATIC_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_LEFT); +DEFINE_STATIC_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_RIGHT); +DEFINE_STATIC_MEMBER_DETECTOR(GOAL_PLANNER); +DEFINE_STATIC_MEMBER_DETECTOR(INTERSECTION); +DEFINE_STATIC_MEMBER_DETECTOR(INTERSECTION_OCCLUSION); +DEFINE_STATIC_MEMBER_DETECTOR(LANE_CHANGE_LEFT); +DEFINE_STATIC_MEMBER_DETECTOR(LANE_CHANGE_RIGHT); +DEFINE_STATIC_MEMBER_DETECTOR(NONE); +DEFINE_STATIC_MEMBER_DETECTOR(NO_DRIVABLE_LANE); +DEFINE_STATIC_MEMBER_DETECTOR(NO_STOPPING_AREA); +DEFINE_STATIC_MEMBER_DETECTOR(OCCLUSION_SPOT); +DEFINE_STATIC_MEMBER_DETECTOR(PULL_OUT); +DEFINE_STATIC_MEMBER_DETECTOR(PULL_OVER); +DEFINE_STATIC_MEMBER_DETECTOR(START_PLANNER); +DEFINE_STATIC_MEMBER_DETECTOR(TRAFFIC_LIGHT); +DEFINE_STATIC_MEMBER_DETECTOR(UNKNOWN); + +#undef DEFINE_STATIC_MEMBER_DETECTOR +} // namespace concealer + +#endif // CONCEALER__MEMBER_DETECTOR_HPP_ diff --git a/external/concealer/include/concealer/service_with_validation.hpp b/external/concealer/include/concealer/service_with_validation.hpp index 256cf30f062..9d1b0c23d9b 100644 --- a/external/concealer/include/concealer/service_with_validation.hpp +++ b/external/concealer/include/concealer/service_with_validation.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -24,34 +25,6 @@ #include #include -template -struct has_data_member_status : public std::false_type -{ -}; - -template -struct has_data_member_status().status)>> -: public std::true_type -{ -}; - -template -constexpr auto has_data_member_status_v = has_data_member_status::value; - -template -struct has_data_member_success : public std::false_type -{ -}; - -template -struct has_data_member_success().success)>> -: public std::true_type -{ -}; - -template -constexpr auto has_data_member_success_v = has_data_member_success::value; - namespace concealer { template @@ -85,7 +58,7 @@ class ServiceWithValidation validateAvailability(); for (std::size_t attempt = 0; attempt < attempts_count; ++attempt, validation_rate.sleep()) { if (const auto & service_call_result = callWithTimeoutValidation(request)) { - if constexpr (has_data_member_status_v) { + if constexpr (DetectMember_status::value) { if constexpr (std::is_same_v< tier4_external_api_msgs::msg::ResponseStatus, decltype(T::Response::status)>) { @@ -129,7 +102,7 @@ class ServiceWithValidation RCLCPP_INFO_STREAM(logger, service_name << " service request has been accepted."); return; } - } else if constexpr (has_data_member_success_v) { + } else if constexpr (DetectMember_success::value) { if constexpr (std::is_same_v) { if (service_call_result->get()->success) { RCLCPP_INFO_STREAM(logger, service_name << " service request has been accepted."); diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 1b3ce86c581..98c563315d6 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -14,9 +14,8 @@ #include #include -#include -#include #include +#include #include #include #include @@ -24,30 +23,16 @@ namespace concealer { -/** - NOTE: For changes from `distance` to start/finish distance. See - https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef -*/ -template -struct HasDistance : public std::false_type -{ -}; - -template -struct HasDistance().distance)>> : public std::true_type -{ -}; - template auto toModuleType(const std::string & module_name) { static const std::unordered_map module_type_map = [&]() { std::unordered_map module_type_map; -#define EMPLACE(IDENTIFIER) \ - if constexpr (HasStatic##IDENTIFIER::value) { \ - module_type_map.emplace(#IDENTIFIER, T::IDENTIFIER); \ - } \ +#define EMPLACE(IDENTIFIER) \ + if constexpr (DetectStaticMember_##IDENTIFIER::value) { \ + module_type_map.emplace(#IDENTIFIER, T::IDENTIFIER); \ + } \ static_assert(true) /* @@ -108,7 +93,7 @@ bool isValidCooperateStatus( * This was also decided after consulting with a member of TIER IV planning and control team. * ref: https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef */ - if constexpr (HasDistance::value) { + if constexpr (DetectMember_distance::value) { return cooperate_status.module.type == module_type && command_type != cooperate_status.command_status.type && cooperate_status.distance >= -20.0; @@ -177,27 +162,27 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) }; auto behavior_name_of = [](auto behavior) constexpr { - if constexpr (HasStaticCOMFORTABLE_STOP::value) { + if constexpr (DetectStaticMember_COMFORTABLE_STOP::value) { if (behavior == autoware_adapi_v1_msgs::msg::MrmState::COMFORTABLE_STOP) { return "COMFORTABLE_STOP"; } } - if constexpr (HasStaticEMERGENCY_STOP::value) { + if constexpr (DetectStaticMember_EMERGENCY_STOP::value) { if (behavior == autoware_adapi_v1_msgs::msg::MrmState::EMERGENCY_STOP) { return "EMERGENCY_STOP"; } } - if constexpr (HasStaticNONE::value) { + if constexpr (DetectStaticMember_NONE::value) { if (behavior == autoware_adapi_v1_msgs::msg::MrmState::NONE) { return "NONE"; } } - if constexpr (HasStaticUNKNOWN::value) { + if constexpr (DetectStaticMember_UNKNOWN::value) { if (behavior == autoware_adapi_v1_msgs::msg::MrmState::UNKNOWN) { return "UNKNOWN"; } } - if constexpr (HasStaticPULL_OVER::value) { + if constexpr (DetectStaticMember_PULL_OVER::value) { if (behavior == autoware_adapi_v1_msgs::msg::MrmState::PULL_OVER) { return "PULL_OVER"; } @@ -347,9 +332,10 @@ auto FieldOperatorApplication::plan(const std::vector and - has_data_member_allow_goal_modification_v< - decltype(std::declval().option)>) { + DetectMember_option::value and + DetectMember_allow_goal_modification< + decltype(std::declval() + .option)>::value) { request->option.allow_goal_modification = get_parameter("allow_goal_modification").get_value(); } From 5c65f27b25dddea63043c9e2547c3a0aec3ed9d9 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 16 Dec 2024 17:51:25 +0900 Subject: [PATCH 130/149] Add using declarations for subscribers and publishers Signed-off-by: yamacir-kit --- .../include/concealer/autoware_universe.hpp | 3 +- .../concealer/field_operator_application.hpp | 55 +++++++++++----- external/concealer/src/autoware_universe.cpp | 4 +- .../src/field_operator_application.cpp | 65 +++++++++---------- 4 files changed, 72 insertions(+), 55 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 18a4d15c080..0a1d3bfd4ef 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -49,6 +49,7 @@ class AutowareUniverse : public rclcpp::Node, using ControlModeReport = autoware_vehicle_msgs::msg::ControlModeReport; using GearCommand = autoware_vehicle_msgs::msg::GearCommand; using GearReport = autoware_vehicle_msgs::msg::GearReport; + using Odometry = nav_msgs::msg::Odometry; using PathWithLaneId = tier4_planning_msgs::msg::PathWithLaneId; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; using SteeringReport = autoware_vehicle_msgs::msg::SteeringReport; @@ -62,7 +63,7 @@ class AutowareUniverse : public rclcpp::Node, SubscriberWrapper getPathWithLaneId; PublisherWrapper setAcceleration; - PublisherWrapper setOdometry; + PublisherWrapper setOdometry; PublisherWrapper setPose; PublisherWrapper setSteeringReport; PublisherWrapper setGearReport; diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index eaace157b8d..64e7c6beec9 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -90,26 +90,45 @@ struct FieldOperatorApplication : public rclcpp::Node, std::string minimum_risk_maneuver_behavior; // clang-format off - SubscriberWrapper getAutowareState; - SubscriberWrapper getCommand; - SubscriberWrapper getCooperateStatusArray; - SubscriberWrapper getEmergencyState; + using AutowareState = autoware_system_msgs::msg::AutowareState; + using Control = autoware_control_msgs::msg::Control; + using CooperateStatusArray = tier4_rtc_msgs::msg::CooperateStatusArray; + using Emergency = tier4_external_api_msgs::msg::Emergency; + using LocalizationInitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; + using MrmState = autoware_adapi_v1_msgs::msg::MrmState; + using PathWithLaneId = tier4_planning_msgs::msg::PathWithLaneId; + using Trajectory = tier4_planning_msgs::msg::Trajectory; + using TurnIndicatorsCommand = autoware_vehicle_msgs::msg::TurnIndicatorsCommand; + + using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; + using CooperateCommands = tier4_rtc_msgs::srv::CooperateCommands; + using Engage = tier4_external_api_msgs::srv::Engage; + using InitializeLocalization = autoware_adapi_v1_msgs::srv::InitializeLocalization; + using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; + using AutoModeWithModule = tier4_rtc_msgs::srv::AutoModeWithModule; + using SetVelocityLimit = tier4_external_api_msgs::srv::SetVelocityLimit; + using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; + + SubscriberWrapper getAutowareState; + SubscriberWrapper getCommand; + SubscriberWrapper getCooperateStatusArray; + SubscriberWrapper getEmergencyState; #if __has_include() - SubscriberWrapper getLocalizationState; + SubscriberWrapper getLocalizationState; #endif - SubscriberWrapper getMrmState; - SubscriberWrapper getPathWithLaneId; - SubscriberWrapper getTrajectory; - SubscriberWrapper getTurnIndicatorsCommand; - - ServiceWithValidation requestClearRoute; - ServiceWithValidation requestCooperateCommands; - ServiceWithValidation requestEngage; - ServiceWithValidation requestInitialPose; - ServiceWithValidation requestSetRoutePoints; - ServiceWithValidation requestSetRtcAutoMode; - ServiceWithValidation requestSetVelocityLimit; - ServiceWithValidation requestEnableAutowareControl; + SubscriberWrapper getMrmState; + SubscriberWrapper getPathWithLaneId; + SubscriberWrapper getTrajectory; + SubscriberWrapper getTurnIndicatorsCommand; + + ServiceWithValidation requestClearRoute; + ServiceWithValidation requestCooperateCommands; + ServiceWithValidation requestEngage; + ServiceWithValidation requestInitialPose; + ServiceWithValidation requestSetRoutePoints; + ServiceWithValidation requestSetRtcAutoMode; + ServiceWithValidation requestSetVelocityLimit; + ServiceWithValidation requestEnableAutowareControl; // clang-format on CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t = 0); diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index af5e5f91a65..094b466b1c5 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -121,7 +121,7 @@ auto AutowareUniverse::updateLocalization() -> void }()); setOdometry([this]() { - nav_msgs::msg::Odometry message; + Odometry message; message.header.stamp = get_clock()->now(); message.header.frame_id = "map"; message.pose.pose = current_pose.load(); @@ -132,7 +132,7 @@ auto AutowareUniverse::updateLocalization() -> void setPose([this]() { // See https://github.com/tier4/autoware.universe/blob/45ab20af979c5663e5a8d4dda787b1dea8d6e55b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L785-L803 - geometry_msgs::msg::PoseWithCovarianceStamped message; + PoseWithCovarianceStamped message; message.header.stamp = get_clock()->now(); message.header.frame_id = "map"; message.pose.pose = current_pose.load(); diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 98c563315d6..01c58937cd1 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -111,19 +111,19 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) getAutowareState("/autoware/state", rclcpp::QoS(1), *this, [this](const auto & message) { auto state_name_of = [](auto state) constexpr { switch (state) { - case autoware_system_msgs::msg::AutowareState::INITIALIZING: + case AutowareState::INITIALIZING: return "INITIALIZING"; - case autoware_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE: + case AutowareState::WAITING_FOR_ROUTE: return "WAITING_FOR_ROUTE"; - case autoware_system_msgs::msg::AutowareState::PLANNING: + case AutowareState::PLANNING: return "PLANNING"; - case autoware_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE: + case AutowareState::WAITING_FOR_ENGAGE: return "WAITING_FOR_ENGAGE"; - case autoware_system_msgs::msg::AutowareState::DRIVING: + case AutowareState::DRIVING: return "DRIVING"; - case autoware_system_msgs::msg::AutowareState::ARRIVED_GOAL: + case AutowareState::ARRIVED_GOAL: return "ARRIVED_GOAL"; - case autoware_system_msgs::msg::AutowareState::FINALIZING: + case AutowareState::FINALIZING: return "FINALIZING"; default: return ""; @@ -145,15 +145,15 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) getMrmState("/api/fail_safe/mrm_state", rclcpp::QoS(1), *this, [this](const auto & message) { auto state_name_of = [](auto state) constexpr { switch (state) { - case autoware_adapi_v1_msgs::msg::MrmState::MRM_FAILED: + case MrmState::MRM_FAILED: return "MRM_FAILED"; - case autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING: + case MrmState::MRM_OPERATING: return "MRM_OPERATING"; - case autoware_adapi_v1_msgs::msg::MrmState::MRM_SUCCEEDED: + case MrmState::MRM_SUCCEEDED: return "MRM_SUCCEEDED"; - case autoware_adapi_v1_msgs::msg::MrmState::NORMAL: + case MrmState::NORMAL: return "NORMAL"; - case autoware_adapi_v1_msgs::msg::MrmState::UNKNOWN: + case MrmState::UNKNOWN: return "UNKNOWN"; default: throw common::Error( @@ -162,28 +162,28 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) }; auto behavior_name_of = [](auto behavior) constexpr { - if constexpr (DetectStaticMember_COMFORTABLE_STOP::value) { - if (behavior == autoware_adapi_v1_msgs::msg::MrmState::COMFORTABLE_STOP) { + if constexpr (DetectStaticMember_COMFORTABLE_STOP::value) { + if (behavior == MrmState::COMFORTABLE_STOP) { return "COMFORTABLE_STOP"; } } - if constexpr (DetectStaticMember_EMERGENCY_STOP::value) { - if (behavior == autoware_adapi_v1_msgs::msg::MrmState::EMERGENCY_STOP) { + if constexpr (DetectStaticMember_EMERGENCY_STOP::value) { + if (behavior == MrmState::EMERGENCY_STOP) { return "EMERGENCY_STOP"; } } - if constexpr (DetectStaticMember_NONE::value) { - if (behavior == autoware_adapi_v1_msgs::msg::MrmState::NONE) { + if constexpr (DetectStaticMember_NONE::value) { + if (behavior == MrmState::NONE) { return "NONE"; } } - if constexpr (DetectStaticMember_UNKNOWN::value) { - if (behavior == autoware_adapi_v1_msgs::msg::MrmState::UNKNOWN) { + if constexpr (DetectStaticMember_UNKNOWN::value) { + if (behavior == MrmState::UNKNOWN) { return "UNKNOWN"; } } - if constexpr (DetectStaticMember_PULL_OVER::value) { - if (behavior == autoware_adapi_v1_msgs::msg::MrmState::PULL_OVER) { + if constexpr (DetectStaticMember_PULL_OVER::value) { + if (behavior == MrmState::PULL_OVER) { return "PULL_OVER"; } } @@ -225,14 +225,14 @@ auto FieldOperatorApplication::clearRoute() -> void Autoware, set the attempts_count to a high value. There is no technical basis for the number 30. */ - requestClearRoute(std::make_shared(), 30); + requestClearRoute(std::make_shared(), 30); }); } auto FieldOperatorApplication::enableAutowareControl() -> void { task_queue.delay([this]() { - auto request = std::make_shared(); + auto request = std::make_shared(); requestEnableAutowareControl(request, 30); }); } @@ -241,7 +241,7 @@ auto FieldOperatorApplication::engage() -> void { task_queue.delay([this]() { waitForAutowareStateToBeDriving([this]() { - auto request = std::make_shared(); + auto request = std::make_shared(); request->engage = true; try { return requestEngage(request); @@ -282,9 +282,7 @@ auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initi task_queue.delay([this, initial_pose]() { waitForAutowareStateToBeWaitingForRoute([&]() { #if __has_include() - if ( - getLocalizationState().state != - autoware_adapi_v1_msgs::msg::LocalizationInitializationState::UNINITIALIZED) { + if (getLocalizationState().state != LocalizationInitializationState::UNINITIALIZED) { return; } #endif @@ -315,7 +313,7 @@ auto FieldOperatorApplication::plan(const std::vector(); + auto request = std::make_shared(); request->header = route.back().header; @@ -332,10 +330,9 @@ auto FieldOperatorApplication::plan(const std::vector::value and + DetectMember_option::value and DetectMember_allow_goal_modification< - decltype(std::declval() - .option)>::value) { + decltype(std::declval().option)>::value) { request->option.allow_goal_modification = get_parameter("allow_goal_modification").get_value(); } @@ -361,7 +358,7 @@ auto FieldOperatorApplication::requestAutoModeForCooperation( */ if (not isPackageExists("rtc_auto_mode_manager")) { task_queue.delay([this, module_name, enable]() { - auto request = std::make_shared(); + auto request = std::make_shared(); request->module.type = toModuleType(module_name); request->enable = enable; /* @@ -450,7 +447,7 @@ auto FieldOperatorApplication::sendCooperateCommand( auto FieldOperatorApplication::setVelocityLimit(double velocity_limit) -> void { task_queue.delay([this, velocity_limit]() { - auto request = std::make_shared(); + auto request = std::make_shared(); request->velocity = velocity_limit; // We attempt to resend the service up to 30 times, but this number of times was determined by // heuristics, not for any technical reason From 2400c4241be0abf033fe1c05a74b92d5616173db Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 16 Dec 2024 18:13:50 +0900 Subject: [PATCH 131/149] Remove member function `FieldOperatorApplication::shutdownAutoware` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 2 - .../src/field_operator_application.cpp | 154 +++++++++--------- 2 files changed, 75 insertions(+), 81 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 64e7c6beec9..eb38d77932b 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -162,8 +162,6 @@ struct FieldOperatorApplication : public rclcpp::Node, auto spinSome() -> void; - auto shutdownAutoware() -> void; - auto engage() -> void; auto engageable() const -> bool; diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 01c58937cd1..c197d4182d1 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -212,7 +212,81 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) FieldOperatorApplication::~FieldOperatorApplication() { - shutdownAutoware(); + if (is_stop_requested.store(true); + process_id != 0 && not std::exchange(is_autoware_exited, true)) { + const auto sigset = [this]() { + if (auto signal_set = sigset_t(); + sigemptyset(&signal_set) or sigaddset(&signal_set, SIGCHLD)) { + RCLCPP_ERROR_STREAM(get_logger(), std::system_error(errno, std::system_category()).what()); + std::exit(EXIT_FAILURE); + } else if (auto error = pthread_sigmask(SIG_BLOCK, &signal_set, nullptr)) { + RCLCPP_ERROR_STREAM(get_logger(), std::system_error(error, std::system_category()).what()); + std::exit(EXIT_FAILURE); + } else { + return signal_set; + } + }(); + + const auto timeout = []() { + auto sigterm_timeout = [](auto value) { + auto node = rclcpp::Node("get_parameter_sigterm_timeout", "simulation"); + node.declare_parameter("sigterm_timeout", value); + node.get_parameter("sigterm_timeout", value); + return value; + }; + auto timeout = timespec(); + timeout.tv_sec = sigterm_timeout(5); + timeout.tv_nsec = 0; + return timeout; + }(); + + if (::kill(process_id, SIGINT); sigtimedwait(&sigset, nullptr, &timeout) < 0) { + switch (errno) { + case EINTR: + /* + The wait was interrupted by an unblocked, caught signal. It shall + be documented in system documentation whether this error causes + these functions to fail. + */ + RCLCPP_ERROR_STREAM( + get_logger(), + "The wait for Autoware launch process termination was interrupted by an unblocked, " + "caught signal."); + break; + + case EAGAIN: + /* + No signal specified by set was generated within the specified + timeout period. + */ + RCLCPP_ERROR_STREAM(get_logger(), "Autoware launch process does not respond. Kill it."); + killpg(process_id, SIGKILL); + break; + + default: + case EINVAL: + /* + The timeout argument specified a tv_nsec value less than zero or + greater than or equal to 1000 million. + */ + RCLCPP_ERROR_STREAM( + get_logger(), + "The parameter sigterm_timeout specified a value less than zero or greater than or " + "equal to 1000 million."); + break; + } + } + + if (int status = 0; waitpid(process_id, &status, 0) < 0) { + if (errno == ECHILD) { + RCLCPP_ERROR_STREAM( + get_logger(), "Try to wait for the autoware process but it was already exited."); + } else { + RCLCPP_ERROR_STREAM(get_logger(), std::system_error(errno, std::system_category()).what()); + } + } + } + // All tasks should be complete before the services used in them will be deinitialized. task_queue.stopAndJoin(); } @@ -484,82 +558,4 @@ auto FieldOperatorApplication::spinSome() -> void rclcpp::spin_some(get_node_base_interface()); } } - -auto FieldOperatorApplication::shutdownAutoware() -> void -{ - if (is_stop_requested.store(true); - process_id != 0 && not std::exchange(is_autoware_exited, true)) { - const auto sigset = [this]() { - if (auto signal_set = sigset_t(); - sigemptyset(&signal_set) or sigaddset(&signal_set, SIGCHLD)) { - RCLCPP_ERROR_STREAM(get_logger(), std::system_error(errno, std::system_category()).what()); - std::exit(EXIT_FAILURE); - } else if (auto error = pthread_sigmask(SIG_BLOCK, &signal_set, nullptr)) { - RCLCPP_ERROR_STREAM(get_logger(), std::system_error(error, std::system_category()).what()); - std::exit(EXIT_FAILURE); - } else { - return signal_set; - } - }(); - - const auto timeout = []() { - auto sigterm_timeout = [](auto value) { - auto node = rclcpp::Node("get_parameter_sigterm_timeout", "simulation"); - node.declare_parameter("sigterm_timeout", value); - node.get_parameter("sigterm_timeout", value); - return value; - }; - auto timeout = timespec(); - timeout.tv_sec = sigterm_timeout(5); - timeout.tv_nsec = 0; - return timeout; - }(); - - if (::kill(process_id, SIGINT); sigtimedwait(&sigset, nullptr, &timeout) < 0) { - switch (errno) { - case EINTR: - /* - The wait was interrupted by an unblocked, caught signal. It shall - be documented in system documentation whether this error causes - these functions to fail. - */ - RCLCPP_ERROR_STREAM( - get_logger(), - "The wait for Autoware launch process termination was interrupted by an unblocked, " - "caught signal."); - break; - - case EAGAIN: - /* - No signal specified by set was generated within the specified - timeout period. - */ - RCLCPP_ERROR_STREAM(get_logger(), "Autoware launch process does not respond. Kill it."); - killpg(process_id, SIGKILL); - break; - - default: - case EINVAL: - /* - The timeout argument specified a tv_nsec value less than zero or - greater than or equal to 1000 million. - */ - RCLCPP_ERROR_STREAM( - get_logger(), - "The parameter sigterm_timeout specified a value less than zero or greater than or " - "equal to 1000 million."); - break; - } - } - - if (int status = 0; waitpid(process_id, &status, 0) < 0) { - if (errno == ECHILD) { - RCLCPP_ERROR_STREAM( - get_logger(), "Try to wait for the autoware process but it was already exited."); - } else { - RCLCPP_ERROR_STREAM(get_logger(), std::system_error(errno, std::system_category()).what()); - } - } - } -} } // namespace concealer From ada25b16d079d8e4fccc79c21333a9f697ccd73a Mon Sep 17 00:00:00 2001 From: Release Bot Date: Mon, 16 Dec 2024 09:18:55 +0000 Subject: [PATCH 132/149] Bump version of scenario_simulator_v2 from version 7.2.0 to version 7.3.0 --- common/math/arithmetic/CHANGELOG.rst | 9 ++++++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 20 ++++++++++++++++ common/math/geometry/package.xml | 2 +- .../CHANGELOG.rst | 9 ++++++++ .../scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 9 ++++++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 9 ++++++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 9 ++++++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 9 ++++++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 9 ++++++++ map/kashiwanoha_map/package.xml | 2 +- map/simple_cross_map/CHANGELOG.rst | 9 ++++++++ map/simple_cross_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 9 ++++++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../CHANGELOG.rst | 9 ++++++++ .../package.xml | 2 +- .../openscenario_interpreter/CHANGELOG.rst | 9 ++++++++ .../openscenario_interpreter/package.xml | 2 +- .../CHANGELOG.rst | 9 ++++++++ .../package.xml | 2 +- .../CHANGELOG.rst | 9 ++++++++ .../openscenario_interpreter_msgs/package.xml | 2 +- .../openscenario_preprocessor/CHANGELOG.rst | 9 ++++++++ .../openscenario_preprocessor/package.xml | 2 +- .../CHANGELOG.rst | 9 ++++++++ .../package.xml | 2 +- .../openscenario_utility/CHANGELOG.rst | 9 ++++++++ openscenario/openscenario_utility/package.xml | 2 +- .../openscenario_validator/CHANGELOG.rst | 9 ++++++++ .../openscenario_validator/package.xml | 2 +- .../openscenario_visualization/CHANGELOG.rst | 9 ++++++++ .../openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 9 ++++++++ .../package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 9 ++++++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 19 +++++++++++++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 9 ++++++++ simulation/do_nothing_plugin/package.xml | 2 +- .../simple_sensor_simulator/CHANGELOG.rst | 23 +++++++++++++++++++ .../simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 11 +++++++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 20 ++++++++++++++++ simulation/traffic_simulator/package.xml | 2 +- .../traffic_simulator_msgs/CHANGELOG.rst | 9 ++++++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 9 ++++++++ test_runner/random_test_runner/package.xml | 2 +- .../scenario_test_runner/CHANGELOG.rst | 9 ++++++++ test_runner/scenario_test_runner/package.xml | 2 +- 58 files changed, 338 insertions(+), 29 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 00157438e0a..c7b6a88b70f 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index eb64a9ff504..f537c68e4ff 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 7.2.0 + 7.3.0 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index ada9a6569bf..66ce967d505 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,26 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge pull request `#1481 `_ from tier4/feature/multi-level-lanelet-support + Feature/multi level lanelet support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* ref(traffic_simulator, simple_sensor_simulator): refactor altitude checks +* Refactor code to improve readability based on SonarQube findings +* Merge branch 'master' into feature/multi-level-lanelet-support +* Remove comment +* Merge branch 'master' into feature/multi-level-lanelet-support +* Fix missing newline at end of file +* [RJD-1369] Improve Collision Solving for Multi-Level Support + - Enhanced BehaviorTree to consider altitude when detecting potential obstacles, + allowing to ignore objects located at different altitudes. + - Modified the detection sensor by introducing Ego plane determination to exclude objects below the Ego plane, + preventing unnecessary slowing or stopping caused by incorrect detections. +* Contributors: Dawid Moszynski, Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 4e6a717ea81..8a7bc2b4d2c 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 7.2.0 + 7.3.0 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 887c77f337e..bafb69b3ca4 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 53da81744bd..3559db66e79 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 7.2.0 + 7.3.0 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index 886ac0d4264..d6f1cd208f2 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 022f47d3339..7befc153190 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 7.2.0 + 7.3.0 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index d35ce5574fd..13c51cc56d2 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 5fc64409eef..4ce85918d8d 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 7.2.0 + 7.3.0 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 4cd7cc43996..cc180c689ca 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge pull request `#1484 `_ from tier4/RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 6308636521d..248f73c126d 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 7.2.0 + 7.3.0 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 173ae3e0eb5..e605b06641b 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,15 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index cf8c7245b1a..3defb0ef4dc 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 7.2.0 + 7.3.0 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 1663efd54b0..4448ce20d6b 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 66c98fb29bd..afd821ef354 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 7.2.0 + 7.3.0 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 b9ea513a641..88832ce9abb 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,15 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index 35b7d07eaf6..4996688461e 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 7.2.0 + 7.3.0 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 d7691f345b9..061df30c948 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 640155968db..958c0bc5792 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 7.2.0 + 7.3.0 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index ef2dfbdd120..fd15942339e 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index a834e1e4b59..573580351d8 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 7.2.0 + 7.3.0 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index bc48149ebdb..38ebf1dc490 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,15 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 5f555daba8b..ff4f7516cc7 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 7.2.0 + 7.3.0 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index ed1774681db..dbaacb10980 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index b5ac17c1c0b..eed406acb5e 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 7.2.0 + 7.3.0 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index 581c13a2606..4265a0ec76d 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index c2a5042dd7c..1d94189bad1 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 7.2.0 + 7.3.0 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index e69a8044f83..66a59a00197 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index ab58136c7f3..02e6c684ff9 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 7.2.0 + 7.3.0 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 4c0231e37d7..1fa9123d832 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index 21b1247bd37..abb4ba5485e 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 7.2.0 + 7.3.0 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 317b0449bc7..744077acb99 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,15 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 88fd01322dc..7d91f787b4d 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 7.2.0 + 7.3.0 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index dfcbe41993a..4317f732786 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,15 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index d576c36356b..8158dc4f1cd 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 7.2.0 + 7.3.0 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 752f5a883ef..42ff8651cd7 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 76f7eefeb60..f514929c106 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 7.2.0 + 7.3.0 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 5a6c43d77c3..b0a3243475b 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,15 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support 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 c3d5e6ceec6..46c36eb6741 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 - 7.2.0 + 7.3.0 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 1efd3513e82..6a1ad058aac 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index cccd49d9046..5048da71c6e 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 7.2.0 + 7.3.0 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 3363674fa38..0a8d758a5bb 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,25 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge pull request `#1481 `_ from tier4/feature/multi-level-lanelet-support + Feature/multi level lanelet support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'feature/multi-level-lanelet-support' of https://github.com/tier4/scenario_simulator_v2 into feature/multi-level-lanelet-support +* ref(traffic_simulator, simple_sensor_simulator): refactor altitude checks +* Refactor code to improve readability based on SonarQube findings +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* [RJD-1369] Improve Collision Solving for Multi-Level Support + - Enhanced BehaviorTree to consider altitude when detecting potential obstacles, + allowing to ignore objects located at different altitudes. + - Modified the detection sensor by introducing Ego plane determination to exclude objects below the Ego plane, + preventing unnecessary slowing or stopping caused by incorrect detections. +* Contributors: Dawid Moszynski, Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index cf39316d865..5ca8a0bda17 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 7.2.0 + 7.3.0 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 0b9b9b1639f..d2c96703654 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 379e7a20b56..76c4550ce48 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 7.2.0 + 7.3.0 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 61926489cbf..c3130844d7f 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,29 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge pull request `#1481 `_ from tier4/feature/multi-level-lanelet-support + Feature/multi level lanelet support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* fix(simple_sensor_simulator): Fix if condition by adding negation to ensure proper logic +* Remove unused function and update comment +* fix(simple_senor_simulator): fix after Szymon discussion +* fix(simple_sensor_simulator): fix after detection_sensor refactor +* ref(traffic_simulator, simple_sensor_simulator): refactor altitude checks +* Refactor code to improve readability based on SonarQube findings +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Removed unrecognized words because spell-check flagged them as invalid +* [RJD-1369] Improve Collision Solving for Multi-Level Support + - Enhanced BehaviorTree to consider altitude when detecting potential obstacles, + allowing to ignore objects located at different altitudes. + - Modified the detection sensor by introducing Ego plane determination to exclude objects below the Ego plane, + preventing unnecessary slowing or stopping caused by incorrect detections. +* Contributors: Dawid Moszynski, Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge pull request `#1484 `_ from tier4/RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index eabfb92b2e2..55060b3d3b9 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 7.2.0 + 7.3.0 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 74a5e915192..12177cf663a 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,17 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* fix(traffic_simulator): revert clang changes +* ref(traffic_simulator, simple_sensor_simulator): refactor altitude checks +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Dawid Moszynski, Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index d085b6043c6..929c22f9a7f 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 7.2.0 + 7.3.0 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index c0362563dad..b6a05f7c227 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,26 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge pull request `#1481 `_ from tier4/feature/multi-level-lanelet-support + Feature/multi level lanelet support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* fix(traffic_simulator): revert clang changes +* ref(traffic_simulator, simple_sensor_simulator): refactor altitude checks +* Merge branch 'master' into feature/multi-level-lanelet-support +* [RJD-1370] Fix 3D Lanelet Matching Issue in cpp_mock_scenario + - Updated the makeRandomPose method to correctly support 3D lanelet matching. +* Fix an issue with unit tests - distanceTest +* Merge branch 'master' into feature/multi-level-lanelet-support +* Removed unrecognized words because spell-check flagged them as invalid +* [RJD-1369] Improve lanelet matching - 3D support + - Enhanced lanelet matching algorithm (`toLaneletPose` method) by incorporating lanelet altitude. + - Defined the `altitude_threshold` parameter that sets the maximum altitude difference to determine when an entity can be matched with a specific lanelet. +* Contributors: Dawid Moszynski, Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge pull request `#1484 `_ from tier4/RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 7a505c915ff..9578bdb927f 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 7.2.0 + 7.3.0 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 6bee56ba464..e9c52213db8 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 69b4b354ff6..b23095deb00 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 7.2.0 + 7.3.0 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 007d0a483de..6289a66c67a 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index c8308bbce73..0c5024660a6 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 7.2.0 + 7.3.0 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index a92463cda5e..f0ca93e1ae8 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,15 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + 7.2.0 (2024-12-16) ------------------ * Merge pull request `#1484 `_ from tier4/RJD-736/autoware_msgs_support_and_localization_sim_mode_support diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 1c72387b39a..bcb0fd35f27 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 7.2.0 + 7.3.0 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From fc3108cc762b3221ae67b89c71523e43a591d177 Mon Sep 17 00:00:00 2001 From: Release Bot Date: Tue, 17 Dec 2024 02:11:26 +0000 Subject: [PATCH 133/149] Bump version of scenario_simulator_v2 from version 7.3.0 to version 7.3.1 --- common/math/arithmetic/CHANGELOG.rst | 9 +++++++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 15 +++++++++++++++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 9 +++++++++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 9 +++++++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 9 +++++++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 9 +++++++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 9 +++++++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 9 +++++++++ map/kashiwanoha_map/package.xml | 2 +- map/simple_cross_map/CHANGELOG.rst | 9 +++++++++ map/simple_cross_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 9 +++++++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../CHANGELOG.rst | 9 +++++++++ .../openscenario_experimental_catalog/package.xml | 2 +- .../openscenario_interpreter/CHANGELOG.rst | 9 +++++++++ openscenario/openscenario_interpreter/package.xml | 2 +- .../CHANGELOG.rst | 9 +++++++++ .../openscenario_interpreter_example/package.xml | 2 +- .../openscenario_interpreter_msgs/CHANGELOG.rst | 9 +++++++++ .../openscenario_interpreter_msgs/package.xml | 2 +- .../openscenario_preprocessor/CHANGELOG.rst | 9 +++++++++ .../openscenario_preprocessor/package.xml | 2 +- .../openscenario_preprocessor_msgs/CHANGELOG.rst | 9 +++++++++ .../openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 9 +++++++++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 9 +++++++++ openscenario/openscenario_validator/package.xml | 2 +- .../openscenario_visualization/CHANGELOG.rst | 9 +++++++++ .../openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 9 +++++++++ .../package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 9 +++++++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 9 +++++++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 9 +++++++++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 9 +++++++++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 9 +++++++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 9 +++++++++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 9 +++++++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 9 +++++++++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 9 +++++++++ test_runner/scenario_test_runner/package.xml | 2 +- 58 files changed, 296 insertions(+), 29 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index c7b6a88b70f..8c2f6c160a9 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index f537c68e4ff..4a6df50ef0a 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 7.3.0 + 7.3.1 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index 66ce967d505..edacd2450c1 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge pull request `#1475 `_ from tier4/fix/math-closest-point + Fix bug in `math::geometry::getClosestPoses` +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Fix spell mistake +* Fix getClosestPoses with naive algorithm +* Apply clang-format +* Add some test cases for getClosestPoses +* Contributors: Kotaro Yoshimoto, f0reachARR + 7.3.0 (2024-12-16) ------------------ * Merge pull request `#1481 `_ from tier4/feature/multi-level-lanelet-support diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 8a7bc2b4d2c..2c5768dffef 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 7.3.0 + 7.3.1 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index bafb69b3ca4..72637382ee5 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 3559db66e79..59c822e347e 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 7.3.0 + 7.3.1 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index d6f1cd208f2..3590c0e5a4e 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 7befc153190..59de004336c 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 7.3.0 + 7.3.1 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index 13c51cc56d2..88d38680851 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 4ce85918d8d..2b11b7d5db3 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 7.3.0 + 7.3.1 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index cc180c689ca..bab0b1415f0 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 248f73c126d..6cd22af85cf 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 7.3.0 + 7.3.1 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index e605b06641b..8b344fe8499 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,15 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 3defb0ef4dc..bfb9482383b 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 7.3.0 + 7.3.1 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 4448ce20d6b..e74373a3a54 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index afd821ef354..3c2e685b8fc 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 7.3.0 + 7.3.1 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 88832ce9abb..58ccabeafe4 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,15 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index 4996688461e..7415c963183 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 7.3.0 + 7.3.1 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 061df30c948..5d225ae6f2d 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 958c0bc5792..1a6f1febf9d 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 7.3.0 + 7.3.1 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index fd15942339e..c3dca724a92 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 573580351d8..91901f76fdb 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 7.3.0 + 7.3.1 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 38ebf1dc490..599614b1734 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,15 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index ff4f7516cc7..dbd79ed73ed 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 7.3.0 + 7.3.1 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index dbaacb10980..a7c23a1a33a 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index eed406acb5e..ac610e687ae 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 7.3.0 + 7.3.1 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index 4265a0ec76d..7c8b46ef5ee 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 1d94189bad1..9c410d95d75 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 7.3.0 + 7.3.1 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index 66a59a00197..f4dcede6aa6 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 02e6c684ff9..3a3403bca59 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 7.3.0 + 7.3.1 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 1fa9123d832..aef6a7e7023 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index abb4ba5485e..6b22f10b33b 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 7.3.0 + 7.3.1 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 744077acb99..88920e74917 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,15 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 7d91f787b4d..8bf819d8461 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 7.3.0 + 7.3.1 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index 4317f732786..09de3672ce7 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,15 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 8158dc4f1cd..c049e91a549 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 7.3.0 + 7.3.1 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 42ff8651cd7..3ac5c9a8796 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index f514929c106..6ea002cc049 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 7.3.0 + 7.3.1 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index b0a3243475b..46162ecca6c 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,15 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support 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 46c36eb6741..d6e7a553626 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 - 7.3.0 + 7.3.1 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 6a1ad058aac..57e73b43af7 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 5048da71c6e..db7195b4768 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 7.3.0 + 7.3.1 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 0a8d758a5bb..a8f95206656 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge pull request `#1481 `_ from tier4/feature/multi-level-lanelet-support diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 5ca8a0bda17..e5c23f826d7 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 7.3.0 + 7.3.1 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index d2c96703654..6e4452593ce 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 76c4550ce48..b378fe699ef 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 7.3.0 + 7.3.1 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index c3130844d7f..d77ed23d8fa 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge pull request `#1481 `_ from tier4/feature/multi-level-lanelet-support diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 55060b3d3b9..db94a3f18ed 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 7.3.0 + 7.3.1 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 12177cf663a..8c8f91ceddb 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 929c22f9a7f..3f569af880e 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 7.3.0 + 7.3.1 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index b6a05f7c227..7b228778c1f 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge pull request `#1481 `_ from tier4/feature/multi-level-lanelet-support diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 9578bdb927f..c67b4d0e330 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 7.3.0 + 7.3.1 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index e9c52213db8..fae37a7c908 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index b23095deb00..e482023f511 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 7.3.0 + 7.3.1 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 6289a66c67a..035eec91233 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,15 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 0c5024660a6..12180bd9930 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 7.3.0 + 7.3.1 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index f0ca93e1ae8..04f4a493b78 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,15 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.1 (2024-12-17) +------------------ +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Merge branch 'master' into fix/math-closest-point +* Contributors: Kotaro Yoshimoto + 7.3.0 (2024-12-16) ------------------ * Merge branch 'master' into feature/multi-level-lanelet-support diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index bcb0fd35f27..44144b785c7 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 7.3.0 + 7.3.1 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 2104027c14d3e5fc70cce27189bb7e00307c8cb8 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 17 Dec 2024 11:31:53 +0900 Subject: [PATCH 134/149] Remove class `ServceWithValidation::TimeoutError` Signed-off-by: yamacir-kit --- .../concealer/service_with_validation.hpp | 14 ++------------ .../src/field_operator_application.cpp | 18 ++++++++---------- 2 files changed, 10 insertions(+), 22 deletions(-) diff --git a/external/concealer/include/concealer/service_with_validation.hpp b/external/concealer/include/concealer/service_with_validation.hpp index 9d1b0c23d9b..da002bf25be 100644 --- a/external/concealer/include/concealer/service_with_validation.hpp +++ b/external/concealer/include/concealer/service_with_validation.hpp @@ -42,17 +42,7 @@ class ServiceWithValidation { } - class TimeoutError : public common::scenario_simulator_exception::Error - { - public: - template - explicit TimeoutError(Ts &&... xs) - : common::scenario_simulator_exception::Error(std::forward(xs)...) - { - } - }; - - auto operator()(const typename T::Request::SharedPtr & request, std::size_t attempts_count = 1) + auto operator()(const typename T::Request::SharedPtr & request, std::size_t attempts_count) -> void { validateAvailability(); @@ -122,7 +112,7 @@ class ServiceWithValidation } } } - throw TimeoutError( + throw common::scenario_simulator_exception::Error( "Requested the service ", std::quoted(service_name), " ", attempts_count, " times, but was not successful."); } diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index c197d4182d1..3ee855dd3bd 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -318,10 +318,9 @@ auto FieldOperatorApplication::engage() -> void auto request = std::make_shared(); request->engage = true; try { - return requestEngage(request); - } catch (const decltype(requestEngage)::TimeoutError &) { - // ignore timeout error because this service is validated by Autoware state transition. - return; + return requestEngage(request, 1); + } catch (const common::Error &) { + return; // Ignore error because this service is validated by Autoware state transition. } }); }); @@ -369,10 +368,9 @@ auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initi std::make_shared(); request->pose.push_back(initial_pose_msg); try { - return requestInitialPose(request); - } catch (const decltype(requestInitialPose)::TimeoutError &) { - // ignore timeout error because this service is validated by Autoware state transition. - return; + return requestInitialPose(request, 1); + } catch (const common::Error &) { + return; // Ignore error because this service is validated by Autoware state transition. } }); }); @@ -417,7 +415,7 @@ auto FieldOperatorApplication::plan(const std::vectorwaypoints.push_back(each.pose); } - requestSetRoutePoints(request); + requestSetRoutePoints(request, 1); waitForAutowareStateToBeWaitingForEngage(); }); @@ -512,7 +510,7 @@ auto FieldOperatorApplication::sendCooperateCommand( request->stamp = latest_cooperate_status_array.stamp; request->commands.push_back(cooperate_command); - task_queue.delay([this, request]() { requestCooperateCommands(request); }); + task_queue.delay([this, request]() { requestCooperateCommands(request, 1); }); used_cooperate_statuses.push_back(*cooperate_status); } From 937daeb99b5fb90b5b40f5af51472719040b1e3a Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 17 Dec 2024 12:18:48 +0900 Subject: [PATCH 135/149] Remove member function `validateAvailability` and `callWithTimeoutValidation` Signed-off-by: yamacir-kit --- .../concealer/service_with_validation.hpp | 56 +++++++++---------- .../src/field_operator_application.cpp | 6 +- 2 files changed, 29 insertions(+), 33 deletions(-) diff --git a/external/concealer/include/concealer/service_with_validation.hpp b/external/concealer/include/concealer/service_with_validation.hpp index da002bf25be..14382070d07 100644 --- a/external/concealer/include/concealer/service_with_validation.hpp +++ b/external/concealer/include/concealer/service_with_validation.hpp @@ -30,6 +30,14 @@ namespace concealer template class ServiceWithValidation { + const std::string service_name; + + rclcpp::Logger logger; + + typename rclcpp::Client::SharedPtr client; + + rclcpp::WallRate validation_rate; + public: template explicit ServiceWithValidation( @@ -45,9 +53,23 @@ class ServiceWithValidation auto operator()(const typename T::Request::SharedPtr & request, std::size_t attempts_count) -> void { - validateAvailability(); + while (!client->service_is_ready()) { + RCLCPP_INFO_STREAM(logger, service_name << " service is not ready."); + validation_rate.sleep(); + } + + auto send = [this](const auto & request) { + if (auto future = client->async_send_request(request); + future.wait_for(validation_rate.period()) == std::future_status::ready) { + return std::optional::SharedFuture>(future); + } else { + RCLCPP_ERROR_STREAM(logger, service_name << " service request has timed out."); + return std::optional::SharedFuture>(); + } + }; + for (std::size_t attempt = 0; attempt < attempts_count; ++attempt, validation_rate.sleep()) { - if (const auto & service_call_result = callWithTimeoutValidation(request)) { + if (const auto & service_call_result = send(request)) { if constexpr (DetectMember_status::value) { if constexpr (std::is_same_v< tier4_external_api_msgs::msg::ResponseStatus, @@ -112,39 +134,11 @@ class ServiceWithValidation } } } + throw common::scenario_simulator_exception::Error( "Requested the service ", std::quoted(service_name), " ", attempts_count, " times, but was not successful."); } - -private: - auto validateAvailability() -> void - { - while (!client->service_is_ready()) { - RCLCPP_INFO_STREAM(logger, service_name << " service is not ready."); - validation_rate.sleep(); - } - } - - auto callWithTimeoutValidation(const typename T::Request::SharedPtr & request) - -> std::optional::SharedFuture> - { - if (auto future = client->async_send_request(request); - future.wait_for(validation_rate.period()) == std::future_status::ready) { - return future; - } else { - RCLCPP_ERROR_STREAM(logger, service_name << " service request has timed out."); - return std::nullopt; - } - } - - const std::string service_name; - - rclcpp::Logger logger; - - typename rclcpp::Client::SharedPtr client; - - rclcpp::WallRate validation_rate; }; } // namespace concealer diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 3ee855dd3bd..bc1f138e5bc 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -521,8 +521,10 @@ auto FieldOperatorApplication::setVelocityLimit(double velocity_limit) -> void task_queue.delay([this, velocity_limit]() { auto request = std::make_shared(); request->velocity = velocity_limit; - // We attempt to resend the service up to 30 times, but this number of times was determined by - // heuristics, not for any technical reason + /* + We attempt to resend the service up to 30 times, but this number of + times was determined by heuristics, not for any technical reason. + */ requestSetVelocityLimit(request, 30); }); } From d86260e1ff125bdea0e0f2f7e9051d9029da4bd8 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 17 Dec 2024 13:06:09 +0900 Subject: [PATCH 136/149] Remove free function `sudokill` Signed-off-by: yamacir-kit --- .../concealer/include/concealer/execute.hpp | 2 -- external/concealer/src/execute.cpp | 18 ------------------ 2 files changed, 20 deletions(-) diff --git a/external/concealer/include/concealer/execute.hpp b/external/concealer/include/concealer/execute.hpp index b9414370ed7..b8569ee7d5a 100644 --- a/external/concealer/include/concealer/execute.hpp +++ b/external/concealer/include/concealer/execute.hpp @@ -24,8 +24,6 @@ auto execute(const std::vector &) -> int; // Emulates shell's $(...) expression. auto dollar(const std::string & command) -> std::string; - -void sudokill(const pid_t process_id); } // namespace concealer #endif // CONCEALER__EXECUTE_HPP_ diff --git a/external/concealer/src/execute.cpp b/external/concealer/src/execute.cpp index 301e48de81a..b6a3c668700 100644 --- a/external/concealer/src/execute.cpp +++ b/external/concealer/src/execute.cpp @@ -71,22 +71,4 @@ auto dollar(const std::string & command) -> std::string return result; } } - -void sudokill(const pid_t process_id) -{ - const auto process_str = std::to_string(process_id); - - const pid_t pid = fork(); - - switch (pid) { - case -1: - std::cout << std::system_error(errno, std::system_category()).what() << std::endl; - break; - case 0: - ::execlp("sudo", "sudo", "kill", "-2", process_str.c_str(), static_cast(nullptr)); - std::cout << std::system_error(errno, std::system_category()).what() << std::endl; - break; - } -} - } // namespace concealer From 0aa63352f52f02658c94a36524ef92f35030e4fd Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 17 Dec 2024 13:57:07 +0900 Subject: [PATCH 137/149] Remove member function `TaskQueue::stopAndJoin` Signed-off-by: yamacir-kit --- .../include/concealer/field_operator_application.hpp | 9 +++++++-- external/concealer/include/concealer/task_queue.hpp | 2 -- external/concealer/src/field_operator_application.cpp | 3 --- external/concealer/src/task_queue.cpp | 4 +--- 4 files changed, 8 insertions(+), 10 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index eb38d77932b..b1a6d2a067b 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -77,8 +77,6 @@ struct FieldOperatorApplication : public rclcpp::Node, const pid_t process_id = 0; - TaskQueue task_queue; - bool initialize_was_called = false; std::string autoware_state; @@ -131,6 +129,13 @@ struct FieldOperatorApplication : public rclcpp::Node, ServiceWithValidation requestEnableAutowareControl; // clang-format on + /* + The task queue must be deconstructed before any services, so it must be + the last class data member. (Class data members are constructed in + declaration order and deconstructed in reverse order.) + */ + TaskQueue task_queue; + CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t = 0); template diff --git a/external/concealer/include/concealer/task_queue.hpp b/external/concealer/include/concealer/task_queue.hpp index 8f1b84f0f0f..4440e890268 100644 --- a/external/concealer/include/concealer/task_queue.hpp +++ b/external/concealer/include/concealer/task_queue.hpp @@ -45,8 +45,6 @@ class TaskQueue public: explicit TaskQueue(); - void stopAndJoin(); - ~TaskQueue(); template diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index bc1f138e5bc..5bad86ff9ec 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -286,9 +286,6 @@ FieldOperatorApplication::~FieldOperatorApplication() } } } - - // All tasks should be complete before the services used in them will be deinitialized. - task_queue.stopAndJoin(); } auto FieldOperatorApplication::clearRoute() -> void diff --git a/external/concealer/src/task_queue.cpp b/external/concealer/src/task_queue.cpp index 32c7b003cc8..cc2298324d1 100644 --- a/external/concealer/src/task_queue.cpp +++ b/external/concealer/src/task_queue.cpp @@ -42,7 +42,7 @@ TaskQueue::TaskQueue() { } -void TaskQueue::stopAndJoin() +TaskQueue::~TaskQueue() { if (dispatcher.joinable()) { is_stop_requested.store(true, std::memory_order_release); @@ -50,8 +50,6 @@ void TaskQueue::stopAndJoin() } } -TaskQueue::~TaskQueue() { stopAndJoin(); } - bool TaskQueue::exhausted() const noexcept { return is_exhausted.load(); } void TaskQueue::rethrow() const From 166a23986296ddfa5884d405651ac3d26f1e5660 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 17 Dec 2024 14:45:56 +0900 Subject: [PATCH 138/149] Remove macro `DEFINE_STATE_PREDICATE` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 19 --------------- .../concealer/service_with_validation.hpp | 2 +- .../concealer/transition_assertion.hpp | 23 ++++++++++--------- .../src/field_operator_application.cpp | 16 ++++++------- 4 files changed, 21 insertions(+), 39 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index b1a6d2a067b..db11a70de50 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -146,25 +146,6 @@ struct FieldOperatorApplication : public rclcpp::Node, ~FieldOperatorApplication(); - /* - NOTE: This predicate should not take the state being compared as an - argument or template parameter. Otherwise, code using this class would - need to have knowledge of the Autoware state type. - */ -#define DEFINE_STATE_PREDICATE(NAME, VALUE) \ - auto is##NAME() const noexcept { return autoware_state == #VALUE; } \ - static_assert(true, "") - - DEFINE_STATE_PREDICATE(Initializing, INITIALIZING_VEHICLE); - DEFINE_STATE_PREDICATE(WaitingForRoute, WAITING_FOR_ROUTE); - DEFINE_STATE_PREDICATE(Planning, PLANNING); - DEFINE_STATE_PREDICATE(WaitingForEngage, WAITING_FOR_ENGAGE); - DEFINE_STATE_PREDICATE(Driving, DRIVING); - DEFINE_STATE_PREDICATE(ArrivedGoal, ARRIVAL_GOAL); - DEFINE_STATE_PREDICATE(Finalizing, FINALIZING); - -#undef DEFINE_STATE_PREDICATE - auto spinSome() -> void; auto engage() -> void; diff --git a/external/concealer/include/concealer/service_with_validation.hpp b/external/concealer/include/concealer/service_with_validation.hpp index 14382070d07..6b49df8d0f0 100644 --- a/external/concealer/include/concealer/service_with_validation.hpp +++ b/external/concealer/include/concealer/service_with_validation.hpp @@ -135,7 +135,7 @@ class ServiceWithValidation } } - throw common::scenario_simulator_exception::Error( + throw common::scenario_simulator_exception::AutowareError( "Requested the service ", std::quoted(service_name), " ", attempts_count, " times, but was not successful."); } diff --git a/external/concealer/include/concealer/transition_assertion.hpp b/external/concealer/include/concealer/transition_assertion.hpp index 39121175319..ce28408a7f1 100644 --- a/external/concealer/include/concealer/transition_assertion.hpp +++ b/external/concealer/include/concealer/transition_assertion.hpp @@ -25,6 +25,7 @@ namespace concealer template struct TransitionAssertion { +protected: const std::chrono::steady_clock::time_point start; const std::chrono::seconds initialize_duration; @@ -42,16 +43,16 @@ struct TransitionAssertion #define DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(STATE) \ template \ - auto waitForAutowareStateToBe##STATE( \ + auto waitForAutowareStateToBe_##STATE( \ Thunk && thunk = [] {}, Interval interval = std::chrono::seconds(1)) \ { \ for (thunk(); not static_cast(*this).is_stop_requested.load() and \ - not static_cast(*this).is##STATE(); \ + static_cast(*this).autoware_state != #STATE; \ rclcpp::GenericRate(interval).sleep()) { \ if ( \ have_never_been_engaged and \ start + initialize_duration <= std::chrono::steady_clock::now()) { \ - const auto state = static_cast(*this).getAutowareStateName(); \ + const auto state = static_cast(*this).autoware_state; \ throw common::AutowareError( \ "Simulator waited for the Autoware state to transition to " #STATE \ ", but time is up. The current Autoware state is ", \ @@ -60,19 +61,19 @@ struct TransitionAssertion thunk(); \ } \ } \ - if constexpr (std::string_view(#STATE) == std::string_view("Driving")) { \ + if constexpr (std::string_view(#STATE) == std::string_view("DRIVING")) { \ have_never_been_engaged = false; \ } \ } \ static_assert(true) - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Initializing); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WaitingForRoute); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Planning); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WaitingForEngage); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Driving); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(ArrivedGoal); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Finalizing); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(INITIALIZING); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WAITING_FOR_ROUTE); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(PLANNING); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WAITING_FOR_ENGAGE); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(DRIVING); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(ARRIVED_GOAL); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(FINALIZING); #undef DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE }; diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 5bad86ff9ec..1c7cf0d04e7 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -311,12 +311,12 @@ auto FieldOperatorApplication::enableAutowareControl() -> void auto FieldOperatorApplication::engage() -> void { task_queue.delay([this]() { - waitForAutowareStateToBeDriving([this]() { + waitForAutowareStateToBe_DRIVING([this]() { auto request = std::make_shared(); request->engage = true; try { return requestEngage(request, 1); - } catch (const common::Error &) { + } catch (const common::AutowareError &) { return; // Ignore error because this service is validated by Autoware state transition. } }); @@ -326,13 +326,13 @@ auto FieldOperatorApplication::engage() -> void auto FieldOperatorApplication::engageable() const -> bool { rethrow(); - return task_queue.exhausted() and isWaitingForEngage(); + return task_queue.exhausted() and autoware_state == "WAITING_FOR_ENGAGE"; } auto FieldOperatorApplication::engaged() const -> bool { rethrow(); - return task_queue.exhausted() and isDriving(); + return task_queue.exhausted() and autoware_state == "DRIVING"; } auto FieldOperatorApplication::getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray @@ -350,7 +350,7 @@ auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initi { if (not std::exchange(initialize_was_called, true)) { task_queue.delay([this, initial_pose]() { - waitForAutowareStateToBeWaitingForRoute([&]() { + waitForAutowareStateToBe_WAITING_FOR_ROUTE([&]() { #if __has_include() if (getLocalizationState().state != LocalizationInitializationState::UNINITIALIZED) { return; @@ -366,7 +366,7 @@ auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initi request->pose.push_back(initial_pose_msg); try { return requestInitialPose(request, 1); - } catch (const common::Error &) { + } catch (const common::AutowareError &) { return; // Ignore error because this service is validated by Autoware state transition. } }); @@ -380,7 +380,7 @@ auto FieldOperatorApplication::plan(const std::vector(); @@ -414,7 +414,7 @@ auto FieldOperatorApplication::plan(const std::vector Date: Tue, 17 Dec 2024 16:33:18 +0900 Subject: [PATCH 139/149] Remove `getAcceleration`, `getSteeringAngle`, `getVelocity` and `getGearSign` Signed-off-by: yamacir-kit --- .../include/concealer/autoware_universe.hpp | 10 +-- external/concealer/src/autoware_universe.cpp | 76 ++++++++++--------- .../ego_entity_simulation.cpp | 32 ++++++-- 3 files changed, 70 insertions(+), 48 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 0a1d3bfd4ef..34c1bda7afd 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -100,19 +100,11 @@ class AutowareUniverse : public rclcpp::Node, auto rethrow() -> void; - auto getAcceleration() const -> double; - - auto getSteeringAngle() const -> double; - - auto getVelocity() const -> double; - auto updateLocalization() -> void; auto updateVehicleState() -> void; - auto getGearSign() const -> double; - - auto getVehicleCommand() const -> std::tuple; + auto getVehicleCommand() const -> std::tuple; auto getRouteLanelets() const -> std::vector; diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index 094b466b1c5..b6aa99411ec 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -51,21 +51,20 @@ AutowareUniverse::AutowareUniverse(bool simulate_localization) response->success = true; } else if (request->mode == ControlModeCommand::Request::MANUAL) { /* - NOTE: - MANUAL request will come when a remote override is triggered. - But scenario_simulator_v2 don't support a remote override for now. + NOTE: MANUAL request will come when a remote override is triggered. + But scenario_simulator_v2 don't support a remote override for now. */ response->success = false; } else { response->success = false; } })), - // Autoware.Universe requires localization topics to send data at 50Hz - localization_update_timer(rclcpp::create_timer( - this, get_clock(), std::chrono::milliseconds(20), [this]() { updateLocalization(); })), - // Autoware.Universe requires vehicle state topics to send data at 30Hz - vehicle_state_update_timer(rclcpp::create_timer( - this, get_clock(), std::chrono::milliseconds(33), [this]() { updateVehicleState(); })), + localization_update_timer( + rclcpp::create_timer( // Autoware.Universe requires localization topics to send data at 50Hz + this, get_clock(), std::chrono::milliseconds(20), [this]() { updateLocalization(); })), + vehicle_state_update_timer( + rclcpp::create_timer( // Autoware.Universe requires vehicle state topics to send data at 30Hz + this, get_clock(), std::chrono::milliseconds(33), [this]() { updateVehicleState(); })), localization_and_vehicle_state_update_thread(std::thread([this]() { try { while (rclcpp::ok() and not is_stop_requested.load()) { @@ -92,18 +91,6 @@ auto AutowareUniverse::rethrow() -> void } } -auto AutowareUniverse::getAcceleration() const -> double -{ - return getCommand().longitudinal.acceleration; -} - -auto AutowareUniverse::getVelocity() const -> double { return getCommand().longitudinal.velocity; } - -auto AutowareUniverse::getSteeringAngle() const -> double -{ - return getCommand().lateral.steering_tire_angle; -} - auto AutowareUniverse::updateLocalization() -> void { setAcceleration([this]() { @@ -162,7 +149,7 @@ auto AutowareUniverse::updateVehicleState() -> void setSteeringReport([this]() { SteeringReport message; message.stamp = get_clock()->now(); - message.steering_tire_angle = getSteeringAngle(); + message.steering_tire_angle = getCommand().lateral.steering_tire_angle; return message; }()); @@ -185,19 +172,40 @@ auto AutowareUniverse::updateVehicleState() -> void }()); } -auto AutowareUniverse::getGearSign() const -> double +auto AutowareUniverse::getVehicleCommand() const -> std::tuple { - /// @todo Add support for GearCommand::NONE to return 0.0 - /// @sa https://github.com/autowarefoundation/autoware.universe/blob/main/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L475 - return getGearCommand().command == GearCommand::REVERSE or - getGearCommand().command == GearCommand::REVERSE_2 - ? -1.0 - : 1.0; -} - -auto AutowareUniverse::getVehicleCommand() const -> std::tuple -{ - return std::make_tuple(getCommand(), getGearCommand()); + const auto control_command = getCommand(); + + const auto gear_command = getGearCommand(); + + auto sign_of = [](auto command) constexpr { + switch (command) { + case GearCommand::REVERSE: + case GearCommand::REVERSE_2: + return -1.0; + case GearCommand::NONE: + return 0.0; + default: + return 1.0; + } + }; + + /* + TODO Currently, acceleration is returned as an unsigned value + (`control_command.longitudinal.acceleration`) and a signed value + (`sign_of(gear_command.command)`), but this is for historical reasons and + there is no longer any reason to do so. + + return std::make_tuple( + control_command.longitudinal.velocity, + sign_of(gear_command.command) * control_command.longitudinal.acceleration, + control_command.lateral.steering_tire_angle, + gear_command.command); + */ + return std::make_tuple( + control_command.longitudinal.velocity, control_command.longitudinal.acceleration, + control_command.lateral.steering_tire_angle, sign_of(gear_command.command), + gear_command.command); } auto AutowareUniverse::getRouteLanelets() const -> std::vector 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 905527b3cb0..b8c0fe8362f 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 @@ -308,20 +308,42 @@ void EgoEntitySimulation::update( auto acceleration_by_slope = calculateAccelerationBySlope(); + const auto [speed, acceleration, tire_angle, gear_sign, gear_command] = + autoware->getVehicleCommand(); + switch (vehicle_model_type_) { case VehicleModelType::DELAY_STEER_ACC: case VehicleModelType::DELAY_STEER_ACC_GEARED: case VehicleModelType::DELAY_STEER_MAP_ACC_GEARED: case VehicleModelType::IDEAL_STEER_ACC: case VehicleModelType::IDEAL_STEER_ACC_GEARED: - input(0) = autoware->getGearSign() * (autoware->getAcceleration() + acceleration_by_slope); - input(1) = autoware->getSteeringAngle(); + /* + TODO FIX THIS!!! + + THIS IS MAYBE INCORRECT. + + SHOULD BE + gear_sign * acceleration + acceleration_by_slope + OR + signed_acceleration + acceleration_by_slope + + Currently, acceleration is obtained as an unsigned value + (`acceleration`) and a signed value (`gear_sign`), but this is for + historical reasons and there is no longer any reason to do so. + + Therefore, when resolving this TODO comment, the assignee should + remove `gear_sign` from the tuple returned by + `AutowareUniverse::getVehicleCommand`, and at the same time change + `acceleration` to a signed value. + */ + input(0) = gear_sign * (acceleration + acceleration_by_slope); + input(1) = tire_angle; break; case VehicleModelType::DELAY_STEER_VEL: case VehicleModelType::IDEAL_STEER_VEL: - input(0) = autoware->getVelocity(); - input(1) = autoware->getSteeringAngle(); + input(0) = speed; + input(1) = tire_angle; break; default: @@ -329,7 +351,7 @@ void EgoEntitySimulation::update( "Unsupported vehicle_model_type ", toString(vehicle_model_type_), "specified"); } - vehicle_model_ptr_->setGear(autoware->getGearCommand().command); + vehicle_model_ptr_->setGear(gear_command); vehicle_model_ptr_->setInput(input); vehicle_model_ptr_->update(step_time); } From 5f599bb68e9577b8f9a232ea439b14b9340e6d29 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 18 Dec 2024 10:10:05 +0900 Subject: [PATCH 140/149] Reformat Signed-off-by: yamacir-kit --- external/concealer/src/autoware_universe.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index b6aa99411ec..7467290ce20 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -178,7 +178,7 @@ auto AutowareUniverse::getVehicleCommand() const -> std::tuple Date: Wed, 18 Dec 2024 10:37:35 +0900 Subject: [PATCH 141/149] Update Docker.yaml Arm64 CI was broken, so remove it temporarily. --- .github/workflows/Docker.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/Docker.yaml b/.github/workflows/Docker.yaml index 6e550ea098d..e48250f18d0 100644 --- a/.github/workflows/Docker.yaml +++ b/.github/workflows/Docker.yaml @@ -25,7 +25,8 @@ jobs: strategy: matrix: rosdistro: [humble] - arch: [amd64, arm64] + # arch: [amd64, arm64] + arch: [amd64] steps: - name: Free Disk Space (Ubuntu) From 07364ce11d74b0a74c4aacacbfe9369a3635049f Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 18 Dec 2024 11:35:55 +0900 Subject: [PATCH 142/149] add auto HdMapUtils::isInIntersection(const lanelet::Id lanelet_id) const -> bool function Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/hdmap_utils/hdmap_utils.hpp | 2 ++ .../traffic_simulator/src/hdmap_utils/hdmap_utils.cpp | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 0f34b2c7c97..1b895770b30 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -315,6 +315,8 @@ class HdMapUtils visualization_msgs::msg::MarkerArray &, const visualization_msgs::msg::MarkerArray &) const -> void; + auto isInIntersection(const lanelet::Id) const -> bool; + auto isInLanelet(const lanelet::Id, const double s) const -> bool; auto isInRoute(const lanelet::Id, const lanelet::Ids & route) const -> bool; diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 650fb0fd802..3e147c1ba9b 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -1352,6 +1353,11 @@ auto HdMapUtils::getVectorFromPose(const geometry_msgs::msg::Pose & pose, const return vector; } +auto HdMapUtils::isInIntersection(const lanelet::Id lanelet_id) const -> bool +{ + return lanelet_map_ptr_->laneletLayer.get(lanelet_id).hasAttribute("turn_direction"); +} + auto HdMapUtils::isInLanelet(const lanelet::Id lanelet_id, const double s) const -> bool { return 0 <= s and s <= getCenterPointsSpline(lanelet_id)->getLength(); From bde1d6ccee930a82ce6cc08f0a497c83998e2dc0 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 18 Dec 2024 12:53:18 +0900 Subject: [PATCH 143/149] add test case for isInIntersection function Signed-off-by: Masaya Kataoka --- .../test/src/hdmap_utils/test_hdmap_utils.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp index dd0dda7330f..e6ba6873f0d 100644 --- a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp +++ b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp @@ -2206,6 +2206,16 @@ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) } } +/** + * @note Test for isInIntersection function + * + */ +TEST_F(HdMapUtilsTest_IntersectionMap, isInIntersection) +{ + EXPECT_TRUE(hdmap_utils.isInIntersection(662)); + EXPECT_FALSE(hdmap_utils.isInIntersection(574)); +} + /** * @note Test basic functionality. * Test obtaining stop line ids correctness with a route that has no stop lines. From d6194daacd05e2b7dd704c18bcdddcaff5a578df Mon Sep 17 00:00:00 2001 From: Release Bot Date: Wed, 18 Dec 2024 03:56:15 +0000 Subject: [PATCH 144/149] Bump version of scenario_simulator_v2 from version 7.3.1 to version 7.3.2 --- common/math/arithmetic/CHANGELOG.rst | 3 +++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 3 +++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 3 +++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 3 +++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 3 +++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 3 +++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 3 +++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 3 +++ map/kashiwanoha_map/package.xml | 2 +- map/simple_cross_map/CHANGELOG.rst | 3 +++ map/simple_cross_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 3 +++ mock/cpp_mock_scenarios/package.xml | 2 +- openscenario/openscenario_experimental_catalog/CHANGELOG.rst | 3 +++ openscenario/openscenario_experimental_catalog/package.xml | 2 +- openscenario/openscenario_interpreter/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter/package.xml | 2 +- openscenario/openscenario_interpreter_example/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter_example/package.xml | 2 +- openscenario/openscenario_interpreter_msgs/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter_msgs/package.xml | 2 +- openscenario/openscenario_preprocessor/CHANGELOG.rst | 3 +++ openscenario/openscenario_preprocessor/package.xml | 2 +- openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst | 3 +++ openscenario/openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 3 +++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 3 +++ openscenario/openscenario_validator/package.xml | 2 +- rviz_plugins/openscenario_visualization/CHANGELOG.rst | 3 +++ rviz_plugins/openscenario_visualization/package.xml | 2 +- .../real_time_factor_control_rviz_plugin/CHANGELOG.rst | 3 +++ rviz_plugins/real_time_factor_control_rviz_plugin/package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 3 +++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 3 +++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 3 +++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 3 +++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 3 +++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 3 +++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 3 +++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 3 +++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 3 +++ test_runner/scenario_test_runner/package.xml | 2 +- 58 files changed, 116 insertions(+), 29 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 8c2f6c160a9..040b50fb01c 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 4a6df50ef0a..7b8ba5ef867 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 7.3.1 + 7.3.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 edacd2450c1..48c8c03f2ac 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge pull request `#1475 `_ from tier4/fix/math-closest-point diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 2c5768dffef..89e34ea33ff 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 7.3.1 + 7.3.2 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 72637382ee5..345e3854954 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 59c822e347e..343d6e4ff1d 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 7.3.1 + 7.3.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 3590c0e5a4e..af902f27736 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 59de004336c..4a1301a14fc 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 7.3.1 + 7.3.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 88d38680851..92777b6e152 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 2b11b7d5db3..9c6aa068a2a 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 7.3.1 + 7.3.2 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index bab0b1415f0..3f8c89bca63 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 6cd22af85cf..8b3e878185b 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 7.3.1 + 7.3.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 8b344fe8499..aeca7b7aa5e 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,9 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index bfb9482383b..ea1a30499aa 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 7.3.1 + 7.3.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 e74373a3a54..e8d5155710a 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 3c2e685b8fc..326ddce9198 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 7.3.1 + 7.3.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 58ccabeafe4..4b6804cac94 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,9 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index 7415c963183..5ba3dcf59ba 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 7.3.1 + 7.3.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 5d225ae6f2d..f8ebea6ff41 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 1a6f1febf9d..d5b8a3de140 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 7.3.1 + 7.3.2 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index c3dca724a92..bc4e5afcc1e 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 91901f76fdb..60b49306c8a 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 7.3.1 + 7.3.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 599614b1734..f882a8b3858 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,9 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index dbd79ed73ed..bb115174373 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 7.3.1 + 7.3.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 a7c23a1a33a..57a0bfa8272 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index ac610e687ae..f2c4fbc4d3c 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 7.3.1 + 7.3.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 7c8b46ef5ee..6bd2ecf90ec 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 9c410d95d75..db28af3d222 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 7.3.1 + 7.3.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 f4dcede6aa6..09bf04e741a 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 3a3403bca59..fb7ca7e6078 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 7.3.1 + 7.3.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 aef6a7e7023..d612fac8c1f 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index 6b22f10b33b..cfccc32488e 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 7.3.1 + 7.3.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 88920e74917..f0cf5287b65 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,9 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 8bf819d8461..b9774bfb757 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 7.3.1 + 7.3.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 09de3672ce7..4f7d28a9f16 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,9 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index c049e91a549..5ffc4ae640c 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 7.3.1 + 7.3.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 3ac5c9a8796..98dbb7b9a2d 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 6ea002cc049..a54dea0b113 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 7.3.1 + 7.3.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 46162ecca6c..6d244515602 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,9 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point 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 d6e7a553626..2f07e6652fe 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 - 7.3.1 + 7.3.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 57e73b43af7..df2f064783f 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index db7195b4768..5d6ae9ab766 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 7.3.1 + 7.3.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 a8f95206656..31263c5d9f9 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index e5c23f826d7..23252531610 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 7.3.1 + 7.3.2 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 6e4452593ce..4b24e49dde5 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index b378fe699ef..f97c5cc8eed 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 7.3.1 + 7.3.2 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index d77ed23d8fa..4ee19a04c94 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index db94a3f18ed..739f3be602b 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 7.3.1 + 7.3.2 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 8c8f91ceddb..359d79cb001 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 3f569af880e..1846a427d1b 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 7.3.1 + 7.3.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 7b228778c1f..7339eacf853 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index c67b4d0e330..2c847afcc4c 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 7.3.1 + 7.3.2 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index fae37a7c908..3da8fda1b0e 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index e482023f511..4358708cc87 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 7.3.1 + 7.3.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 035eec91233..6e990f1890b 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 12180bd9930..b5465445bfc 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 7.3.1 + 7.3.2 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 04f4a493b78..fe33a07bb08 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,9 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.2 (2024-12-18) +------------------ + 7.3.1 (2024-12-17) ------------------ * Merge branch 'master' into fix/math-closest-point diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 44144b785c7..40482170121 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 7.3.1 + 7.3.2 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 644d5983763f6f59a8a36e4bd9bd410ad70c9710 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 18 Dec 2024 15:01:50 +0900 Subject: [PATCH 145/149] SpellCheckAll and SpellCheck do almost same thing, so merge them. Signed-off-by: Masaya Kataoka --- .github/workflows/SpellCheck.yaml | 1 + .github/workflows/SpellCheckAll.yaml | 23 ----------------------- 2 files changed, 1 insertion(+), 23 deletions(-) delete mode 100644 .github/workflows/SpellCheckAll.yaml diff --git a/.github/workflows/SpellCheck.yaml b/.github/workflows/SpellCheck.yaml index 29cd2a30942..df66e736e2f 100644 --- a/.github/workflows/SpellCheck.yaml +++ b/.github/workflows/SpellCheck.yaml @@ -20,3 +20,4 @@ jobs: local-cspell-json: .github/workflows/custom_spell.json dict-packages: | https://github.com/tier4/cspell-dicts + incremental-files-only: false diff --git a/.github/workflows/SpellCheckAll.yaml b/.github/workflows/SpellCheckAll.yaml deleted file mode 100644 index bacef52f9be..00000000000 --- a/.github/workflows/SpellCheckAll.yaml +++ /dev/null @@ -1,23 +0,0 @@ -name: Spell check all - -on: - workflow_dispatch: - pull_request: - branches: - - master - -jobs: - spell-check: - runs-on: ubuntu-latest - steps: - - name: Check out repository - uses: actions/checkout@v4 - - - name: Run spell-check - uses: autowarefoundation/autoware-github-actions/spell-check@v1 - with: - cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json - local-cspell-json: .github/workflows/custom_spell.json - dict-packages: | - https://github.com/tier4/cspell-dicts - incremental-files-only: false From 25604aafd2a9e3a7bbf3b1f95d6e42d646cf819f Mon Sep 17 00:00:00 2001 From: Release Bot Date: Wed, 18 Dec 2024 07:36:29 +0000 Subject: [PATCH 146/149] Bump version of scenario_simulator_v2 from version 7.3.2 to version 7.3.3 --- common/math/arithmetic/CHANGELOG.rst | 3 +++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 3 +++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 3 +++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 3 +++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 3 +++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 3 +++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 3 +++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 3 +++ map/kashiwanoha_map/package.xml | 2 +- map/simple_cross_map/CHANGELOG.rst | 3 +++ map/simple_cross_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 3 +++ mock/cpp_mock_scenarios/package.xml | 2 +- openscenario/openscenario_experimental_catalog/CHANGELOG.rst | 3 +++ openscenario/openscenario_experimental_catalog/package.xml | 2 +- openscenario/openscenario_interpreter/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter/package.xml | 2 +- openscenario/openscenario_interpreter_example/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter_example/package.xml | 2 +- openscenario/openscenario_interpreter_msgs/CHANGELOG.rst | 3 +++ openscenario/openscenario_interpreter_msgs/package.xml | 2 +- openscenario/openscenario_preprocessor/CHANGELOG.rst | 3 +++ openscenario/openscenario_preprocessor/package.xml | 2 +- openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst | 3 +++ openscenario/openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 3 +++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 3 +++ openscenario/openscenario_validator/package.xml | 2 +- rviz_plugins/openscenario_visualization/CHANGELOG.rst | 3 +++ rviz_plugins/openscenario_visualization/package.xml | 2 +- .../real_time_factor_control_rviz_plugin/CHANGELOG.rst | 3 +++ rviz_plugins/real_time_factor_control_rviz_plugin/package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 3 +++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 3 +++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 3 +++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 3 +++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 3 +++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 3 +++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 3 +++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 3 +++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 3 +++ test_runner/scenario_test_runner/package.xml | 2 +- 58 files changed, 116 insertions(+), 29 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 040b50fb01c..671d8468afc 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 7b8ba5ef867..43144e045cf 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 7.3.2 + 7.3.3 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index 48c8c03f2ac..a17fa198577 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 89e34ea33ff..77824dc8f6d 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 7.3.2 + 7.3.3 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 345e3854954..e1d1b8efa4f 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 343d6e4ff1d..10d0c457c8b 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 7.3.2 + 7.3.3 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index af902f27736..1d657748452 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 4a1301a14fc..12fac8ec9ba 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 7.3.2 + 7.3.3 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index 92777b6e152..e88612dcf0a 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 9c6aa068a2a..0f64c49f50c 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 7.3.2 + 7.3.3 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 3f8c89bca63..a7804196c74 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 8b3e878185b..dcc0097f4da 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 7.3.2 + 7.3.3 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index aeca7b7aa5e..e3cb1d8504e 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,9 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index ea1a30499aa..7e885acc485 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 7.3.2 + 7.3.3 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index e8d5155710a..08447e57c83 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 326ddce9198..a3eea43a4bb 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 7.3.2 + 7.3.3 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 4b6804cac94..1b8bb722c89 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,9 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index 5ba3dcf59ba..5f125934880 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 7.3.2 + 7.3.3 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 f8ebea6ff41..841b602c571 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index d5b8a3de140..cf889068b11 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 7.3.2 + 7.3.3 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index bc4e5afcc1e..364a9e9c1fe 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 60b49306c8a..73796294d47 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 7.3.2 + 7.3.3 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index f882a8b3858..ba2289b2651 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,9 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index bb115174373..839836b9883 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 7.3.2 + 7.3.3 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 57a0bfa8272..c61f4d6fe38 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index f2c4fbc4d3c..632939c53c9 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 7.3.2 + 7.3.3 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index 6bd2ecf90ec..c0051b2b9e4 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index db28af3d222..f03c224d125 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 7.3.2 + 7.3.3 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index 09bf04e741a..e2a38462061 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index fb7ca7e6078..ad4a7b2a5eb 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 7.3.2 + 7.3.3 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index d612fac8c1f..ced29dd8ced 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index cfccc32488e..68b31f7a7e2 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 7.3.2 + 7.3.3 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index f0cf5287b65..6e3f378ff3b 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,9 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index b9774bfb757..252511d03b5 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 7.3.2 + 7.3.3 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index 4f7d28a9f16..9a9bfd3553f 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,9 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 5ffc4ae640c..0bec936527a 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 7.3.2 + 7.3.3 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 98dbb7b9a2d..11392ee6e87 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index a54dea0b113..1e61fbb71fa 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 7.3.2 + 7.3.3 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 6d244515602..8165f563e08 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,9 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index 2f07e6652fe..0a1e4dec6ca 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 - 7.3.2 + 7.3.3 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index df2f064783f..0a4ce535319 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 5d6ae9ab766..cd1d275872f 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 7.3.2 + 7.3.3 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 31263c5d9f9..23efca04175 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 23252531610..719f547e0d4 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 7.3.2 + 7.3.3 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 4b24e49dde5..fa0638970a6 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index f97c5cc8eed..e2c909221c7 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 7.3.2 + 7.3.3 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 4ee19a04c94..c308850588e 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 739f3be602b..4b9e898f9cf 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 7.3.2 + 7.3.3 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 359d79cb001..04be353d19e 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 1846a427d1b..84aa2a32683 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 7.3.2 + 7.3.3 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index 7339eacf853..f83c47590de 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 2c847afcc4c..cc685f5d438 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 7.3.2 + 7.3.3 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 3da8fda1b0e..2a4320c42c2 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 4358708cc87..537c8fbb700 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 7.3.2 + 7.3.3 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 6e990f1890b..65a2102ac38 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,9 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index b5465445bfc..8e89f1ddc53 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 7.3.2 + 7.3.3 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index fe33a07bb08..51cdb19a75e 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,9 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.3 (2024-12-18) +------------------ + 7.3.2 (2024-12-18) ------------------ diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 40482170121..aaf469ce8dc 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 7.3.2 + 7.3.3 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 539580da26d5fe076e751481e3e0e6392da9e4cf Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 18 Dec 2024 11:42:41 +0100 Subject: [PATCH 147/149] style fix --- .../launch/mock_test.launch.py | 54 +++++-------------- .../src/traffic/traffic_sink.cpp | 3 +- 2 files changed, 13 insertions(+), 44 deletions(-) diff --git a/mock/cpp_mock_scenarios/launch/mock_test.launch.py b/mock/cpp_mock_scenarios/launch/mock_test.launch.py index ddfbda57f54..0a1f08e45f4 100644 --- a/mock/cpp_mock_scenarios/launch/mock_test.launch.py +++ b/mock/cpp_mock_scenarios/launch/mock_test.launch.py @@ -30,13 +30,7 @@ from launch.events import Shutdown from launch.event_handlers import OnProcessExit, OnProcessIO -from launch.actions import ( - EmitEvent, - RegisterEventHandler, - LogInfo, - TimerAction, - OpaqueFunction, -) +from launch.actions import EmitEvent, RegisterEventHandler, LogInfo, TimerAction, OpaqueFunction from launch.actions.declare_launch_argument import DeclareLaunchArgument from launch.substitutions.launch_configuration import LaunchConfiguration @@ -46,7 +40,6 @@ from pathlib import Path - class Color: BLACK = "\033[30m" RED = "\033[31m" @@ -104,10 +97,7 @@ def default_autoware_launch_file_of(architecture_type): def default_rviz_config_file(): - return ( - Path(get_package_share_directory("traffic_simulator")) - / "config/scenario_simulator_v2.rviz" - ) + return Path(get_package_share_directory("traffic_simulator")) / "config/scenario_simulator_v2.rviz" def launch_setup(context, *args, **kwargs): @@ -138,38 +128,20 @@ def launch_setup(context, *args, **kwargs): junit_path = LaunchConfiguration("junit_path", default="/tmp/output.xunit.xml") # fmt: on - print( - f"architecture_type := {architecture_type.perform(context)}" - ) - print( - f"autoware_launch_file := {autoware_launch_file.perform(context)}" - ) - print( - f"autoware_launch_package := {autoware_launch_package.perform(context)}" - ) - print( - f"consider_acceleration_by_road_slope := {consider_acceleration_by_road_slope.perform(context)}" - ) - print( - f"consider_pose_by_road_slope := {consider_pose_by_road_slope.perform(context)}" - ) - print( - f"global_frame_rate := {global_frame_rate.perform(context)}" - ) - print( - f"global_real_time_factor := {global_real_time_factor.perform(context)}" - ) + print(f"architecture_type := {architecture_type.perform(context)}") + print(f"autoware_launch_file := {autoware_launch_file.perform(context)}") + print(f"autoware_launch_package := {autoware_launch_package.perform(context)}") + print(f"consider_acceleration_by_road_slope := {consider_acceleration_by_road_slope.perform(context)}") + print(f"consider_pose_by_road_slope := {consider_pose_by_road_slope.perform(context)}") + print(f"global_frame_rate := {global_frame_rate.perform(context)}") + print(f"global_real_time_factor := {global_real_time_factor.perform(context)}") print(f"global_timeout := {global_timeout.perform(context)}") - print( - f"initialize_duration := {initialize_duration.perform(context)}" - ) + print(f"initialize_duration := {initialize_duration.perform(context)}") print(f"launch_autoware := {launch_autoware.perform(context)}") print(f"launch_rviz := {launch_rviz.perform(context)}") print(f"output_directory := {output_directory.perform(context)}") print(f"port := {port.perform(context)}") - print( - f"publish_empty_context := {publish_empty_context.perform(context)}" - ) + print(f"publish_empty_context := {publish_empty_context.perform(context)}") print(f"record := {record.perform(context)}") print(f"rviz_config := {rviz_config.perform(context)}") print(f"scenario := {scenario.perform(context)}") @@ -185,9 +157,7 @@ def make_parameters(): {"architecture_type": architecture_type}, {"autoware_launch_file": autoware_launch_file}, {"autoware_launch_package": autoware_launch_package}, - { - "consider_acceleration_by_road_slope": consider_acceleration_by_road_slope - }, + {"consider_acceleration_by_road_slope": consider_acceleration_by_road_slope}, {"consider_pose_by_road_slope": consider_pose_by_road_slope}, {"initialize_duration": initialize_duration}, {"launch_autoware": launch_autoware}, diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 1c8238f1b7b..dc41a3a3a72 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -48,8 +48,7 @@ TrafficSink::TrafficSink( auto TrafficSink::execute( [[maybe_unused]] const double current_time, [[maybe_unused]] const double step_time) -> void { - const auto entity_names = entity_manager_ptr_->getEntityNames(); - for (const auto & entity_name : entity_names) { + for (const auto & entity_name : entity_manager_ptr_->getEntityNames()) { if (isEntitySinkable(entity_name)) { entity_manager_ptr_->despawnEntity(entity_name); } From 13b1d0dd53f50caf95bbddeb3cd6221ae77cf496 Mon Sep 17 00:00:00 2001 From: Release Bot Date: Fri, 20 Dec 2024 02:01:20 +0000 Subject: [PATCH 148/149] Bump version of scenario_simulator_v2 from version 7.3.3 to version 7.3.4 --- common/math/arithmetic/CHANGELOG.rst | 7 +++++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 7 +++++++ common/math/geometry/package.xml | 2 +- common/scenario_simulator_exception/CHANGELOG.rst | 7 +++++++ common/scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 7 +++++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 7 +++++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 7 +++++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 7 +++++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 7 +++++++ map/kashiwanoha_map/package.xml | 2 +- map/simple_cross_map/CHANGELOG.rst | 7 +++++++ map/simple_cross_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 7 +++++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../openscenario_experimental_catalog/CHANGELOG.rst | 7 +++++++ .../openscenario_experimental_catalog/package.xml | 2 +- openscenario/openscenario_interpreter/CHANGELOG.rst | 7 +++++++ openscenario/openscenario_interpreter/package.xml | 2 +- .../openscenario_interpreter_example/CHANGELOG.rst | 7 +++++++ .../openscenario_interpreter_example/package.xml | 2 +- .../openscenario_interpreter_msgs/CHANGELOG.rst | 7 +++++++ .../openscenario_interpreter_msgs/package.xml | 2 +- openscenario/openscenario_preprocessor/CHANGELOG.rst | 7 +++++++ openscenario/openscenario_preprocessor/package.xml | 2 +- .../openscenario_preprocessor_msgs/CHANGELOG.rst | 7 +++++++ .../openscenario_preprocessor_msgs/package.xml | 2 +- openscenario/openscenario_utility/CHANGELOG.rst | 7 +++++++ openscenario/openscenario_utility/package.xml | 2 +- openscenario/openscenario_validator/CHANGELOG.rst | 7 +++++++ openscenario/openscenario_validator/package.xml | 2 +- rviz_plugins/openscenario_visualization/CHANGELOG.rst | 7 +++++++ rviz_plugins/openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 7 +++++++ .../real_time_factor_control_rviz_plugin/package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 7 +++++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 7 +++++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 7 +++++++ simulation/do_nothing_plugin/package.xml | 2 +- simulation/simple_sensor_simulator/CHANGELOG.rst | 7 +++++++ simulation/simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 7 +++++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 11 +++++++++++ simulation/traffic_simulator/package.xml | 2 +- simulation/traffic_simulator_msgs/CHANGELOG.rst | 7 +++++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 7 +++++++ test_runner/random_test_runner/package.xml | 2 +- test_runner/scenario_test_runner/CHANGELOG.rst | 7 +++++++ test_runner/scenario_test_runner/package.xml | 2 +- 58 files changed, 236 insertions(+), 29 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 671d8468afc..b1990388d1e 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 43144e045cf..276a02424a8 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 7.3.3 + 7.3.4 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index a17fa198577..c894f6648d3 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 77824dc8f6d..3ba8d1e03ee 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 7.3.3 + 7.3.4 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index e1d1b8efa4f..b36a3e870a5 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 10d0c457c8b..16ad5cc994d 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 7.3.3 + 7.3.4 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index 1d657748452..ee87d78b431 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 12fac8ec9ba..e89a52d035d 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 7.3.3 + 7.3.4 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index e88612dcf0a..fc067b4a694 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 0f64c49f50c..3ee7fe91ca2 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 7.3.3 + 7.3.4 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index a7804196c74..7e413de30fd 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/external/concealer/package.xml b/external/concealer/package.xml index dcc0097f4da..8e45d3dacd2 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 7.3.3 + 7.3.4 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index e3cb1d8504e..bd14425d88b 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,13 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 7e885acc485..3c5d0fc2928 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 7.3.3 + 7.3.4 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 08447e57c83..ab8ce7f88b9 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index a3eea43a4bb..af728c97db3 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 7.3.3 + 7.3.4 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 1b8bb722c89..f461df6dcab 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,13 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index 5f125934880..8184e01cdcc 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 7.3.3 + 7.3.4 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 841b602c571..b5794370fd5 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index cf889068b11..f5deee6c519 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 7.3.3 + 7.3.4 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 364a9e9c1fe..c989d5a500a 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 73796294d47..711b29922ea 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 7.3.3 + 7.3.4 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index ba2289b2651..7c1c3ba061b 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,13 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 839836b9883..f19165c9ea0 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 7.3.3 + 7.3.4 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index c61f4d6fe38..bb1543cfd65 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 632939c53c9..5a82865da2d 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 7.3.3 + 7.3.4 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index c0051b2b9e4..20aa0a56ada 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index f03c224d125..cef6794ddd9 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 7.3.3 + 7.3.4 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index e2a38462061..d303b752d48 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index ad4a7b2a5eb..cfc3443902b 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 7.3.3 + 7.3.4 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index ced29dd8ced..49b351deb7a 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index 68b31f7a7e2..a3761a96515 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 7.3.3 + 7.3.4 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 6e3f378ff3b..8b5ba7504e1 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,13 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 252511d03b5..823425bf405 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 7.3.3 + 7.3.4 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index 9a9bfd3553f..d7ebed4452c 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,13 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 0bec936527a..c8c1f3e8290 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 7.3.3 + 7.3.4 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 11392ee6e87..3b121e5e41d 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 1e61fbb71fa..9e59f7aa3f4 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 7.3.3 + 7.3.4 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 8165f563e08..319e3948ef1 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,13 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index 0a1e4dec6ca..f25ca6ae622 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 - 7.3.3 + 7.3.4 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 0a4ce535319..4bc838c2d8e 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index cd1d275872f..8ab9aba0f29 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 7.3.3 + 7.3.4 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 23efca04175..84569aa6805 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 719f547e0d4..ca030b27eb7 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 7.3.3 + 7.3.4 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index fa0638970a6..1178998e210 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index e2c909221c7..b371ee4b1d2 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 7.3.3 + 7.3.4 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index c308850588e..e3cc433f07b 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 4b9e898f9cf..dde8d966a34 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 7.3.3 + 7.3.4 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 04be353d19e..b366d4d1ea4 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 84aa2a32683..5026875237e 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 7.3.3 + 7.3.4 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index f83c47590de..5a6c14044a5 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,17 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge pull request `#1490 `_ from tier4/feature/is_in_intersection + Feature/is in intersection +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* add test case for isInIntersection function +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* add auto HdMapUtils::isInIntersection(const lanelet::Id lanelet_id) const -> bool function +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index cc685f5d438..3ec2c44f6e6 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 7.3.3 + 7.3.4 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 2a4320c42c2..38b4fb60c5c 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 537c8fbb700..42eb9a4638e 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 7.3.3 + 7.3.4 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 65a2102ac38..264e6162a7f 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,13 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 8e89f1ddc53..0bda10c41f0 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 7.3.3 + 7.3.4 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 51cdb19a75e..f9f242c5c07 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,13 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.4 (2024-12-20) +------------------ +* Merge branch 'master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Merge remote-tracking branch 'origin/master' into feature/is_in_intersection +* Contributors: Masaya Kataoka + 7.3.3 (2024-12-18) ------------------ diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index aaf469ce8dc..86707ea6f20 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 7.3.3 + 7.3.4 scenario test runner package Tatsuya Yamasaki Apache License 2.0 From 901efb4d284d30509397083ceff995b414066000 Mon Sep 17 00:00:00 2001 From: Release Bot Date: Fri, 20 Dec 2024 06:06:02 +0000 Subject: [PATCH 149/149] Bump version of scenario_simulator_v2 from version 7.3.4 to version 7.3.5 --- common/math/arithmetic/CHANGELOG.rst | 12 +++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 12 +++++ common/math/geometry/package.xml | 2 +- .../CHANGELOG.rst | 12 +++++ .../scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 12 +++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 12 +++++ common/status_monitor/package.xml | 2 +- external/concealer/CHANGELOG.rst | 50 +++++++++++++++++++ external/concealer/package.xml | 2 +- external/embree_vendor/CHANGELOG.rst | 12 +++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 12 +++++ map/kashiwanoha_map/package.xml | 2 +- map/simple_cross_map/CHANGELOG.rst | 12 +++++ map/simple_cross_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 12 +++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../CHANGELOG.rst | 12 +++++ .../package.xml | 2 +- .../openscenario_interpreter/CHANGELOG.rst | 15 ++++++ .../openscenario_interpreter/package.xml | 2 +- .../CHANGELOG.rst | 12 +++++ .../package.xml | 2 +- .../CHANGELOG.rst | 12 +++++ .../openscenario_interpreter_msgs/package.xml | 2 +- .../openscenario_preprocessor/CHANGELOG.rst | 12 +++++ .../openscenario_preprocessor/package.xml | 2 +- .../CHANGELOG.rst | 12 +++++ .../package.xml | 2 +- .../openscenario_utility/CHANGELOG.rst | 12 +++++ openscenario/openscenario_utility/package.xml | 2 +- .../openscenario_validator/CHANGELOG.rst | 12 +++++ .../openscenario_validator/package.xml | 2 +- .../openscenario_visualization/CHANGELOG.rst | 12 +++++ .../openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 12 +++++ .../package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 12 +++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 12 +++++ simulation/behavior_tree_plugin/package.xml | 2 +- simulation/do_nothing_plugin/CHANGELOG.rst | 12 +++++ simulation/do_nothing_plugin/package.xml | 2 +- .../simple_sensor_simulator/CHANGELOG.rst | 17 +++++++ .../simple_sensor_simulator/package.xml | 2 +- simulation/simulation_interface/CHANGELOG.rst | 12 +++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 17 +++++++ simulation/traffic_simulator/package.xml | 2 +- .../traffic_simulator_msgs/CHANGELOG.rst | 12 +++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 12 +++++ test_runner/random_test_runner/package.xml | 2 +- .../scenario_test_runner/CHANGELOG.rst | 13 +++++ test_runner/scenario_test_runner/package.xml | 2 +- 58 files changed, 429 insertions(+), 29 deletions(-) diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index b1990388d1e..aa16adc5c22 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 276a02424a8..5359557440a 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 7.3.4 + 7.3.5 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index c894f6648d3..13b2337a6d1 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 3ba8d1e03ee..e71c610b491 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 7.3.4 + 7.3.5 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index b36a3e870a5..a4da7e1cd2a 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 16ad5cc994d..8c7c57d12ad 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 7.3.4 + 7.3.5 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index ee87d78b431..6c02e36f7fd 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index e89a52d035d..9cd71f93546 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 7.3.4 + 7.3.5 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index fc067b4a694..12597381f87 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 3ee7fe91ca2..50ba0ad6d62 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 7.3.4 + 7.3.5 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 7e413de30fd..9ba0ac68007 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,56 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge pull request `#1488 `_ from tier4/refactor/concealer-1 + Refactor/concealer 1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Reformat +* Remove `getAcceleration`, `getSteeringAngle`, `getVelocity` and `getGearSign` +* Remove macro `DEFINE_STATE_PREDICATE` +* Remove member function `TaskQueue::stopAndJoin` +* Remove free function `sudokill` +* Remove member function `validateAvailability` and `callWithTimeoutValidation` +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Remove class `ServceWithValidation::TimeoutError` +* Remove member function `FieldOperatorApplication::shutdownAutoware` +* Add using declarations for subscribers and publishers +* Add new header file `concealer/member_detector.hpp` +* Cleanup class template `SubscriberWrapper` +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Remove header file `autoware_stream.hpp` +* Move member function implementations into `field_operator_application.cpp` +* Remove function template `listup` +* Remove function template `toAutowareStateString` +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Remove class template `FieldOperatorApplicationFor` +* Update all member function of `FieldOperatorApplication` to be non-virtual +* Update some member functions of `FieldOperatorApplication` to be non-virtual +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Move data members into base class `FieldOperatorApplication` +* Remove member function `receiveMrmState` +* Remove member function `receiveEmergencyState` +* Move base class `TransitionAssertion` to `FieldOperatorApplication` +* Move `getAutowareState` into `FieldOperatorApplication` +* Update all members of `FieldOperatorApplicationFor<...>` to be public +* Remove macro `CONCEALER_ISOLATE_STANDARD_OUTPUT` +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Update `~FieldOperatorApplication()` to be non-virtual +* Remove member function `isStopRequested` and `stopRequest` +* Remove virtual function `sendSIGINT` +* Cleanup `FieldOperatorApplication::spinSome` +* Remove member function `FieldOperatorApplication::checkAutowareProcess` +* Remove member function `FieldOperatorApplication::restrictTargetSpeed` +* Remove stream input/output operator for `TurnIndicatorsCommand` +* Update `FieldOperatorApplication::getTurnIndicatorsCommand` to pure virtual +* Remove member function `AutowareUniverse::set` +* Remove member function `AutowareUniverse::stopAndJoin` +* Remove class `concealer::Autoware` +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 8e45d3dacd2..2ea0d6d8de6 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 7.3.4 + 7.3.5 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index bd14425d88b..34389cb2c5e 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,18 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 3c5d0fc2928..74d2abe8c3a 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 7.3.4 + 7.3.5 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index ab8ce7f88b9..a2910ce13dd 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index af728c97db3..f34b5f7b3be 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 7.3.4 + 7.3.5 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 f461df6dcab..3263e339e64 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,18 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index 8184e01cdcc..81a77c0f301 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 7.3.4 + 7.3.5 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 b5794370fd5..a760632bc5b 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index f5deee6c519..d83691093fb 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 7.3.4 + 7.3.5 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index c989d5a500a..977679cb5da 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 711b29922ea..5a89fe31969 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 7.3.4 + 7.3.5 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 7c1c3ba061b..6a15fa3fc17 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,21 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge pull request `#1488 `_ from tier4/refactor/concealer-1 + Refactor/concealer 1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Remove stream input/output operator for `TurnIndicatorsCommand` +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index f19165c9ea0..bd4daf1f0bb 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 7.3.4 + 7.3.5 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index bb1543cfd65..fd8a0324aab 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 5a82865da2d..e77c7ee6ad7 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 7.3.4 + 7.3.5 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index 20aa0a56ada..38fb4c7d955 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index cef6794ddd9..a98c5eb65b0 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 7.3.4 + 7.3.5 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index d303b752d48..32b7662e190 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index cfc3443902b..dbdb0806b62 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 7.3.4 + 7.3.5 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 49b351deb7a..f1fbbfd1283 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index a3761a96515..f45bf0f082d 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 7.3.4 + 7.3.5 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 8b5ba7504e1..779618ec6d5 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,18 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 823425bf405..a47baf3892b 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 7.3.4 + 7.3.5 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index d7ebed4452c..200ed160363 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,18 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index c8c1f3e8290..497198557e8 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 7.3.4 + 7.3.5 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 3b121e5e41d..db1a5c86dd8 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 9e59f7aa3f4..4c94d14dc72 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 7.3.4 + 7.3.5 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 319e3948ef1..c6d26f61b5b 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,18 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection 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 f25ca6ae622..41f06a4ee79 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 - 7.3.4 + 7.3.5 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 4bc838c2d8e..1907f27a9ff 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 8ab9aba0f29..eb685eeeeeb 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 7.3.4 + 7.3.5 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 84569aa6805..47a0d0a36c1 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index ca030b27eb7..96cd2d00545 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 7.3.4 + 7.3.5 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 1178998e210..1c4c69cac35 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index b371ee4b1d2..9db341168e1 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 7.3.4 + 7.3.5 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index e3cc433f07b..5cf69bb503f 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,23 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge pull request `#1488 `_ from tier4/refactor/concealer-1 + Refactor/concealer 1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Remove `getAcceleration`, `getSteeringAngle`, `getVelocity` and `getGearSign` +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Remove member function `AutowareUniverse::set` +* Remove class `concealer::Autoware` +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index dde8d966a34..2ff7d5b7519 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 7.3.4 + 7.3.5 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index b366d4d1ea4..e4c7440adc2 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 5026875237e..01661d060bf 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 7.3.4 + 7.3.5 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index 5a6c14044a5..4681820ff34 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,23 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge pull request `#1488 `_ from tier4/refactor/concealer-1 + Refactor/concealer 1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Remove class template `FieldOperatorApplicationFor` +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Remove member function `FieldOperatorApplication::restrictTargetSpeed` +* Remove class `concealer::Autoware` +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge pull request `#1490 `_ from tier4/feature/is_in_intersection diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 3ec2c44f6e6..57fb4c28ae3 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 7.3.4 + 7.3.5 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 38b4fb60c5c..c85edc363de 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 42eb9a4638e..ec34af2836b 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 7.3.4 + 7.3.5 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 264e6162a7f..e642432dfff 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 0bda10c41f0..eb67a633808 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 7.3.4 + 7.3.5 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index f9f242c5c07..656922eada0 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,19 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.5 (2024-12-20) +------------------ +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge branch 'master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Merge remote-tracking branch 'origin/master' into refactor/concealer-1 +* Remove parameter `random_offset` from scenario `sample.yaml` +* Contributors: Tatsuya Yamasaki, yamacir-kit + 7.3.4 (2024-12-20) ------------------ * Merge branch 'master' into feature/is_in_intersection diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 86707ea6f20..b54ac457339 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 7.3.4 + 7.3.5 scenario test runner package Tatsuya Yamasaki Apache License 2.0