From 201ab96b605de161707dea57fe0738cb22635fc1 Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Tue, 17 Sep 2024 19:58:00 +0900 Subject: [PATCH 1/3] Customize API::getGoalPoses for goal modifications. --- .../include/concealer/autoware_universe.hpp | 2 ++ ...operator_application_for_autoware_universe.hpp | 2 ++ external/concealer/package.xml | 1 + external/concealer/src/autoware_universe.cpp | 1 + pyproject.toml | 2 +- .../traffic_simulator/entity/ego_entity.hpp | 2 ++ .../traffic_simulator/src/entity/ego_entity.cpp | 15 +++++++++++++++ 7 files changed, 24 insertions(+), 1 deletion(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 855e21b871e..5e578c3210e 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,7 @@ class AutowareUniverse : public Autoware SubscriberWrapper getGearCommandImpl; SubscriberWrapper getTurnIndicatorsCommand; SubscriberWrapper getPathWithLaneId; + SubscriberWrapper getAutowareMissionRoute; PublisherWrapper setAcceleration; PublisherWrapper setOdometry; 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 0202d488eb3..5433145c964 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 @@ -108,6 +108,7 @@ class FieldOperatorApplicationFor public: SubscriberWrapper getPathWithLaneId; + SubscriberWrapper getAutowareMissionRoute; public: template @@ -123,6 +124,7 @@ class FieldOperatorApplicationFor #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), diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 3978cf815d1..560a22b2545 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -17,6 +17,7 @@ autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs + autoware_planning_msgs tier4_external_api_msgs diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index dbecc8b51d9..488fbcaf9f6 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/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), diff --git a/pyproject.toml b/pyproject.toml index d1a78ee1c26..6d58bb35bf4 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -25,5 +25,5 @@ catkin-pkg = "^1.0.0" [build-system] -requires = ["poetry-core"] +requires = ["poetry-core", "setuptools"] build-backend = "poetry.core.masonry.api" 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 dbe2668f758..9cb396319c3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -82,6 +82,8 @@ class EgoEntity : public VehicleEntity auto getObstacle() -> std::optional override; + auto getGoalPoses() -> std::vector override; + auto getRouteLanelets(double horizon = 100) -> lanelet::Ids override; auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override; diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 7ba999c60b1..4ed5be519e9 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -100,6 +100,21 @@ auto EgoEntity::getBehaviorParameter() const -> traffic_simulator_msgs::msg::Beh return behavior_parameter_; } +auto EgoEntity::getGoalPoses() -> std::vector +{ + std::vector lanelet_pose; + + if (const auto universe = + dynamic_cast *>( + field_operator_application.get()); + universe) { + lanelet_pose.push_back(traffic_simulator::pose::toCanonicalizedLaneletPose( + universe->getAutowareMissionRoute().goal_pose, false, hdmap_utils_ptr_) + .value()); + } + return lanelet_pose; +} + auto EgoEntity::getEntityTypename() const -> const std::string & { static const std::string result = "EgoEntity"; From e55cb4c1a8928a9d1200dc43e491f0748af163b5 Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Thu, 19 Sep 2024 16:32:38 +0900 Subject: [PATCH 2/3] Removed unnecessary change. --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 6d58bb35bf4..d1a78ee1c26 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -25,5 +25,5 @@ catkin-pkg = "^1.0.0" [build-system] -requires = ["poetry-core", "setuptools"] +requires = ["poetry-core"] build-backend = "poetry.core.masonry.api" From f4de90b56b73c8051b7979a3668b48cf8f5e2234 Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Wed, 2 Oct 2024 18:34:17 +0900 Subject: [PATCH 3/3] Enhanced the variable validation --- simulation/traffic_simulator/src/entity/ego_entity.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 4ed5be519e9..4375228c454 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -108,10 +108,13 @@ auto EgoEntity::getGoalPoses() -> std::vector dynamic_cast *>( field_operator_application.get()); universe) { - lanelet_pose.push_back(traffic_simulator::pose::toCanonicalizedLaneletPose( - universe->getAutowareMissionRoute().goal_pose, false, hdmap_utils_ptr_) - .value()); + 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; }