From 57ea89413ac3a81cfbe3b45a31fff09aab4fe1ec Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 18 Dec 2024 11:36:14 +0900 Subject: [PATCH 01/15] Rename class `PublisherWrapper` to `Publisher` Signed-off-by: yamacir-kit --- .../include/concealer/autoware_universe.hpp | 18 +++++++++--------- .../concealer/field_operator_application.hpp | 2 +- .../{publisher_wrapper.hpp => publisher.hpp} | 10 +++++----- 3 files changed, 15 insertions(+), 15 deletions(-) rename external/concealer/include/concealer/{publisher_wrapper.hpp => publisher.hpp} (82%) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 34c1bda7afd..8a0be6efd3e 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -26,7 +26,7 @@ #include #include #include -#include +#include #include #include #include @@ -62,14 +62,14 @@ class AutowareUniverse : public rclcpp::Node, SubscriberWrapper getTurnIndicatorsCommand; SubscriberWrapper getPathWithLaneId; - PublisherWrapper setAcceleration; - PublisherWrapper setOdometry; - PublisherWrapper setPose; - PublisherWrapper setSteeringReport; - PublisherWrapper setGearReport; - PublisherWrapper setControlModeReport; - PublisherWrapper setVelocityReport; - PublisherWrapper setTurnIndicatorsReport; + Publisher setAcceleration; + Publisher setOdometry; + Publisher setPose; + Publisher setSteeringReport; + Publisher setGearReport; + Publisher setControlModeReport; + Publisher setVelocityReport; + Publisher setTurnIndicatorsReport; std::atomic current_acceleration; std::atomic current_pose; diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index db11a70de50..59a640032a7 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -32,7 +32,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/external/concealer/include/concealer/publisher_wrapper.hpp b/external/concealer/include/concealer/publisher.hpp similarity index 82% rename from external/concealer/include/concealer/publisher_wrapper.hpp rename to external/concealer/include/concealer/publisher.hpp index 472ae028c8f..b01730f858c 100644 --- a/external/concealer/include/concealer/publisher_wrapper.hpp +++ b/external/concealer/include/concealer/publisher.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONCEALER__PUBLISHER_WRAPPER_HPP_ -#define CONCEALER__PUBLISHER_WRAPPER_HPP_ +#ifndef CONCEALER__PUBLISHER_HPP_ +#define CONCEALER__PUBLISHER_HPP_ #include #include @@ -21,7 +21,7 @@ namespace concealer { template -class PublisherWrapper +class Publisher { private: typename rclcpp::Publisher::SharedPtr publisher; @@ -33,7 +33,7 @@ class PublisherWrapper } template - explicit PublisherWrapper(std::string topic, NodeInterface & autoware_interface) + explicit Publisher(std::string topic, NodeInterface & autoware_interface) : publisher( autoware_interface.template create_publisher(topic, rclcpp::QoS(1).reliable())) { @@ -41,4 +41,4 @@ class PublisherWrapper }; } // namespace concealer -#endif // CONCEALER__PUBLISHER_WRAPPER_HPP_ +#endif // CONCEALER__PUBLISHER_HPP_ From f601263cb8db9b046c6c67fe1382b996b93db8bc Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 18 Dec 2024 11:55:11 +0900 Subject: [PATCH 02/15] Lipsticks Signed-off-by: yamacir-kit --- .../concealer/include/concealer/publisher.hpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/external/concealer/include/concealer/publisher.hpp b/external/concealer/include/concealer/publisher.hpp index b01730f858c..aeb323084e4 100644 --- a/external/concealer/include/concealer/publisher.hpp +++ b/external/concealer/include/concealer/publisher.hpp @@ -20,23 +20,22 @@ namespace concealer { -template +template class Publisher { -private: - typename rclcpp::Publisher::SharedPtr publisher; + typename rclcpp::Publisher::SharedPtr publisher; public: - auto operator()(const MessageType & message) const -> decltype(auto) + template + explicit Publisher(const std::string & topic, Node & node) + : publisher(node.template create_publisher(topic, rclcpp::QoS(1).reliable())) { - return publisher->publish(message); } - template - explicit Publisher(std::string topic, NodeInterface & autoware_interface) - : publisher( - autoware_interface.template create_publisher(topic, rclcpp::QoS(1).reliable())) + template + auto operator()(Ts &&... xs) const -> decltype(auto) { + return publisher->publish(std::forward(xs)...); } }; } // namespace concealer From 5ac238a2c70e6c16ed82067188f390e099388523 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 19 Dec 2024 12:03:00 +0900 Subject: [PATCH 03/15] Rename class `SubscriberWrapper` to `Subscriber` Signed-off-by: yamacir-kit --- .../include/concealer/autoware_universe.hpp | 10 +++++----- .../concealer/field_operator_application.hpp | 20 +++++++++---------- ...{subscriber_wrapper.hpp => subscriber.hpp} | 12 +++++------ 3 files changed, 21 insertions(+), 21 deletions(-) rename external/concealer/include/concealer/{subscriber_wrapper.hpp => subscriber.hpp} (89%) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 8a0be6efd3e..dddaf9adbb5 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -27,7 +27,7 @@ #include #include #include -#include +#include #include #include #include @@ -57,10 +57,10 @@ class AutowareUniverse : public rclcpp::Node, using TurnIndicatorsReport = autoware_vehicle_msgs::msg::TurnIndicatorsReport; using VelocityReport = autoware_vehicle_msgs::msg::VelocityReport; - SubscriberWrapper getCommand; - SubscriberWrapper getGearCommand; - SubscriberWrapper getTurnIndicatorsCommand; - SubscriberWrapper getPathWithLaneId; + Subscriber getCommand; + Subscriber getGearCommand; + Subscriber getTurnIndicatorsCommand; + Subscriber getPathWithLaneId; Publisher setAcceleration; Publisher setOdometry; diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 59a640032a7..7b70906fc25 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -34,7 +34,7 @@ #include #include #include -#include +#include #include #include #include @@ -107,17 +107,17 @@ struct FieldOperatorApplication : public rclcpp::Node, using SetVelocityLimit = tier4_external_api_msgs::srv::SetVelocityLimit; using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; - SubscriberWrapper getAutowareState; - SubscriberWrapper getCommand; - SubscriberWrapper getCooperateStatusArray; - SubscriberWrapper getEmergencyState; + Subscriber getAutowareState; + Subscriber getCommand; + Subscriber getCooperateStatusArray; + Subscriber getEmergencyState; #if __has_include() - SubscriberWrapper getLocalizationState; + Subscriber getLocalizationState; #endif - SubscriberWrapper getMrmState; - SubscriberWrapper getPathWithLaneId; - SubscriberWrapper getTrajectory; - SubscriberWrapper getTurnIndicatorsCommand; + Subscriber getMrmState; + Subscriber getPathWithLaneId; + Subscriber getTrajectory; + Subscriber getTurnIndicatorsCommand; ServiceWithValidation requestClearRoute; ServiceWithValidation requestCooperateCommands; diff --git a/external/concealer/include/concealer/subscriber_wrapper.hpp b/external/concealer/include/concealer/subscriber.hpp similarity index 89% rename from external/concealer/include/concealer/subscriber_wrapper.hpp rename to external/concealer/include/concealer/subscriber.hpp index eb4fde75af0..e32a51ab9a4 100644 --- a/external/concealer/include/concealer/subscriber_wrapper.hpp +++ b/external/concealer/include/concealer/subscriber.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONCEALER__SUBSCRIBER_WRAPPER_HPP_ -#define CONCEALER__SUBSCRIBER_WRAPPER_HPP_ +#ifndef CONCEALER__SUBSCRIBER_HPP_ +#define CONCEALER__SUBSCRIBER_HPP_ #include #include @@ -21,7 +21,7 @@ namespace concealer { template -class SubscriberWrapper +class Subscriber { typename Message::ConstSharedPtr current_value = std::make_shared(); @@ -31,7 +31,7 @@ class SubscriberWrapper auto operator()() const -> const auto & { return *std::atomic_load(¤t_value); } template - explicit SubscriberWrapper( + explicit Subscriber( const std::string & topic, const rclcpp::QoS & quality_of_service, Autoware & autoware, const Callback & callback) : subscription(autoware.template create_subscription( @@ -45,7 +45,7 @@ class SubscriberWrapper } template - explicit SubscriberWrapper( + explicit Subscriber( const std::string & topic, const rclcpp::QoS & quality_of_service, Autoware & autoware) : subscription(autoware.template create_subscription( topic, quality_of_service, [this](const typename Message::ConstSharedPtr & message) { @@ -56,4 +56,4 @@ class SubscriberWrapper }; } // namespace concealer -#endif // CONCEALER__SUBSCRIBER_WRAPPER_HPP_ +#endif // CONCEALER__SUBSCRIBER_HPP_ From 71094df9aa156f0bec89d3fc8bd106c37c644abe Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 19 Dec 2024 12:34:00 +0900 Subject: [PATCH 04/15] Rename class `ServiceWithValidation` to `Service` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 18 +++++++++--------- ...service_with_validation.hpp => service.hpp} | 18 +++++++++--------- 2 files changed, 18 insertions(+), 18 deletions(-) rename external/concealer/include/concealer/{service_with_validation.hpp => service.hpp} (92%) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 7b70906fc25..d5be067e0b0 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -33,7 +33,7 @@ #include #include #include -#include +#include #include #include #include @@ -119,14 +119,14 @@ struct FieldOperatorApplication : public rclcpp::Node, Subscriber getTrajectory; Subscriber getTurnIndicatorsCommand; - ServiceWithValidation requestClearRoute; - ServiceWithValidation requestCooperateCommands; - ServiceWithValidation requestEngage; - ServiceWithValidation requestInitialPose; - ServiceWithValidation requestSetRoutePoints; - ServiceWithValidation requestSetRtcAutoMode; - ServiceWithValidation requestSetVelocityLimit; - ServiceWithValidation requestEnableAutowareControl; + Service requestClearRoute; + Service requestCooperateCommands; + Service requestEngage; + Service requestInitialPose; + Service requestSetRoutePoints; + Service requestSetRtcAutoMode; + Service requestSetVelocityLimit; + Service requestEnableAutowareControl; // clang-format on /* diff --git a/external/concealer/include/concealer/service_with_validation.hpp b/external/concealer/include/concealer/service.hpp similarity index 92% rename from external/concealer/include/concealer/service_with_validation.hpp rename to external/concealer/include/concealer/service.hpp index 6b49df8d0f0..171cad0e729 100644 --- a/external/concealer/include/concealer/service_with_validation.hpp +++ b/external/concealer/include/concealer/service.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONCEALER__SERVICE_WITH_VALIDATION_HPP_ -#define CONCEALER__SERVICE_WITH_VALIDATION_HPP_ +#ifndef CONCEALER__SERVICE_HPP_ +#define CONCEALER__SERVICE_HPP_ #include #include @@ -28,7 +28,7 @@ namespace concealer { template -class ServiceWithValidation +class Service { const std::string service_name; @@ -39,13 +39,13 @@ class ServiceWithValidation rclcpp::WallRate validation_rate; public: - template - explicit ServiceWithValidation( - const std::string & service_name, FieldOperatorApplication & autoware, + template + explicit Service( + const std::string & service_name, Node & node, const std::chrono::nanoseconds validation_interval = std::chrono::seconds(1)) : service_name(service_name), - logger(autoware.get_logger()), - client(autoware.template create_client(service_name, rmw_qos_profile_default)), + logger(node.get_logger()), + client(node.template create_client(service_name, rmw_qos_profile_default)), validation_rate(validation_interval) { } @@ -142,4 +142,4 @@ class ServiceWithValidation }; } // namespace concealer -#endif //CONCEALER__SERVICE_WITH_VALIDATION_HPP_ +#endif //CONCEALER__SERVICE_HPP_ From 544fd7bc54c81342bbabfbb7238bb3667e09ae95 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 19 Dec 2024 13:54:46 +0900 Subject: [PATCH 05/15] Remove static member function `EgoEntity::makeFieldOperatorApplication` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 9 +-------- .../traffic_simulator/entity/ego_entity.hpp | 4 ---- .../src/entity/ego_entity.cpp | 18 +++++++++--------- 3 files changed, 10 insertions(+), 21 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index d5be067e0b0..13026c8e5cb 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -31,7 +31,6 @@ #include #include #include -#include #include #include #include @@ -136,13 +135,7 @@ struct FieldOperatorApplication : public rclcpp::Node, */ TaskQueue task_queue; - CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t = 0); - - template - CONCEALER_PUBLIC explicit FieldOperatorApplication(Ts &&... xs) - : FieldOperatorApplication(ros2_launch(std::forward(xs)...)) - { - } + CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t); ~FieldOperatorApplication(); 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 91cab0f7f97..5580e106e39 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -35,10 +35,6 @@ class EgoEntity : public VehicleEntity { const std::unique_ptr field_operator_application; - static auto makeFieldOperatorApplication( - const Configuration &, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &) - -> std::unique_ptr; - bool is_controlled_by_simulator_{false}; std::optional target_speed_; traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter_; diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index e4f9560c180..194ef8287d1 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -32,7 +33,7 @@ namespace traffic_simulator { namespace entity { -auto EgoEntity::makeFieldOperatorApplication( +auto makeFieldOperatorApplication( const Configuration & configuration, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters) -> std::unique_ptr @@ -56,11 +57,12 @@ auto EgoEntity::makeFieldOperatorApplication( parameters.push_back("localization_sim_mode:=" + std::string(getParameter(node_parameters, "simulate_localization") ? "api" : "pose_twist_estimator")); // clang-format on - return getParameter(node_parameters, "launch_autoware", true) - ? std::make_unique( - getParameter(node_parameters, "autoware_launch_package"), - getParameter(node_parameters, "autoware_launch_file"), parameters) - : std::make_unique(); + return std::make_unique( + getParameter(node_parameters, "launch_autoware", true) + ? concealer::ros2_launch( + getParameter(node_parameters, "autoware_launch_package"), + getParameter(node_parameters, "autoware_launch_file"), parameters) + : 0); } else { throw common::SemanticError( "Unexpected architecture_type ", std::quoted(architecture_type), " was given."); @@ -113,9 +115,7 @@ auto EgoEntity::getRouteLanelets(double /*unused horizon*/) -> lanelet::Ids { lanelet::Ids ids{}; - if (const auto universe = - dynamic_cast(field_operator_application.get()); - universe) { + if (const auto universe = field_operator_application.get(); universe) { for (const auto & point : universe->getPathWithLaneId().points) { ids += point.lane_ids; } From 2f5f1b09908f4a824be2fd3a9d0b4960b8add4b0 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 19 Dec 2024 16:00:00 +0900 Subject: [PATCH 06/15] Update `EgoEntity` to have `FieldOperatorApplication` as non-pointer Signed-off-by: yamacir-kit --- .../traffic_simulator/entity/ego_entity.hpp | 6 +- .../traffic_simulator/entity/entity_base.hpp | 8 +- .../src/entity/ego_entity.cpp | 104 ++++++++---------- .../src/entity/entity_base.cpp | 2 +- .../src/entity/entity_manager.cpp | 2 +- 5 files changed, 56 insertions(+), 66 deletions(-) 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 5580e106e39..c20c9c26c52 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -33,7 +33,7 @@ namespace entity { class EgoEntity : public VehicleEntity { - const std::unique_ptr field_operator_application; + concealer::FieldOperatorApplication field_operator_application; bool is_controlled_by_simulator_{false}; std::optional target_speed_; @@ -59,7 +59,7 @@ class EgoEntity : public VehicleEntity auto operator=(const EgoEntity &) -> EgoEntity & = delete; - auto asFieldOperatorApplication() const -> concealer::FieldOperatorApplication & override; + auto asFieldOperatorApplication() -> concealer::FieldOperatorApplication & override; auto getCurrentAction() const -> std::string override; @@ -80,7 +80,7 @@ class EgoEntity : public VehicleEntity auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override; - auto updateFieldOperatorApplication() const -> void; + auto updateFieldOperatorApplication() -> void; void onUpdate(double current_time, double step_time) override; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index 1bf628fb474..07ebecd6de2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -58,7 +58,7 @@ class EntityBase virtual void appendDebugMarker(visualization_msgs::msg::MarkerArray & /*unused*/); - virtual auto asFieldOperatorApplication() const -> concealer::FieldOperatorApplication &; + virtual auto asFieldOperatorApplication() -> concealer::FieldOperatorApplication &; virtual void cancelRequest(); @@ -144,7 +144,7 @@ class EntityBase virtual void requestLaneChange(const lanelet::Id) { /** - * @note There are Entities such as MiscObjectEntity for which lane change is not possible, + * @note There are Entities such as MiscObjectEntity for which lane change is not possible, * and since it is necessary to implement appropriate overrides for each Entity, no operation is performed on the base type. */ } @@ -152,7 +152,7 @@ class EntityBase virtual void requestLaneChange(const traffic_simulator::lane_change::Parameter &) { /** - * @note There are Entities such as MiscObjectEntity for which lane change is not possible, + * @note There are Entities such as MiscObjectEntity for which lane change is not possible, * and since it is necessary to implement appropriate overrides for each Entity, no operation is performed on the base type. */ } @@ -182,7 +182,7 @@ class EntityBase virtual void requestClearRoute() { /** - * @note Since there are Entities such as MiscObjectEntity that do not perform routing, + * @note Since there are Entities such as MiscObjectEntity that do not perform routing, * and routing methods differ from Entity to Entity, this function performs no operation in the base type. */ } diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 194ef8287d1..012f74b318d 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -33,42 +33,6 @@ namespace traffic_simulator { namespace entity { -auto makeFieldOperatorApplication( - const Configuration & configuration, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters) - -> std::unique_ptr -{ - if (const auto architecture_type = - getParameter(node_parameters, "architecture_type", "awf/universe/20240605"); - architecture_type.find("awf/universe") != std::string::npos) { - auto parameters = getParameter>(node_parameters, "autoware.", {}); - - // clang-format off - parameters.push_back("map_path:=" + configuration.map_path.string()); - parameters.push_back("lanelet2_map_file:=" + configuration.getLanelet2MapFile()); - parameters.push_back("pointcloud_map_file:=" + configuration.getPointCloudMapFile()); - parameters.push_back("sensor_model:=" + getParameter(node_parameters, "sensor_model")); - parameters.push_back("vehicle_model:=" + getParameter(node_parameters, "vehicle_model")); - parameters.push_back("rviz_config:=" + getParameter(node_parameters, "rviz_config")); - parameters.push_back("scenario_simulation:=true"); - parameters.push_back("use_foa:=false"); - parameters.push_back("perception/enable_traffic_light:=" + std::string(architecture_type >= "awf/universe/20230906" ? "true" : "false")); - parameters.push_back("use_sim_time:=" + std::string(getParameter(node_parameters, "use_sim_time", false) ? "true" : "false")); - parameters.push_back("localization_sim_mode:=" + std::string(getParameter(node_parameters, "simulate_localization") ? "api" : "pose_twist_estimator")); - // clang-format on - - return std::make_unique( - getParameter(node_parameters, "launch_autoware", true) - ? concealer::ros2_launch( - getParameter(node_parameters, "autoware_launch_package"), - getParameter(node_parameters, "autoware_launch_file"), parameters) - : 0); - } else { - throw common::SemanticError( - "Unexpected architecture_type ", std::quoted(architecture_type), " was given."); - } -} - EgoEntity::EgoEntity( const std::string & name, const CanonicalizedEntityStatus & entity_status, const std::shared_ptr & hdmap_utils_ptr, @@ -76,19 +40,47 @@ EgoEntity::EgoEntity( const Configuration & configuration, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters) : VehicleEntity(name, entity_status, hdmap_utils_ptr, parameters), - field_operator_application(makeFieldOperatorApplication(configuration, node_parameters)) + field_operator_application([&]() { + if (const auto architecture_type = + getParameter(node_parameters, "architecture_type", "awf/universe/20240605"); + architecture_type.find("awf/universe") != std::string::npos) { + auto parameters = getParameter>(node_parameters, "autoware.", {}); + + // clang-format off + parameters.push_back("map_path:=" + configuration.map_path.string()); + parameters.push_back("lanelet2_map_file:=" + configuration.getLanelet2MapFile()); + parameters.push_back("pointcloud_map_file:=" + configuration.getPointCloudMapFile()); + parameters.push_back("sensor_model:=" + getParameter(node_parameters, "sensor_model")); + parameters.push_back("vehicle_model:=" + getParameter(node_parameters, "vehicle_model")); + parameters.push_back("rviz_config:=" + getParameter(node_parameters, "rviz_config")); + parameters.push_back("scenario_simulation:=true"); + parameters.push_back("use_foa:=false"); + parameters.push_back("perception/enable_traffic_light:=" + std::string(architecture_type >= "awf/universe/20230906" ? "true" : "false")); + parameters.push_back("use_sim_time:=" + std::string(getParameter(node_parameters, "use_sim_time", false) ? "true" : "false")); + parameters.push_back("localization_sim_mode:=" + std::string(getParameter(node_parameters, "simulate_localization") ? "api" : "pose_twist_estimator")); + // clang-format on + + return getParameter(node_parameters, "launch_autoware", true) + ? concealer::ros2_launch( + getParameter(node_parameters, "autoware_launch_package"), + getParameter(node_parameters, "autoware_launch_file"), parameters) + : 0; + } else { + throw common::SemanticError( + "Unexpected architecture_type ", std::quoted(architecture_type), " was given."); + } + }()) { } -auto EgoEntity::asFieldOperatorApplication() const -> concealer::FieldOperatorApplication & +auto EgoEntity::asFieldOperatorApplication() -> concealer::FieldOperatorApplication & { - assert(field_operator_application); - return *field_operator_application; + return field_operator_application; } auto EgoEntity::getCurrentAction() const -> std::string { - const auto state = field_operator_application->getAutowareStateName(); + const auto state = field_operator_application.getAutowareStateName(); return state.empty() ? "Launching" : state; } @@ -115,10 +107,8 @@ auto EgoEntity::getRouteLanelets(double /*unused horizon*/) -> lanelet::Ids { lanelet::Ids ids{}; - if (const auto universe = field_operator_application.get(); universe) { - for (const auto & point : universe->getPathWithLaneId().points) { - ids += point.lane_ids; - } + for (const auto & point : field_operator_application.getPathWithLaneId().points) { + ids += point.lane_ids; } return ids; @@ -131,13 +121,13 @@ auto EgoEntity::getCurrentPose() const -> const geometry_msgs::msg::Pose & auto EgoEntity::getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray { - return field_operator_application->getWaypoints(); + return field_operator_application.getWaypoints(); } -void EgoEntity::updateFieldOperatorApplication() const +void EgoEntity::updateFieldOperatorApplication() { - field_operator_application->rethrow(); - field_operator_application->spinSome(); + field_operator_application.rethrow(); + field_operator_application.spinSome(); } void EgoEntity::onUpdate(double current_time, double step_time) @@ -155,7 +145,7 @@ void EgoEntity::onUpdate(double current_time, double step_time) // prefer current lanelet on ss2 side setStatus(non_canonicalized_updated_status.value(), status_->getLaneletIds()); } else { - field_operator_application->enableAutowareControl(); + field_operator_application.enableAutowareControl(); is_controlled_by_simulator_ = false; } } @@ -202,13 +192,13 @@ void EgoEntity::requestAssignRoute(const std::vector & route.push_back(pose_stamped); } - if (not field_operator_application->initialized()) { - field_operator_application->initialize(getMapPose()); - field_operator_application->plan(route); + if (not field_operator_application.initialized()) { + field_operator_application.initialize(getMapPose()); + field_operator_application.plan(route); // NOTE: engage() will be executed at simulation-time 0. } else { - field_operator_application->plan(route); - field_operator_application->engage(); + field_operator_application.plan(route); + field_operator_application.engage(); } } @@ -254,7 +244,7 @@ auto EgoEntity::requestSpeedChange( "purposes only."); } -void EgoEntity::requestClearRoute() { field_operator_application->clearRoute(); } +void EgoEntity::requestClearRoute() { field_operator_application.clearRoute(); } auto EgoEntity::getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints & @@ -288,7 +278,7 @@ auto EgoEntity::requestSpeedChange( auto EgoEntity::setVelocityLimit(double value) -> void // { behavior_parameter_.dynamic_constraints.max_speed = value; - field_operator_application->setVelocityLimit(value); + field_operator_application.setVelocityLimit(value); } auto EgoEntity::setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 0b1d7af890a..139fbd51a2d 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -68,7 +68,7 @@ EntityBase::EntityBase( void EntityBase::appendDebugMarker(visualization_msgs::msg::MarkerArray & /*unused*/) {} -auto EntityBase::asFieldOperatorApplication() const -> concealer::FieldOperatorApplication & +auto EntityBase::asFieldOperatorApplication() -> concealer::FieldOperatorApplication & { throw common::Error( "An operation was requested for Entity ", std::quoted(name), diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 4211cc33c93..f38b2f9e740 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -354,7 +354,7 @@ auto EntityManager::updateNpcLogic( // Update npc completely if logic has started, otherwise update Autoware only - if it is Ego if (npc_logic_started_) { entity->onUpdate(current_time, step_time); - } else if (const auto ego_entity = std::dynamic_pointer_cast(entity)) { + } else if (const auto ego_entity = std::dynamic_pointer_cast(entity)) { ego_entity->updateFieldOperatorApplication(); } return entity->getCanonicalizedStatus(); From fddce82e3f448684b5789adce2a02e4c5c2acc3a Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 19 Dec 2024 16:41:18 +0900 Subject: [PATCH 07/15] Update `EgoEntity` to have `FieldOperatorApplication` as its base class Signed-off-by: yamacir-kit --- .../traffic_simulator/entity/ego_entity.hpp | 4 +-- .../src/entity/ego_entity.cpp | 31 +++++++++---------- 2 files changed, 16 insertions(+), 19 deletions(-) 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 c20c9c26c52..cbb9f01861f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -31,10 +31,8 @@ namespace traffic_simulator { namespace entity { -class EgoEntity : public VehicleEntity +class EgoEntity : public VehicleEntity, public concealer::FieldOperatorApplication { - concealer::FieldOperatorApplication field_operator_application; - bool is_controlled_by_simulator_{false}; std::optional target_speed_; traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter_; diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 012f74b318d..b4f2dc1c447 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -39,8 +39,7 @@ EgoEntity::EgoEntity( const traffic_simulator_msgs::msg::VehicleParameters & parameters, const Configuration & configuration, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters) -: VehicleEntity(name, entity_status, hdmap_utils_ptr, parameters), - field_operator_application([&]() { +: VehicleEntity(name, entity_status, hdmap_utils_ptr, parameters), FieldOperatorApplication([&]() { if (const auto architecture_type = getParameter(node_parameters, "architecture_type", "awf/universe/20240605"); architecture_type.find("awf/universe") != std::string::npos) { @@ -75,12 +74,12 @@ EgoEntity::EgoEntity( auto EgoEntity::asFieldOperatorApplication() -> concealer::FieldOperatorApplication & { - return field_operator_application; + return *this; } auto EgoEntity::getCurrentAction() const -> std::string { - const auto state = field_operator_application.getAutowareStateName(); + const auto state = getAutowareStateName(); return state.empty() ? "Launching" : state; } @@ -107,7 +106,7 @@ auto EgoEntity::getRouteLanelets(double /*unused horizon*/) -> lanelet::Ids { lanelet::Ids ids{}; - for (const auto & point : field_operator_application.getPathWithLaneId().points) { + for (const auto & point : getPathWithLaneId().points) { ids += point.lane_ids; } @@ -121,13 +120,13 @@ auto EgoEntity::getCurrentPose() const -> const geometry_msgs::msg::Pose & auto EgoEntity::getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray { - return field_operator_application.getWaypoints(); + return FieldOperatorApplication::getWaypoints(); } void EgoEntity::updateFieldOperatorApplication() { - field_operator_application.rethrow(); - field_operator_application.spinSome(); + rethrow(); + spinSome(); } void EgoEntity::onUpdate(double current_time, double step_time) @@ -145,7 +144,7 @@ void EgoEntity::onUpdate(double current_time, double step_time) // prefer current lanelet on ss2 side setStatus(non_canonicalized_updated_status.value(), status_->getLaneletIds()); } else { - field_operator_application.enableAutowareControl(); + enableAutowareControl(); is_controlled_by_simulator_ = false; } } @@ -192,13 +191,13 @@ void EgoEntity::requestAssignRoute(const std::vector & route.push_back(pose_stamped); } - if (not field_operator_application.initialized()) { - field_operator_application.initialize(getMapPose()); - field_operator_application.plan(route); + if (not initialized()) { + initialize(getMapPose()); + plan(route); // NOTE: engage() will be executed at simulation-time 0. } else { - field_operator_application.plan(route); - field_operator_application.engage(); + plan(route); + engage(); } } @@ -244,7 +243,7 @@ auto EgoEntity::requestSpeedChange( "purposes only."); } -void EgoEntity::requestClearRoute() { field_operator_application.clearRoute(); } +void EgoEntity::requestClearRoute() { clearRoute(); } auto EgoEntity::getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints & @@ -278,7 +277,7 @@ auto EgoEntity::requestSpeedChange( auto EgoEntity::setVelocityLimit(double value) -> void // { behavior_parameter_.dynamic_constraints.max_speed = value; - field_operator_application.setVelocityLimit(value); + FieldOperatorApplication::setVelocityLimit(value); } auto EgoEntity::setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void From 3fd6cde4f5cd295755bf2495a40f26bce38e18eb Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 24 Dec 2024 15:30:17 +0900 Subject: [PATCH 08/15] Cleanup `ControlModeCommand` service callback Signed-off-by: yamacir-kit --- external/concealer/src/autoware_universe.cpp | 27 +++++++++++-------- .../traffic_simulator/entity/ego_entity.hpp | 2 +- 2 files changed, 17 insertions(+), 12 deletions(-) diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index 7467290ce20..57490a28aae 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -46,17 +46,22 @@ AutowareUniverse::AutowareUniverse(bool simulate_localization) [this]( const ControlModeCommand::Request::SharedPtr request, ControlModeCommand::Response::SharedPtr response) { - if (request->mode == ControlModeCommand::Request::AUTONOMOUS) { - current_control_mode.store(ControlModeReport::AUTONOMOUS); - response->success = true; - } else if (request->mode == ControlModeCommand::Request::MANUAL) { - /* - NOTE: MANUAL request will come when a remote override is triggered. - But scenario_simulator_v2 don't support a remote override for now. - */ - response->success = false; - } else { - response->success = false; + switch (request->mode) { + case ControlModeCommand::Request::AUTONOMOUS: + current_control_mode.store(ControlModeReport::AUTONOMOUS); + response->success = true; + break; + case ControlModeCommand::Request::MANUAL: + /* + NOTE: MANUAL request will come when a remote override is + triggered. But scenario_simulator_v2 don't support a remote + override for now. + */ + response->success = false; + break; + default: + response->success = false; + break; } })), localization_update_timer( 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 cbb9f01861f..44f2e23601e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -31,7 +31,7 @@ namespace traffic_simulator { namespace entity { -class EgoEntity : public VehicleEntity, public concealer::FieldOperatorApplication +class EgoEntity : public VehicleEntity, private concealer::FieldOperatorApplication { bool is_controlled_by_simulator_{false}; std::optional target_speed_; From 14f2990c4f6d76fbc274dc0f76a52544726db6b3 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 24 Dec 2024 15:50:36 +0900 Subject: [PATCH 09/15] Remove member function `AutowareUniverse::updateLocalization` Signed-off-by: yamacir-kit --- .../include/concealer/autoware_universe.hpp | 4 +- external/concealer/src/autoware_universe.cpp | 97 +++++++++---------- 2 files changed, 49 insertions(+), 52 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index dddaf9adbb5..d4becd62048 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -83,7 +83,7 @@ class AutowareUniverse : public rclcpp::Node, const rclcpp::TimerBase::SharedPtr vehicle_state_update_timer; - std::thread localization_and_vehicle_state_update_thread; + std::thread spinner; std::atomic is_stop_requested = false; @@ -100,8 +100,6 @@ class AutowareUniverse : public rclcpp::Node, auto rethrow() -> void; - auto updateLocalization() -> void; - auto updateVehicleState() -> void; auto getVehicleCommand() const -> std::tuple; diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index 57490a28aae..d4dcebf0b63 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -64,13 +64,56 @@ AutowareUniverse::AutowareUniverse(bool simulate_localization) break; } })), - localization_update_timer( - rclcpp::create_timer( // Autoware.Universe requires localization topics to send data at 50Hz - this, get_clock(), std::chrono::milliseconds(20), [this]() { updateLocalization(); })), + localization_update_timer(rclcpp::create_timer( + this, get_clock(), + std::chrono::milliseconds( + 20), // Autoware.Universe requires localization topics to send data at 50Hz + [this]() { + setAcceleration([this]() { + AccelWithCovarianceStamped message; + message.header.stamp = get_clock()->now(); + message.header.frame_id = "/base_link"; + message.accel.accel = current_acceleration.load(); + message.accel.covariance.at(6 * 0 + 0) = 0.001; // linear x + message.accel.covariance.at(6 * 1 + 1) = 0.001; // linear y + message.accel.covariance.at(6 * 2 + 2) = 0.001; // linear z + message.accel.covariance.at(6 * 3 + 3) = 0.001; // angular x + message.accel.covariance.at(6 * 4 + 4) = 0.001; // angular y + message.accel.covariance.at(6 * 5 + 5) = 0.001; // angular z + return message; + }()); + + setOdometry([this]() { + Odometry message; + message.header.stamp = get_clock()->now(); + message.header.frame_id = "map"; + message.pose.pose = current_pose.load(); + message.pose.covariance = {}; + message.twist.twist = current_twist.load(); + return message; + }()); + + setPose([this]() { + // See https://github.com/tier4/autoware.universe/blob/45ab20af979c5663e5a8d4dda787b1dea8d6e55b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L785-L803 + PoseWithCovarianceStamped message; + message.header.stamp = get_clock()->now(); + message.header.frame_id = "map"; + message.pose.pose = current_pose.load(); + message.pose.covariance.at(6 * 0 + 0) = 0.0225; // XYZRPY_COV_IDX::X_X + message.pose.covariance.at(6 * 1 + 1) = 0.0225; // XYZRPY_COV_IDX::Y_Y + message.pose.covariance.at(6 * 2 + 2) = 0.0225; // XYZRPY_COV_IDX::Z_Z + message.pose.covariance.at(6 * 3 + 3) = 0.000625; // XYZRPY_COV_IDX::ROLL_ROLL + message.pose.covariance.at(6 * 4 + 4) = 0.000625; // XYZRPY_COV_IDX::PITCH_PITCH + message.pose.covariance.at(6 * 5 + 5) = 0.000625; // XYZRPY_COV_IDX::YAW_YAW + return message; + }()); + + setTransform(current_pose.load()); + })), vehicle_state_update_timer( rclcpp::create_timer( // Autoware.Universe requires vehicle state topics to send data at 30Hz this, get_clock(), std::chrono::milliseconds(33), [this]() { updateVehicleState(); })), - localization_and_vehicle_state_update_thread(std::thread([this]() { + spinner(std::thread([this]() { try { while (rclcpp::ok() and not is_stop_requested.load()) { rclcpp::spin_some(get_node_base_interface()); @@ -86,7 +129,7 @@ AutowareUniverse::AutowareUniverse(bool simulate_localization) AutowareUniverse::~AutowareUniverse() { is_stop_requested.store(true); - localization_and_vehicle_state_update_thread.join(); + spinner.join(); } auto AutowareUniverse::rethrow() -> void @@ -96,50 +139,6 @@ auto AutowareUniverse::rethrow() -> void } } -auto AutowareUniverse::updateLocalization() -> void -{ - setAcceleration([this]() { - AccelWithCovarianceStamped message; - message.header.stamp = get_clock()->now(); - message.header.frame_id = "/base_link"; - message.accel.accel = current_acceleration.load(); - message.accel.covariance.at(6 * 0 + 0) = 0.001; // linear x - message.accel.covariance.at(6 * 1 + 1) = 0.001; // linear y - message.accel.covariance.at(6 * 2 + 2) = 0.001; // linear z - message.accel.covariance.at(6 * 3 + 3) = 0.001; // angular x - message.accel.covariance.at(6 * 4 + 4) = 0.001; // angular y - message.accel.covariance.at(6 * 5 + 5) = 0.001; // angular z - return message; - }()); - - setOdometry([this]() { - Odometry message; - message.header.stamp = get_clock()->now(); - message.header.frame_id = "map"; - message.pose.pose = current_pose.load(); - message.pose.covariance = {}; - message.twist.twist = current_twist.load(); - return message; - }()); - - setPose([this]() { - // See https://github.com/tier4/autoware.universe/blob/45ab20af979c5663e5a8d4dda787b1dea8d6e55b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L785-L803 - PoseWithCovarianceStamped message; - message.header.stamp = get_clock()->now(); - message.header.frame_id = "map"; - message.pose.pose = current_pose.load(); - message.pose.covariance.at(6 * 0 + 0) = 0.0225; // XYZRPY_COV_IDX::X_X - message.pose.covariance.at(6 * 1 + 1) = 0.0225; // XYZRPY_COV_IDX::Y_Y - message.pose.covariance.at(6 * 2 + 2) = 0.0225; // XYZRPY_COV_IDX::Z_Z - message.pose.covariance.at(6 * 3 + 3) = 0.000625; // XYZRPY_COV_IDX::ROLL_ROLL - message.pose.covariance.at(6 * 4 + 4) = 0.000625; // XYZRPY_COV_IDX::PITCH_PITCH - message.pose.covariance.at(6 * 5 + 5) = 0.000625; // XYZRPY_COV_IDX::YAW_YAW - return message; - }()); - - setTransform(current_pose.load()); -} - auto AutowareUniverse::updateVehicleState() -> void { setControlModeReport(getControlModeReport()); From 62104e4949ad3f3c746fa8ee5610d401fc0d15d2 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 24 Dec 2024 16:05:52 +0900 Subject: [PATCH 10/15] Remove member function `AutowareUniverse::updateVehicleState` Signed-off-by: yamacir-kit --- .../include/concealer/autoware_universe.hpp | 2 - external/concealer/src/autoware_universe.cpp | 79 +++++++++---------- 2 files changed, 39 insertions(+), 42 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index d4becd62048..3e28ffe89c3 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -100,8 +100,6 @@ class AutowareUniverse : public rclcpp::Node, auto rethrow() -> void; - auto updateVehicleState() -> void; - auto getVehicleCommand() const -> std::tuple; auto getRouteLanelets() const -> std::vector; diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index d4dcebf0b63..b67bdbe7fd1 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -110,9 +110,45 @@ AutowareUniverse::AutowareUniverse(bool simulate_localization) setTransform(current_pose.load()); })), - vehicle_state_update_timer( - rclcpp::create_timer( // Autoware.Universe requires vehicle state topics to send data at 30Hz - this, get_clock(), std::chrono::milliseconds(33), [this]() { updateVehicleState(); })), + vehicle_state_update_timer(rclcpp::create_timer( + this, get_clock(), + std::chrono::milliseconds( + 33), // Autoware.Universe requires vehicle state topics to send data at 30Hz + [this]() { + setControlModeReport(getControlModeReport()); + + setGearReport([this]() { + GearReport message; + message.stamp = get_clock()->now(); + message.report = getGearCommand().command; + return message; + }()); + + setSteeringReport([this]() { + SteeringReport message; + message.stamp = get_clock()->now(); + message.steering_tire_angle = getCommand().lateral.steering_tire_angle; + return message; + }()); + + setVelocityReport([this]() { + const auto twist = current_twist.load(); + VelocityReport message; + message.header.stamp = get_clock()->now(); + message.header.frame_id = "base_link"; + message.longitudinal_velocity = twist.linear.x; + message.lateral_velocity = twist.linear.y; + message.heading_rate = twist.angular.z; + return message; + }()); + + setTurnIndicatorsReport([this]() { + TurnIndicatorsReport message; + message.stamp = get_clock()->now(); + message.report = getTurnIndicatorsCommand().command; + return message; + }()); + })), spinner(std::thread([this]() { try { while (rclcpp::ok() and not is_stop_requested.load()) { @@ -139,43 +175,6 @@ auto AutowareUniverse::rethrow() -> void } } -auto AutowareUniverse::updateVehicleState() -> void -{ - setControlModeReport(getControlModeReport()); - - setGearReport([this]() { - GearReport message; - message.stamp = get_clock()->now(); - message.report = getGearCommand().command; - return message; - }()); - - setSteeringReport([this]() { - SteeringReport message; - message.stamp = get_clock()->now(); - message.steering_tire_angle = getCommand().lateral.steering_tire_angle; - return message; - }()); - - setVelocityReport([this]() { - const auto twist = current_twist.load(); - VelocityReport message; - message.header.stamp = get_clock()->now(); - message.header.frame_id = "base_link"; - message.longitudinal_velocity = twist.linear.x; - message.lateral_velocity = twist.linear.y; - message.heading_rate = twist.angular.z; - return message; - }()); - - setTurnIndicatorsReport([this]() { - TurnIndicatorsReport message; - message.stamp = get_clock()->now(); - message.report = getTurnIndicatorsCommand().command; - return message; - }()); -} - auto AutowareUniverse::getVehicleCommand() const -> std::tuple { const auto control_command = getCommand(); From 223ce5d369bc579367c1b35618cf49dc9770f415 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 24 Dec 2024 17:10:22 +0900 Subject: [PATCH 11/15] Remove data member `FieldOperatorApplication::latest_cooperate_status_array` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 2 -- .../include/concealer/subscriber.hpp | 3 +- .../src/field_operator_application.cpp | 31 ++++++++++--------- 3 files changed, 17 insertions(+), 19 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 13026c8e5cb..2e24fd77d26 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -80,8 +80,6 @@ struct FieldOperatorApplication : public rclcpp::Node, std::string autoware_state; - tier4_rtc_msgs::msg::CooperateStatusArray latest_cooperate_status_array; - std::string minimum_risk_maneuver_state; std::string minimum_risk_maneuver_behavior; diff --git a/external/concealer/include/concealer/subscriber.hpp b/external/concealer/include/concealer/subscriber.hpp index e32a51ab9a4..e08c74cf7b0 100644 --- a/external/concealer/include/concealer/subscriber.hpp +++ b/external/concealer/include/concealer/subscriber.hpp @@ -21,13 +21,12 @@ namespace concealer { template -class Subscriber +struct Subscriber { typename Message::ConstSharedPtr current_value = std::make_shared(); typename rclcpp::Subscription::SharedPtr subscription; -public: auto operator()() const -> const auto & { return *std::atomic_load(¤t_value); } template diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 1c7cf0d04e7..fc2d154dc33 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -81,18 +81,18 @@ bool isValidCooperateStatus( const CooperateStatusType & cooperate_status, std::uint8_t command_type, std::uint8_t module_type) { /** - * NOTE1: the finish_distance filter is set to over -20.0, - * because some valid rtc statuses has negative finish_distance due to the errors of - * localization or numerical calculation. This threshold is advised by a member of TIER IV - * planning and control team. - */ + The finish_distance filter is set to over -20.0, because some valid rtc + statuses has negative finish_distance due to the errors of localization or + numerical calculation. This threshold is advised by a member of TIER IV + planning and control team. + */ /** - * NOTE2: The difference in the variable referred as a distance is the impact of the - * message specification changes in the following URL. - * This was also decided after consulting with a member of TIER IV planning and control team. - * ref: https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef - */ + The difference in the variable referred as a distance is the impact of the + message specification changes in the following URL. This was also decided + after consulting with a member of TIER IV planning and control team. ref: + https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef + */ if constexpr (DetectMember_distance::value) { return cooperate_status.module.type == module_type && command_type != cooperate_status.command_status.type && @@ -133,7 +133,7 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) autoware_state = state_name_of(message.state); }), getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), - getCooperateStatusArray("/api/external/get/rtc_status", rclcpp::QoS(1), *this, [this](const auto & v) { latest_cooperate_status_array = v; }), + getCooperateStatusArray("/api/external/get/rtc_status", rclcpp::QoS(1), *this), getEmergencyState("/api/external/get/emergency", rclcpp::QoS(1), *this, [this](const auto & message) { if (message.emergency) { throw common::Error("Emergency state received"); @@ -480,9 +480,10 @@ auto FieldOperatorApplication::sendCooperateCommand( }) != used_cooperate_statuses.end(); }; + const auto cooperate_status_array = getCooperateStatusArray(); + if (const auto cooperate_status = std::find_if( - latest_cooperate_status_array.statuses.begin(), - latest_cooperate_status_array.statuses.end(), + cooperate_status_array.statuses.begin(), cooperate_status_array.statuses.end(), [module_type = toModuleType(module_name), command_type = to_command_type(command), is_used_cooperate_status](const auto & cooperate_status) { @@ -490,7 +491,7 @@ auto FieldOperatorApplication::sendCooperateCommand( cooperate_status, command_type, module_type) && not is_used_cooperate_status(cooperate_status); }); - cooperate_status == latest_cooperate_status_array.statuses.end()) { + cooperate_status == cooperate_status_array.statuses.end()) { std::stringstream what; what << "Failed to send a cooperate command: Cannot find a valid request to cooperate for module " @@ -504,7 +505,7 @@ auto FieldOperatorApplication::sendCooperateCommand( cooperate_command.command.type = to_command_type(command); auto request = std::make_shared(); - request->stamp = latest_cooperate_status_array.stamp; + request->stamp = cooperate_status_array.stamp; request->commands.push_back(cooperate_command); task_queue.delay([this, request]() { requestCooperateCommands(request, 1); }); From 4378dbe2492abb1db30d27a1aeffd0267cf9669e Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 24 Dec 2024 17:37:32 +0900 Subject: [PATCH 12/15] Remove member function `FieldOperatorApplication::getAutowareStateName` Signed-off-by: yamacir-kit --- .../include/concealer/field_operator_application.hpp | 4 +--- simulation/traffic_simulator/src/entity/ego_entity.cpp | 3 +-- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 2e24fd77d26..ade08bd64d0 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -78,7 +78,7 @@ struct FieldOperatorApplication : public rclcpp::Node, bool initialize_was_called = false; - std::string autoware_state; + std::string autoware_state = "LAUNCHING"; std::string minimum_risk_maneuver_state; @@ -151,8 +151,6 @@ struct FieldOperatorApplication : public rclcpp::Node, auto clearRoute() -> void; - auto getAutowareStateName() const { return autoware_state; } - auto getMinimumRiskManeuverBehaviorName() const { return minimum_risk_maneuver_behavior; } auto getMinimumRiskManeuverStateName() const { return minimum_risk_maneuver_state; } diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index b4f2dc1c447..ca174980300 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -79,8 +79,7 @@ auto EgoEntity::asFieldOperatorApplication() -> concealer::FieldOperatorApplicat auto EgoEntity::getCurrentAction() const -> std::string { - const auto state = getAutowareStateName(); - return state.empty() ? "Launching" : state; + return autoware_state; } auto EgoEntity::getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter From d89385185765192aa10a0378aaeee1d9184d5bd1 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 24 Dec 2024 17:57:12 +0900 Subject: [PATCH 13/15] Remove some member accessor from struct `FieldOperatorApplication` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 23 +------------------ .../src/field_operator_application.cpp | 2 +- .../syntax/user_defined_value_condition.cpp | 7 +++--- .../src/entity/ego_entity.cpp | 7 ++---- 4 files changed, 8 insertions(+), 31 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index ade08bd64d0..7118d87fdf7 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -54,19 +54,6 @@ namespace concealer { -/* ---- NOTE ------------------------------------------------------------------- - * - * The magic class 'FieldOperatorApplication' is a class that makes it easy to work with - * Autoware from C++. The main features of this class are as follows - * - * (1) Launch Autoware in an independent process upon instantiation of the - * class. - * (2) Properly terminates the Autoware process started by the constructor - * upon destruction of the class. - * (3) Probably the simplest instructions to Autoware, consisting of - * initialize, plan, and engage. - * - * -------------------------------------------------------------------------- */ struct FieldOperatorApplication : public rclcpp::Node, public TransitionAssertion { @@ -76,7 +63,7 @@ struct FieldOperatorApplication : public rclcpp::Node, const pid_t process_id = 0; - bool initialize_was_called = false; + bool initialized = false; std::string autoware_state = "LAUNCHING"; @@ -151,16 +138,8 @@ struct FieldOperatorApplication : public rclcpp::Node, auto clearRoute() -> void; - auto getMinimumRiskManeuverBehaviorName() const { return minimum_risk_maneuver_behavior; } - - auto getMinimumRiskManeuverStateName() const { return minimum_risk_maneuver_state; } - - auto getEmergencyStateName() const { return minimum_risk_maneuver_state; } - auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray; - auto initialized() const noexcept { return initialize_was_called; } - auto requestAutoModeForCooperation(const std::string &, bool) -> void; auto rethrow() const { task_queue.rethrow(); } diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index fc2d154dc33..3c037480171 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -348,7 +348,7 @@ auto FieldOperatorApplication::getWaypoints() const -> traffic_simulator_msgs::m auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initial_pose) -> void { - if (not std::exchange(initialize_was_called, true)) { + if (not std::exchange(initialized, true)) { task_queue.delay([this, initial_pose]() { waitForAutowareStateToBe_WAITING_FOR_ROUTE([&]() { #if __has_include() 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 b9cfae284bc..93e2a126cfb 100644 --- a/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp @@ -131,18 +131,19 @@ UserDefinedValueCondition::UserDefinedValueCondition(const pugi::xml_node & node "currentMinimumRiskManeuverState.behavior", [result]() { return make( - asFieldOperatorApplication(result.str(1)).getMinimumRiskManeuverBehaviorName()); + asFieldOperatorApplication(result.str(1)).minimum_risk_maneuver_behavior); }), std::make_pair( "currentMinimumRiskManeuverState.state", [result]() { return make( - asFieldOperatorApplication(result.str(1)).getMinimumRiskManeuverStateName()); + asFieldOperatorApplication(result.str(1)).minimum_risk_maneuver_state); }), std::make_pair( "currentEmergencyState", [result]() { - return make(asFieldOperatorApplication(result.str(1)).getEmergencyStateName()); + return make( + asFieldOperatorApplication(result.str(1)).minimum_risk_maneuver_state); }), std::make_pair( "currentTurnIndicatorsState", diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index ca174980300..abb85f7885f 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -77,10 +77,7 @@ auto EgoEntity::asFieldOperatorApplication() -> concealer::FieldOperatorApplicat return *this; } -auto EgoEntity::getCurrentAction() const -> std::string -{ - return autoware_state; -} +auto EgoEntity::getCurrentAction() const -> std::string { return autoware_state; } auto EgoEntity::getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter { @@ -190,7 +187,7 @@ void EgoEntity::requestAssignRoute(const std::vector & route.push_back(pose_stamped); } - if (not initialized()) { + if (not initialized) { initialize(getMapPose()); plan(route); // NOTE: engage() will be executed at simulation-time 0. From 352696bcfef4c145d05e78a7c0a0bd888af3845a Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 24 Dec 2024 18:16:05 +0900 Subject: [PATCH 14/15] Move function `isValidCooperateStatus` into `sendCooperateCommand` Signed-off-by: yamacir-kit --- .../src/field_operator_application.cpp | 57 +++++++++---------- 1 file changed, 26 insertions(+), 31 deletions(-) diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 3c037480171..97a1ddeda0f 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -76,34 +76,6 @@ auto toModuleType(const std::string & module_name) } } -template -bool isValidCooperateStatus( - const CooperateStatusType & cooperate_status, std::uint8_t command_type, std::uint8_t module_type) -{ - /** - The finish_distance filter is set to over -20.0, because some valid rtc - statuses has negative finish_distance due to the errors of localization or - numerical calculation. This threshold is advised by a member of TIER IV - planning and control team. - */ - - /** - The difference in the variable referred as a distance is the impact of the - message specification changes in the following URL. This was also decided - after consulting with a member of TIER IV planning and control team. ref: - https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef - */ - if constexpr (DetectMember_distance::value) { - return cooperate_status.module.type == module_type && - command_type != cooperate_status.command_status.type && - cooperate_status.distance >= -20.0; - } else { - return cooperate_status.module.type == module_type && - command_type != cooperate_status.command_status.type && - cooperate_status.finish_distance >= -20.0; - } -} - // clang-format off FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) : rclcpp::Node("concealer_user", "simulation", rclcpp::NodeOptions().use_global_arguments(false)), @@ -480,15 +452,38 @@ auto FieldOperatorApplication::sendCooperateCommand( }) != used_cooperate_statuses.end(); }; + auto is_valid_cooperate_status = + [](const auto & cooperate_status, auto command_type, auto module_type) { + /** + The finish_distance filter is set to over -20.0, because some valid rtc + statuses has negative finish_distance due to the errors of localization or + numerical calculation. This threshold is advised by a member of TIER IV + planning and control team. + + The difference in the variable referred as a distance is the impact of the + message specification changes in the following URL. This was also decided + after consulting with a member of TIER IV planning and control team. ref: + https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef + */ + if constexpr (DetectMember_distance::value) { + return cooperate_status.module.type == module_type && + command_type != cooperate_status.command_status.type && + cooperate_status.distance >= -20.0; + } else { + return cooperate_status.module.type == module_type && + command_type != cooperate_status.command_status.type && + cooperate_status.finish_distance >= -20.0; + } + }; + const auto cooperate_status_array = getCooperateStatusArray(); if (const auto cooperate_status = std::find_if( cooperate_status_array.statuses.begin(), cooperate_status_array.statuses.end(), - [module_type = toModuleType(module_name), + [&, module_type = toModuleType(module_name), command_type = to_command_type(command), is_used_cooperate_status](const auto & cooperate_status) { - return isValidCooperateStatus( - cooperate_status, command_type, module_type) && + return is_valid_cooperate_status(cooperate_status, command_type, module_type) && not is_used_cooperate_status(cooperate_status); }); cooperate_status == cooperate_status_array.statuses.end()) { From 0897326bc0edbfb2961fc687d2ae048b3137bcdf Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 24 Dec 2024 18:25:44 +0900 Subject: [PATCH 15/15] Cleanup member function `FieldOperatorApplication::sendCooperateCommand` Signed-off-by: yamacir-kit --- .../src/field_operator_application.cpp | 24 ++++++++----------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 97a1ddeda0f..5e42344bf14 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -418,18 +418,15 @@ auto FieldOperatorApplication::requestAutoModeForCooperation( auto FieldOperatorApplication::sendCooperateCommand( const std::string & module_name, const std::string & command) -> void { - auto to_command_type = [](const auto & command) { - static const std::unordered_map command_type_map = { - {"ACTIVATE", tier4_rtc_msgs::msg::Command::ACTIVATE}, - {"DEACTIVATE", tier4_rtc_msgs::msg::Command::DEACTIVATE}, - }; - if (const auto command_type = command_type_map.find(command); - command_type == command_type_map.end()) { - throw common::Error("Unexpected command for tier4_rtc_msgs::msg::Command: ", command, "."); + const auto command_type = [&]() { + if (command == "ACTIVATE") { + return tier4_rtc_msgs::msg::Command::ACTIVATE; + } else if (command == "DEACTIVATE") { + return tier4_rtc_msgs::msg::Command::DEACTIVATE; } else { - return command_type->second; + throw common::Error("Unexpected command for tier4_rtc_msgs::msg::Command: ", command, "."); } - }; + }(); /* NOTE: Used cooperate statuses will be deleted correctly in Autoware side @@ -480,9 +477,8 @@ auto FieldOperatorApplication::sendCooperateCommand( if (const auto cooperate_status = std::find_if( cooperate_status_array.statuses.begin(), cooperate_status_array.statuses.end(), - [&, module_type = toModuleType(module_name), - command_type = to_command_type(command), - is_used_cooperate_status](const auto & cooperate_status) { + [&, module_type = toModuleType(module_name)]( + const auto & cooperate_status) { return is_valid_cooperate_status(cooperate_status, command_type, module_type) && not is_used_cooperate_status(cooperate_status); }); @@ -497,7 +493,7 @@ auto FieldOperatorApplication::sendCooperateCommand( tier4_rtc_msgs::msg::CooperateCommand cooperate_command; cooperate_command.module = cooperate_status->module; cooperate_command.uuid = cooperate_status->uuid; - cooperate_command.command.type = to_command_type(command); + cooperate_command.command.type = command_type; auto request = std::make_shared(); request->stamp = cooperate_status_array.stamp;