diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 71dbfe2a58d..2c80e36baa2 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -83,6 +84,7 @@ struct FieldOperatorApplication : public rclcpp::Node using CooperateCommands = tier4_rtc_msgs::srv::CooperateCommands; using Engage = tier4_external_api_msgs::srv::Engage; using InitializeLocalization = autoware_adapi_v1_msgs::srv::InitializeLocalization; + using SetRoute = autoware_adapi_v1_msgs::srv::SetRoute; using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; using AutoModeWithModule = tier4_rtc_msgs::srv::AutoModeWithModule; using SetVelocityLimit = tier4_external_api_msgs::srv::SetVelocityLimit; @@ -109,6 +111,7 @@ struct FieldOperatorApplication : public rclcpp::Node Service requestCooperateCommands; Service requestEngage; Service requestInitialPose; + Service requestSetRoute; Service requestSetRoutePoints; Service requestSetRtcAutoMode; Service requestSetVelocityLimit; @@ -159,7 +162,24 @@ struct FieldOperatorApplication : public rclcpp::Node auto initialize(const geometry_msgs::msg::Pose &) -> void; - auto plan(const std::vector &, const bool) -> void; + [[deprecated( + "This function was deprecated since version 16.5.0 (released on 20250603). It will be deleted " + "after a half-year transition period (~20251203). Please use other overloads instead.")]] auto + plan(const std::vector &, const bool) -> void; + +#if __has_include() + using RouteOption = autoware_adapi_v1_msgs::msg::RouteOption; +#else + using RouteOption = void; +#endif + + auto plan( + const geometry_msgs::msg::Pose & goal, const std::vector &, + const RouteOption &) -> void; + + auto plan( + const geometry_msgs::msg::Pose & goal, + const std::vector &, const RouteOption &) -> void; auto clearRoute() -> void; diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 78451624a43..ec3943ee297 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -156,7 +156,8 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) 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. + // NOTE: routing takes a long time to return. But the specified duration is not decided by any technical reasons. + requestSetRoute("/api/routing/set_route", *this, std::chrono::seconds(10)), 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), @@ -357,7 +358,67 @@ auto FieldOperatorApplication::plan( { assert(not route.empty()); - task_queue.delay([this, route, allow_goal_modification]() { + std::vector waypoints; + if (route.size() > 1) { + std::transform( + route.begin(), route.end() - 1, std::back_inserter(waypoints), + [](const auto & stamped_pose) { return stamped_pose.pose; }); + } + + RouteOption route_option; + if (DetectMember_allow_goal_modification::value) { + route_option.allow_goal_modification = allow_goal_modification; + } + + plan(route.back().pose, waypoints, route_option); +} + +template < + typename Request, typename Waypoint, + typename = std::enable_if_t< + std::is_same_v || + std::is_same_v > > +static auto make( + const geometry_msgs::msg::Pose & goal, const std::vector & waypoints, + const FieldOperatorApplication::RouteOption & option) -> std::shared_ptr +{ + auto request = std::make_shared(); + + request->header.frame_id = "map"; + request->goal = goal; + + if constexpr (std::is_same_v) { + request->waypoints.assign(waypoints.begin(), waypoints.end()); + } else if constexpr (std::is_same_v) { + request->segments.assign(waypoints.begin(), waypoints.end()); + } + + /* + 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 ( + DetectMember_option::value and + DetectMember_allow_goal_modification().option)>::value) { + request->option.allow_goal_modification = option.allow_goal_modification; + } + + return request; +} + +auto FieldOperatorApplication::plan( + const geometry_msgs::msg::Pose & goal, const std::vector & waypoints, + const RouteOption & option) -> void +{ + task_queue.delay([this, goal, waypoints, option]() { switch (const auto state = getLegacyAutowareState(); state.value) { default: throw common::AutowareError( @@ -370,39 +431,35 @@ auto FieldOperatorApplication::plan( waitForAutowareStateToBe(state, LegacyAutowareState::waiting_for_route); [[fallthrough]]; case LegacyAutowareState::waiting_for_route: - requestSetRoutePoints( - [&]() { - auto request = std::make_shared(); - - request->header = route.back().header; - request->goal = route.back().pose; - - for (const auto & each : route | boost::adaptors::sliced(0, route.size() - 1)) { - request->waypoints.push_back(each.pose); - } - - /* - 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 ( - DetectMember_option::value and - DetectMember_allow_goal_modification< - decltype(std::declval().option)>::value) { - request->option.allow_goal_modification = allow_goal_modification; - } + requestSetRoutePoints(make(goal, waypoints, option), 30); + waitForAutowareStateToBe( + LegacyAutowareState::waiting_for_route, LegacyAutowareState::planning); + waitForAutowareStateToBe( + LegacyAutowareState::planning, LegacyAutowareState::waiting_for_engage); + break; + } + }); +} - return request; - }(), - 30); +auto FieldOperatorApplication::plan( + const geometry_msgs::msg::Pose & goal, + const std::vector & waypoints, + const RouteOption & option) -> void +{ + task_queue.delay([this, goal, waypoints, option]() { + switch (const auto state = getLegacyAutowareState(); state.value) { + default: + throw common::AutowareError( + "The simulator attempted to send a goal to Autoware, but was aborted because Autoware's " + "current state is ", + state, "."); + case LegacyAutowareState::initializing: + // The initial pose has been sent but has not yet reached Autoware. + case LegacyAutowareState::arrived_goal: + waitForAutowareStateToBe(state, LegacyAutowareState::waiting_for_route); + [[fallthrough]]; + case LegacyAutowareState::waiting_for_route: + requestSetRoute(make(goal, waypoints, option), 30); waitForAutowareStateToBe( LegacyAutowareState::waiting_for_route, LegacyAutowareState::planning); waitForAutowareStateToBe( diff --git a/openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp b/openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp index 1f92fda1e88..d5a0d8a4f20 100644 --- a/openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp @@ -53,45 +53,43 @@ auto AcquirePositionAction::start() -> void return applyAcquirePositionAction(actor, static_cast(position), options); }); - auto allow_goal_modification_from_parameter = [&]() -> bool { - try { - return ref(std::string("AcquirePositionAction.allow_goal_modification")); - } catch (const SyntaxError &) { - // default value of allow_goal_modification - return false; - } - }(); - - auto get_allow_goal_modification_from_property = - [this](const EntityRef & entity_ref) -> std::optional { + auto get_from_controller_property = + [this](const EntityRef & entity_ref, const std::string & property_name) -> std::optional { if (const auto & entity = global().entities->ref(entity_ref); entity.template is()) { const auto & object_controller = entity.as().object_controller; if (object_controller.isAutoware() && object_controller.is()) { auto controller = object_controller.as(); - if (controller.properties.contains("allowGoalModification")) { - return controller.properties.template get("allowGoalModification"); + if (controller.properties.contains(property_name)) { + return controller.properties.template get(property_name); } } } return std::nullopt; }; + auto get_from_parameter = + [&](const std::string & parameter_name, const bool default_value) -> bool { + try { + return ref(parameter_name); + } catch (const SyntaxError &) { + return default_value; + } + }; + for (const auto & actor : actors) { actor.apply([&](const auto & object) { - traffic_simulator::RouteOption options; - options.allow_goal_modification = [&]() { - if (auto allow_goal_modification_from_property = - get_allow_goal_modification_from_property(object); - allow_goal_modification_from_property.has_value()) { + traffic_simulator::v2::RouteOption route_option; + route_option.allow_goal_modification = [&]() { + if (auto from_property = get_from_controller_property(object, "allowGoalModification")) { // property specification takes precedence - return allow_goal_modification_from_property.value(); + return from_property.value(); } else { - return allow_goal_modification_from_parameter; + return get_from_parameter("RoutingAction__allow_goal_modification", false); } }(); - apply(acquire_position, position, object, options); + apply(acquire_position, position, object, route_option); }); } } diff --git a/openscenario/openscenario_interpreter/src/syntax/assign_route_action.cpp b/openscenario/openscenario_interpreter/src/syntax/assign_route_action.cpp index 775b0ec4af4..c1b00449a67 100644 --- a/openscenario/openscenario_interpreter/src/syntax/assign_route_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/assign_route_action.cpp @@ -16,8 +16,11 @@ #include #include #include +#include +#include #include #include +#include #include namespace openscenario_interpreter @@ -53,10 +56,47 @@ auto AssignRouteAction::run() -> void {} auto AssignRouteAction::start() -> void { + auto get_from_controller_property = + [this](const EntityRef & entity_ref, const std::string & property_name) -> std::optional { + if (const auto & entity = global().entities->ref(entity_ref); + entity.template is()) { + const auto & object_controller = entity.as().object_controller; + if (object_controller.isAutoware() && object_controller.is()) { + auto controller = object_controller.as(); + if (controller.properties.contains(property_name)) { + return controller.properties.template get(property_name); + } + } + } + return std::nullopt; + }; + + auto get_from_parameter = + [&](const std::string & parameter_name, const bool default_value) -> bool { + try { + return ref(parameter_name); + } catch (const SyntaxError &) { + return default_value; + } + }; + + traffic_simulator::v2::RouteOption route_option; + route_option.use_lane_ids_for_routing = + get_from_parameter("RoutingAction__use_lane_ids_for_routing", false); + for (const auto & actor : actors) { actor.apply([&](const auto & object) { + route_option.allow_goal_modification = [&]() { + if (auto from_property = get_from_controller_property(object, "allowGoalModification")) { + // property specification takes precedence + return from_property.value(); + } else { + return get_from_parameter("RoutingAction__allow_goal_modification", false); + } + }(); applyAssignRouteAction( - object, static_cast>(route.as())); + object, static_cast>(route.as()), + route_option); }); } } diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/route_option.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/route_option.hpp index 40391d1acf6..d3d37c6ced3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/route_option.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/route_option.hpp @@ -19,11 +19,29 @@ namespace traffic_simulator { inline namespace route_option { -inline namespace v1 +inline namespace v2 { struct RouteOption { bool allow_goal_modification = false; + bool use_lane_ids_for_routing = false; +}; +} // namespace v2 + +namespace v1 +{ +struct RouteOption +{ + bool allow_goal_modification = false; + + // conversion without loss of information + operator v2::RouteOption() const + { + v2::RouteOption v2; + v2.allow_goal_modification = allow_goal_modification; + v2.use_lane_ids_for_routing = false; + return v2; + } }; } // namespace v1 } // namespace route_option 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 8333faf46d9..b141fdcd717 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -82,21 +82,23 @@ class EgoEntity : public VehicleEntity, private concealer::FieldOperatorApplicat void requestAcquirePosition(const CanonicalizedLaneletPose &) override; - void requestAcquirePosition(const CanonicalizedLaneletPose &, const RouteOption &) override; + void requestAcquirePosition( + const CanonicalizedLaneletPose &, const traffic_simulator::RouteOption &) override; void requestAcquirePosition(const geometry_msgs::msg::Pose &) override; - void requestAcquirePosition(const geometry_msgs::msg::Pose &, const RouteOption &) override; + void requestAcquirePosition( + const geometry_msgs::msg::Pose &, const traffic_simulator::RouteOption &) override; void requestAssignRoute(const std::vector &) override; void requestAssignRoute( - const std::vector &, const RouteOption &) override; + const std::vector &, const traffic_simulator::RouteOption &) override; void requestAssignRoute(const std::vector &) override; void requestAssignRoute( - const std::vector &, const RouteOption &) override; + const std::vector &, const traffic_simulator::RouteOption &) override; auto requestFollowTrajectory( const std::shared_ptr &) -> void override; diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index c1c774155e7..e1a27fe1eaa 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -197,78 +197,150 @@ void EgoEntity::onUpdate(double current_time, double step_time) void EgoEntity::requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) { - RouteOption options; - options.allow_goal_modification = get_parameter_or("allow_goal_modification", false); - return requestAcquirePosition(lanelet_pose, options); + traffic_simulator::RouteOption option; + option.allow_goal_modification = get_parameter_or("allow_goal_modification", false); + return requestAcquirePosition(lanelet_pose, option); } void EgoEntity::requestAcquirePosition(const geometry_msgs::msg::Pose & map_pose) { - RouteOption options; - options.allow_goal_modification = get_parameter_or("allow_goal_modification", false); - return requestAcquirePosition(map_pose, options); + traffic_simulator::RouteOption option; + option.allow_goal_modification = get_parameter_or("allow_goal_modification", false); + return requestAcquirePosition(map_pose, option); } void EgoEntity::requestAcquirePosition( - const CanonicalizedLaneletPose & lanelet_pose, const RouteOption & options) + const CanonicalizedLaneletPose & lanelet_pose, const traffic_simulator::RouteOption & option) { - requestAssignRoute({lanelet_pose}, options); + requestAssignRoute({lanelet_pose}, option); } void EgoEntity::requestAcquirePosition( - const geometry_msgs::msg::Pose & map_pose, const RouteOption & options) + const geometry_msgs::msg::Pose & map_pose, const traffic_simulator::RouteOption & option) { - requestAssignRoute({map_pose}, options); + requestAssignRoute({map_pose}, option); } -void EgoEntity::requestAssignRoute(const std::vector & waypoints) +void EgoEntity::requestAssignRoute(const std::vector & route) { - std::vector route; - for (const auto & waypoint : waypoints) { - route.push_back(static_cast(waypoint)); + std::vector route_poses; + for (const auto & lanelet_pose : route) { + route_poses.push_back(static_cast(lanelet_pose)); } - requestAssignRoute(route); + traffic_simulator::RouteOption option; + option.allow_goal_modification = get_parameter_or("allow_goal_modification", false); + return requestAssignRoute(route_poses, option); } -void EgoEntity::requestAssignRoute(const std::vector & waypoints) +void EgoEntity::requestAssignRoute(const std::vector & route) { - RouteOption options; - options.allow_goal_modification = get_parameter_or("allow_goal_modification", false); - return requestAssignRoute(waypoints, options); + traffic_simulator::RouteOption option; + option.allow_goal_modification = get_parameter_or("allow_goal_modification", false); + return requestAssignRoute(route, option); } void EgoEntity::requestAssignRoute( - const std::vector & waypoints, const RouteOption & options) -{ - std::vector route; - for (const auto & waypoint : waypoints) { - route.push_back(static_cast(waypoint)); + const std::vector & route, + const traffic_simulator::RouteOption & option) +{ + if (option.use_lane_ids_for_routing) { + concealer::FieldOperatorApplication::RouteOption route_option; + route_option.allow_goal_modification = option.allow_goal_modification; + + assert(not route.empty()); + + auto goal = static_cast(route.back()); + using autoware_adapi_v1_msgs::msg::RouteSegment; + auto make_segment = [](const int64_t id) { + RouteSegment segment; + segment.preferred.id = id; + segment.preferred.type = "lane"; + // NOTE: If traffic_simulator supports to the overlap of lanelet pose, + // the second and subsequent lanelet pose are packed into segment.alternatives. + return segment; + }; + + std::vector route_segments; + traffic_simulator::RoutingConfiguration routing_configuration; + routing_configuration.allow_lane_change = true; + if (auto current_lanelet_pose = getCanonicalizedLaneletPose()) { + route_segments.push_back(make_segment(current_lanelet_pose->getLaneletId())); + } else { + throw common::Error( + "Failed to get current lanelet of ego entity. (", __FILE__, ":", __LINE__, ")"); + } + + for (const auto & route_point : route) { + // NOTE: Interpolating between lanelets because set route API requires continuous lanelet ids on lanelet graph + auto segment_route = hdmap_utils_ptr_->getRoute( + route_segments.back().preferred.id, route_point.getLaneletId(), routing_configuration); + std::transform( + segment_route.begin(), segment_route.end(), std::back_inserter(route_segments), + [make_segment](const int64_t & lanelet_id) { return make_segment(lanelet_id); }); + } + + // NOTE: Make the lanelet IDs unique, because set route API recognizes duplicate IDs as loops and does not accept. + route_segments.erase( + std::unique( + route_segments.begin(), route_segments.end(), + [](const RouteSegment & a, const RouteSegment & b) { + return a.preferred.id == b.preferred.id; + }), + route_segments.end()); + + requestClearRoute(); + + if (not initialized) { + initialize(getMapPose()); + plan(goal, route_segments, route_option); + // NOTE: engage() will be executed at simulation-time 0. + } else { + plan(goal, route_segments, route_option); + FieldOperatorApplication::engage(); + } + } else { + std::vector route_poses; + for (const auto & lanelet_pose : route) { + route_poses.push_back(static_cast(lanelet_pose)); + } + requestAssignRoute(route_poses, option); } - requestAssignRoute(route, options); } void EgoEntity::requestAssignRoute( - const std::vector & waypoints, const RouteOption & options) -{ - std::vector route; - for (const auto & waypoint : waypoints) { - geometry_msgs::msg::PoseStamped pose_stamped; - { - pose_stamped.header.frame_id = "map"; - pose_stamped.pose = waypoint; + const std::vector & route, + const traffic_simulator::RouteOption & option) +{ + if (option.use_lane_ids_for_routing) { + std::vector lanelet_poses; + for (const auto & pose : route) { + if (auto lanelet_pose = pose::toCanonicalizedLaneletPose(pose, false)) { + lanelet_poses.push_back(*lanelet_pose); + } } - route.push_back(pose_stamped); - } + requestAssignRoute(lanelet_poses, option); + } else { + requestClearRoute(); - requestClearRoute(); + concealer::FieldOperatorApplication::RouteOption route_option; + route_option.allow_goal_modification = option.allow_goal_modification; - if (not initialized) { - initialize(getMapPose()); - plan(route, options.allow_goal_modification); - // NOTE: engage() will be executed at simulation-time 0. - } else { - plan(route, options.allow_goal_modification); - FieldOperatorApplication::engage(); + assert(not route.empty()); + + auto goal = route.back(); + std::vector waypoints; + if (route.size() > 1) { + waypoints.assign(route.begin(), route.end() - 1); + } + + if (not initialized) { + initialize(getMapPose()); + plan(goal, waypoints, route_option); + // NOTE: engage() will be executed at simulation-time 0. + } else { + plan(goal, waypoints, route_option); + FieldOperatorApplication::engage(); + } } } @@ -321,7 +393,23 @@ auto EgoEntity::requestReplanRoute( -> void { clearRoute(); - plan(route, allow_goal_modification); + /* + NOTE: + This function does not support use_lane_ids_for_routing option. + The developers should consider manual override simulation to determine whether support it or not. + */ + { + concealer::FieldOperatorApplication::RouteOption route_option; + route_option.allow_goal_modification = allow_goal_modification; + assert(not route.empty()); + std::vector waypoints; + if (route.size() > 1) { + std::transform( + route.begin(), route.end() - 1, waypoints.begin(), + [](const geometry_msgs::msg::PoseStamped & pose) { return pose.pose; }); + } + plan(route.back().pose, waypoints, route_option); + } enableAutowareControl(); FieldOperatorApplication::engage(); } diff --git a/test_runner/scenario_test_runner/scenario/RoutingAction.AcquirePositionAction-allow_goal_modification.yaml b/test_runner/scenario_test_runner/scenario/RoutingAction.AcquirePositionAction-allow_goal_modification.yaml index 1a37249a349..3440e349ff5 100644 --- a/test_runner/scenario_test_runner/scenario/RoutingAction.AcquirePositionAction-allow_goal_modification.yaml +++ b/test_runner/scenario_test_runner/scenario/RoutingAction.AcquirePositionAction-allow_goal_modification.yaml @@ -7,7 +7,7 @@ OpenSCENARIO: revMinor: 3 ParameterDeclarations: ParameterDeclaration: - - name: AcquirePositionAction.allow_goal_modification + - name: RoutingAction__allow_goal_modification parameterType: boolean value: true CatalogLocations: @@ -97,7 +97,7 @@ OpenSCENARIO: - name: '' ParameterDeclarations: ParameterDeclaration: - - name: AcquirePositionAction.allow_goal_modification + - name: RoutingAction__allow_goal_modification parameterType: boolean value: false Event: diff --git a/test_runner/scenario_test_runner/scenario/RoutingAction.AssignRouteAction-use_lane_ids_for_routing.yaml b/test_runner/scenario_test_runner/scenario/RoutingAction.AssignRouteAction-use_lane_ids_for_routing.yaml new file mode 100644 index 00000000000..455b8808fbf --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/RoutingAction.AssignRouteAction-use_lane_ids_for_routing.yaml @@ -0,0 +1,200 @@ +ScenarioModifiers: + ScenarioModifier: [] +OpenSCENARIO: + FileHeader: + revMajor: 1 + revMinor: 1 + date: '2024-11-15T02:21:26.404Z' + 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: 'Autoware' + Properties: + Property: [] + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: &INITIAL_POSITION + LanePosition: + roadId: '' + laneId: '34513' + s: 5 + Orientation: &DEFAULT_ORIENTATION + type: relative + h: 0 + p: 0 + r: 0 + - RoutingAction: + AcquirePositionAction: + Position: &STANDBY_POSITION + LanePosition: + roadId: '' + laneId: '34513' + s: 30 + Orientation: *DEFAULT_ORIENTATION + Story: + - name: '' + Act: + - name: act_reroute + ManeuverGroup: + - maximumExecutionCount: 1 + name: act_reroute + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + ParameterDeclarations: + ParameterDeclaration: + - name: RoutingAction__use_lane_ids_for_routing + parameterType: boolean + value: true + Event: + - name: reroute + priority: parallel + Action: + - name: '' + PrivateAction: + RoutingAction: + AssignRouteAction: + Route: + name: '' + closed: false + Waypoint: + - Position: &CHECK_POINT_0 + LanePosition: + roadId: '' + laneId: '34606' + s: 15 + Orientation: *DEFAULT_ORIENTATION + routeStrategy: shortest + - Position: &CHECK_POINT_1 + LanePosition: + roadId: '' + laneId: '34600' + s: 30 + Orientation: *DEFAULT_ORIENTATION + routeStrategy: shortest + - Position: &CHECK_POINT_2 + LanePosition: + roadId: '' + laneId: '34630' + s: 13 + Orientation: *DEFAULT_ORIENTATION + routeStrategy: shortest + - Position: &DESTINATION + LanePosition: + roadId: '' + laneId: '34741' + s: 25 + Orientation: *DEFAULT_ORIENTATION + routeStrategy: shortest + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + UserDefinedValueCondition: + value: WAITING_FOR_ROUTE + name: ego.currentState + rule: equalTo + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: *STANDBY_POSITION + tolerance: 1 + - 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 + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: *DESTINATION + tolerance: 1 + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 180 + rule: greaterThan + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitFailure + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + StopTrigger: + ConditionGroup: []