Customize API::getGoalPoses for goal modifications.

external/concealer/include/concealer/autoware_universe.hpp

@@ -22,6 +22,7 @@
 #include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <concealer/autoware.hpp>
 #include <concealer/publisher_wrapper.hpp>
 #include <concealer/subscriber_wrapper.hpp>
@@ -41,6 +42,7 @@ class AutowareUniverse : public Autoware
   SubscriberWrapper<autoware_auto_vehicle_msgs::msg::GearCommand,             ThreadSafety::safe> getGearCommandImpl;
   SubscriberWrapper<autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand,   ThreadSafety::safe> getTurnIndicatorsCommand;
   SubscriberWrapper<autoware_auto_planning_msgs::msg::PathWithLaneId,         ThreadSafety::safe> getPathWithLaneId;
+  SubscriberWrapper<autoware_planning_msgs::msg::LaneletRoute,                ThreadSafety::safe> getAutowareMissionRoute;
 
   PublisherWrapper<geometry_msgs::msg::AccelWithCovarianceStamped>        setAcceleration;
   PublisherWrapper<nav_msgs::msg::Odometry>                               setOdometry;

external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp

@@ -113,6 +113,7 @@ class FieldOperatorApplicationFor<AutowareUniverse>
 
 public:
   SubscriberWrapper<autoware_auto_planning_msgs::msg::PathWithLaneId> getPathWithLaneId;
+  SubscriberWrapper<autoware_planning_msgs::msg::LaneletRoute> getAutowareMissionRoute;
 
 public:
   template <typename... Ts>
@@ -128,6 +129,7 @@ class FieldOperatorApplicationFor<AutowareUniverse>
 #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),
+    getAutowareMissionRoute("/planning/mission_planning/route", 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),

external/concealer/package.xml

@@ -17,6 +17,7 @@
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_system_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
 
   <!-- TIER IV Autoware.Universe -->
   <depend>tier4_external_api_msgs</depend>

external/concealer/src/autoware_universe.cpp

@@ -23,6 +23,7 @@ AutowareUniverse::AutowareUniverse()
   getPathWithLaneId(
     "/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1),
     *this),
+  getAutowareMissionRoute("/planning/mission_planning/route", rclcpp::QoS(1), *this),
   setAcceleration("/localization/acceleration", *this),
   setOdometry("/localization/kinematic_state", *this),
   setSteeringReport("/vehicle/status/steering_status", *this),

simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp

@@ -82,6 +82,8 @@ class EgoEntity : public VehicleEntity
 
   auto getObstacle() -> std::optional<traffic_simulator_msgs::msg::Obstacle> override;
 
+  auto getGoalPoses() -> std::vector<CanonicalizedLaneletPose> override;
+
   auto getRouteLanelets(double horizon = 100) -> lanelet::Ids override;
 
   auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override;

simulation/traffic_simulator/src/entity/ego_entity.cpp

@@ -100,6 +100,24 @@ auto EgoEntity::getBehaviorParameter() const -> traffic_simulator_msgs::msg::Beh
   return behavior_parameter_;
 }
 
+auto EgoEntity::getGoalPoses() -> std::vector<CanonicalizedLaneletPose>
+{
+  std::vector<CanonicalizedLaneletPose> lanelet_pose;
+
+  if (const auto universe =
+        dynamic_cast<concealer::FieldOperatorApplicationFor<concealer::AutowareUniverse> *>(
+          field_operator_application.get());
+      universe) {
+    if (const auto pose_opt = traffic_simulator::pose::toCanonicalizedLaneletPose(
+          universe->getAutowareMissionRoute().goal_pose, false, hdmap_utils_ptr_);
+        pose_opt.has_value()) {
+      lanelet_pose.push_back(pose_opt.value());
+    }
+  }
+
+  return lanelet_pose;
+}
+
 auto EgoEntity::getEntityTypename() const -> const std::string &
 {
   static const std::string result = "EgoEntity";