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 e9d14665928..0202d488eb3 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 @@ -114,17 +114,17 @@ class FieldOperatorApplicationFor CONCEALER_PUBLIC explicit FieldOperatorApplicationFor(Ts &&... xs) : FieldOperatorApplication(std::forward(xs)...), // clang-format off - getAckermannControlCommand("/control/command/control_cmd", *this), - getAutowareState("/api/iv_msgs/autoware/state", *this), - getCooperateStatusArray("/api/external/get/rtc_status", *this, [this](const auto & v) { latest_cooperate_status_array = v; }), - getEmergencyState("/api/external/get/emergency", *this, [this](const auto & v) { receiveEmergencyState(v); }), + getAckermannControlCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), + getAutowareState("/api/iv_msgs/autoware/state", 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); }), #if __has_include() - getLocalizationState("/api/localization/initialization_state", *this), + getLocalizationState("/api/localization/initialization_state", rclcpp::QoS(1).transient_local(), *this), #endif - getMrmState("/api/fail_safe/mrm_state", *this, [this](const auto & v) { receiveMrmState(v); }), - getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", *this), - getTrajectory("/api/iv_msgs/planning/scenario_planning/trajectory", *this), - getTurnIndicatorsCommandImpl("/control/command/turn_indicators_cmd", *this), + 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), requestCooperateCommands("/api/external/set/rtc_commands", *this), requestEngage("/api/external/set/engage", *this), diff --git a/external/concealer/include/concealer/subscriber_wrapper.hpp b/external/concealer/include/concealer/subscriber_wrapper.hpp index e8869fb86ce..8247a872e3a 100644 --- a/external/concealer/include/concealer/subscriber_wrapper.hpp +++ b/external/concealer/include/concealer/subscriber_wrapper.hpp @@ -41,10 +41,12 @@ class SubscriberWrapper template SubscriberWrapper( - const std::string & topic, NodeInterface & autoware_interface, + const std::string & topic, const rclcpp::QoS & quality_of_service, + NodeInterface & autoware_interface, const std::function & callback = {}) : subscription(autoware_interface.template create_subscription( - topic, 1, [this, callback](const typename MessageType::ConstSharedPtr message) { + 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) { diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index 6016eb0d322..dbecc8b51d9 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -17,11 +17,12 @@ namespace concealer { AutowareUniverse::AutowareUniverse() -: getAckermannControlCommand("/control/command/control_cmd", *this), - getGearCommandImpl("/control/command/gear_cmd", *this), - getTurnIndicatorsCommand("/control/command/turn_indicators_cmd", *this), +: getAckermannControlCommand("/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", *this), + "/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), + *this), setAcceleration("/localization/acceleration", *this), setOdometry("/localization/kinematic_state", *this), setSteeringReport("/vehicle/status/steering_status", *this), diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/user_defined_value_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/user_defined_value_condition.hpp index 585d6d9e273..7ca1707fd9e 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/user_defined_value_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/user_defined_value_condition.hpp @@ -45,8 +45,6 @@ class UserDefinedValueCondition : private SimulatorCore::NonStandardOperation std::function evaluate_value; - static uint64_t magic_subscription_counter; - public: const String name; 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 fd99947d906..29d0424434d 100644 --- a/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp @@ -33,47 +33,77 @@ namespace openscenario_interpreter inline namespace syntax { template -struct MagicSubscription : private rclcpp::Node, public T +struct MagicSubscription : private T { - std::promise promise; + struct Node : public rclcpp::Node + { + std::atomic_bool stop_requested = false; - std::thread thread; + std::mutex mutex; - std::exception_ptr thrown; + std::exception_ptr thrown; - typename rclcpp::Subscription::SharedPtr subscription; + std::thread thread; -public: - explicit MagicSubscription(const std::string & node_name, const std::string & topic_name) - : rclcpp::Node(node_name), - thread( - [this](auto future) { - while (rclcpp::ok() and - future.wait_for(std::chrono::milliseconds(1)) == std::future_status::timeout) { + explicit Node() + : rclcpp::Node("magic_subscription"), thread([this]() { + while (rclcpp::ok() and not stop_requested) { try { rclcpp::spin_some(get_node_base_interface()); } catch (...) { + auto lock = std::lock_guard(mutex); thrown = std::current_exception(); } } - }, - std::move(promise.get_future())), - subscription(create_subscription(topic_name, 1, [this](const typename T::SharedPtr message) { - static_cast(*this) = *message; - })) + }) + { + } + + ~Node() + { + if (thread.joinable() and not stop_requested.exchange(true)) { + thread.join(); + } + } + + auto rethrow() + { + if (auto lock = std::lock_guard(mutex); thrown) { + std::rethrow_exception(thrown); + } + } + }; + + static inline std::size_t count = 0; + + static inline std::unique_ptr node = nullptr; + + typename rclcpp::Subscription::SharedPtr subscription; + + explicit MagicSubscription(const std::string & topic_name) { + if (not count++) { + node = std::make_unique(); + } + + subscription = node->template create_subscription( + topic_name, rclcpp::QoS(1).best_effort(), + [this](const typename T::SharedPtr message) { static_cast(*this) = *message; }); } ~MagicSubscription() { - if (thread.joinable()) { - promise.set_value(); - thread.join(); + if (not --count) { + node.reset(); } } -}; -uint64_t UserDefinedValueCondition::magic_subscription_counter = 0; + auto get() const -> const auto & + { + node->rethrow(); + return static_cast(*this); + } +}; UserDefinedValueCondition::UserDefinedValueCondition(const pugi::xml_node & node, Scope & scope) : name(readAttribute("name", node, scope)), @@ -134,31 +164,29 @@ UserDefinedValueCondition::UserDefinedValueCondition(const pugi::xml_node & node using tier4_simulation_msgs::msg::UserDefinedValueType; evaluate_value = - [&, current_message = std::make_shared>( - result.str(1) + "_subscription_" + std::to_string(++magic_subscription_counter), - result.str(0))]() { - auto evaluate = [](const auto & user_defined_value) { - switch (user_defined_value.type.data) { + [this, subscriber = std::make_shared>(result.str(0))]() { + if (const auto message = subscriber->get(); message.value.empty()) { + return unspecified; + } else { + switch (message.type.data) { case UserDefinedValueType::BOOLEAN: - return make(user_defined_value.value); + return make(message.value); case UserDefinedValueType::DATE_TIME: - return make(user_defined_value.value); + return make(message.value); case UserDefinedValueType::DOUBLE: - return make(user_defined_value.value); + return make(message.value); case UserDefinedValueType::INTEGER: - return make(user_defined_value.value); + return make(message.value); case UserDefinedValueType::STRING: - return make(user_defined_value.value); + return make(message.value); case UserDefinedValueType::UNSIGNED_INT: - return make(user_defined_value.value); + return make(message.value); case UserDefinedValueType::UNSIGNED_SHORT: - return make(user_defined_value.value); + return make(message.value); default: return unspecified; } - }; - - return not current_message->value.empty() ? evaluate(*current_message) : unspecified; + } }; #else throw SyntaxError( diff --git a/test_runner/scenario_test_runner/scenario/ByValueCondition.UserDefinedValueCondition.yaml b/test_runner/scenario_test_runner/scenario/ByValueCondition.UserDefinedValueCondition.yaml index 0b340721977..427fcce95df 100644 --- a/test_runner/scenario_test_runner/scenario/ByValueCondition.UserDefinedValueCondition.yaml +++ b/test_runner/scenario_test_runner/scenario/ByValueCondition.UserDefinedValueCondition.yaml @@ -8,7 +8,9 @@ OpenSCENARIO: ParameterDeclarations: ParameterDeclaration: [] CatalogLocations: - CatalogLocation: [] + VehicleCatalog: + Directory: + path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle RoadNetwork: LogicFile: filepath: $(find-pkg-share kashiwanoha_map)/map @@ -17,45 +19,32 @@ OpenSCENARIO: Entities: ScenarioObject: - name: ego - Vehicle: + CatalogReference: + catalogName: sample_vehicle + entryName: sample_vehicle + ObjectController: + Controller: + name: '' + Properties: + Property: + - name: isEgo + value: 'true' + - name: barricade + MiscObject: + mass: 1.0 + miscObjectCategory: obstacle name: '' - vehicleCategory: car BoundingBox: Center: x: 0 y: 0 - z: 1.25 + z: 0 Dimensions: - width: 2.25 - length: 4.77 - height: 2.5 - Performance: - maxSpeed: 50 - maxAcceleration: INF - maxDeceleration: INF - Axles: - FrontAxle: - maxSteering: 0.5236 - wheelDiameter: 0.6 - trackWidth: 4 - positionX: 1 - positionZ: 0.3 - RearAxle: - maxSteering: 0.5236 - wheelDiameter: 0.6 - trackWidth: 4 - positionX: 0 - positionZ: 0.3 + width: 10 + length: 1 + height: 10 Properties: Property: [] - ObjectController: - Controller: - name: '' - Properties: - Property: - - { name: isEgo, value: 'true' } - - { name: maxJerk, value: 1.5 } - - { name: minJerk, value: -1.5 } Storyboard: Init: Actions: @@ -69,33 +58,30 @@ OpenSCENARIO: laneId: '34513' s: 0 offset: 0 - Orientation: + Orientation: &ORIENTATION_ZERO type: relative h: 0 p: 0 r: 0 - RoutingAction: AcquirePositionAction: - Position: + Position: &EGO_DESTINATION LanePosition: roadId: '' laneId: '34507' s: 50 offset: 0 - Orientation: - type: relative - h: 0 - p: 0 - r: 0 - - LongitudinalAction: - SpeedAction: - SpeedActionDynamics: - dynamicsDimension: time - value: 0 - dynamicsShape: step - SpeedActionTarget: - AbsoluteTargetSpeed: - value: 2.778 + Orientation: *ORIENTATION_ZERO + - entityRef: barricade + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: 34513 + s: 20 + offset: 0 + Orientation: *ORIENTATION_ZERO Story: - name: '' Act: @@ -120,7 +106,7 @@ OpenSCENARIO: conditionEdge: none ByValueCondition: SimulationTimeCondition: - value: 120 + value: 180 rule: greaterThan - Condition: - name: '' @@ -133,26 +119,8 @@ OpenSCENARIO: - entityRef: ego EntityCondition: ReachPositionCondition: - Position: - LanePosition: - roadId: '' - laneId: '34507' - s: 50 - offset: 0 - Orientation: - type: relative - h: 0 - p: 0 - r: 0 + Position: *EGO_DESTINATION tolerance: 0.5 - - name: '' - delay: 0 - conditionEdge: none - ByValueCondition: - UserDefinedValueCondition: - name: ego.currentState - rule: equalTo - value: ArrivedGoal Action: - name: '' UserDefinedAction: @@ -163,23 +131,178 @@ OpenSCENARIO: StartTrigger: ConditionGroup: - Condition: - - name: '' + - &COUNT_UP + name: '' delay: 0 conditionEdge: none ByValueCondition: UserDefinedValueCondition: - name: /count_up - value: 500 + name: /count_up # ros2 run openscenario_interpreter_example count_up + value: 100 rule: greaterThan - # - Condition: - - name: '' - delay: 0 - conditionEdge: none - ByValueCondition: - UserDefinedValueCondition: - name: /timeout - value: true - rule: equalTo + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 10 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 20 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 30 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 40 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 50 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 60 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 70 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 80 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 90 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 100 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 110 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 120 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 130 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 140 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 150 Action: - name: '' UserDefinedAction: