diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 2c80e36baa2..a6820f01c4c 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -89,6 +90,7 @@ struct FieldOperatorApplication : public rclcpp::Node using AutoModeWithModule = tier4_rtc_msgs::srv::AutoModeWithModule; using SetVelocityLimit = tier4_external_api_msgs::srv::SetVelocityLimit; using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; + using Route = autoware_adapi_v1_msgs::msg::Route; Subscriber getAutowareState; Subscriber getCommand; @@ -101,7 +103,7 @@ struct FieldOperatorApplication : public rclcpp::Node #if __has_include() Subscriber getOperationModeState; #endif - Subscriber getPathWithLaneId; + Subscriber getRoute; #if __has_include() Subscriber getRouteState; #endif diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index ec3943ee297..f48438cd4d0 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -147,7 +147,7 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) #if __has_include() getOperationModeState("/api/operation_mode/state", rclcpp::QoS(1).transient_local(), *this), #endif - getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), *this), + getRoute("/api/routing/route", rclcpp::QoS(1), *this), #if __has_include() getRouteState("/api/routing/state", rclcpp::QoS(1).transient_local(), *this), #endif diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index e1a27fe1eaa..753d6032025 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -143,11 +143,11 @@ auto EgoEntity::getObstacle() -> std::optional lanelet::Ids { lanelet::Ids ids{}; - - for (const auto & point : getPathWithLaneId().points) { - ids += point.lane_ids; + for (const auto & route_data : getRoute().data) { + for (const auto & segment : route_data.segments) { + ids.push_back(segment.preferred.id); + } } - return ids; }