From 09de4c5df200b333133ffc293cd4c82880f7b169 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 12:12:16 +0900 Subject: [PATCH 01/34] rename argument and member variables Signed-off-by: Masaya Kataoka --- .../traffic/traffic_sink.hpp | 14 ++++++------- .../src/traffic/traffic_sink.cpp | 20 +++++++++---------- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 0754ace3f92..4409aef8d10 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -42,10 +42,10 @@ class TrafficSink : public TrafficModuleBase { public: explicit TrafficSink( - lanelet::Id lanelet_id, double radius, const geometry_msgs::msg::Point & position, - const std::function(void)> & get_entity_names_function, - const std::function & get_entity_pose_function, - const std::function & despawn_function); + const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, + const std::function(void)> & get_entity_names, + const std::function & get_entity_pose, + const std::function & despawn); const lanelet::Id lanelet_id; const double radius; const geometry_msgs::msg::Point position; @@ -54,9 +54,9 @@ class TrafficSink : public TrafficModuleBase -> void override; private: - const std::function(void)> get_entity_names_function; - const std::function get_entity_pose_function; - const std::function despawn_function; + const std::function(void)> get_entity_names; + const std::function get_entity_pose; + const std::function despawn; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index b85663f17d6..990a439b50b 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -38,28 +38,28 @@ namespace traffic_simulator namespace traffic { TrafficSink::TrafficSink( - lanelet::Id lanelet_id, double radius, const geometry_msgs::msg::Point & position, - const std::function(void)> & get_entity_names_function, - const std::function & get_entity_pose_function, - const std::function & despawn_function) + const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, + const std::function(void)> & get_entity_names, + const std::function & get_entity_pose, + const std::function & despawn) : TrafficModuleBase(), lanelet_id(lanelet_id), radius(radius), position(position), - get_entity_names_function(get_entity_names_function), - get_entity_pose_function(get_entity_pose_function), - despawn_function(despawn_function) + get_entity_names(get_entity_names), + get_entity_pose(get_entity_pose), + despawn(despawn) { } void TrafficSink::execute( [[maybe_unused]] const double current_time, [[maybe_unused]] const double step_time) { - const auto names = get_entity_names_function(); + const auto names = get_entity_names(); for (const auto & name : names) { - const auto pose = get_entity_pose_function(name); + const auto pose = get_entity_pose(name); if (math::geometry::getDistance(position, pose) <= radius) { - despawn_function(name); + despawn(name); } } } From fddf3db4c395b3158b39ac3753be601cc7dcf33d Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 12:16:47 +0900 Subject: [PATCH 02/34] rename variable in TrafficController class Signed-off-by: Masaya Kataoka --- .../traffic/traffic_controller.hpp | 12 ++++++------ .../src/traffic/traffic_controller.cpp | 15 +++++++-------- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 75c2a53ea83..f5696f42d90 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -43,9 +43,9 @@ class TrafficController public: explicit TrafficController( std::shared_ptr hdmap_utils, - const std::function(void)> & get_entity_names_function, - const std::function & get_entity_pose_function, - const std::function & despawn_function, bool auto_sink = false); + const std::function(void)> & get_entity_names, + const std::function & get_entity_pose, + const std::function & despawn, bool auto_sink = false); template void addModule(Ts &&... xs) @@ -60,9 +60,9 @@ class TrafficController void autoSink(); const std::shared_ptr hdmap_utils_; std::vector> modules_; - const std::function(void)> get_entity_names_function; - const std::function get_entity_pose_function; - const std::function despawn_function; + const std::function(void)> get_entity_names; + const std::function get_entity_pose; + const std::function despawn; public: const bool auto_sink; diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index cd8ab6d5a37..cc11ab58661 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -38,13 +38,13 @@ namespace traffic { TrafficController::TrafficController( std::shared_ptr hdmap_utils, - const std::function(void)> & get_entity_names_function, - const std::function & get_entity_pose_function, - const std::function & despawn_function, bool auto_sink) + const std::function(void)> & get_entity_names, + const std::function & get_entity_pose, + const std::function & despawn, bool auto_sink) : hdmap_utils_(hdmap_utils), - get_entity_names_function(get_entity_names_function), - get_entity_pose_function(get_entity_pose_function), - despawn_function(despawn_function), + get_entity_names(get_entity_names), + get_entity_pose(get_entity_pose), + despawn(despawn), auto_sink(auto_sink) { if (auto_sink) { @@ -61,8 +61,7 @@ void TrafficController::autoSink() lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_); addModule( - lanelet_id, 1, pose.position, get_entity_names_function, get_entity_pose_function, - despawn_function); + lanelet_id, 1, pose.position, get_entity_names, get_entity_pose, despawn); } } } From 1d622794d2d0e86b8d8fa5321efcd17e0307c943 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 12:36:22 +0900 Subject: [PATCH 03/34] add get_entity_type function Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/api/api.hpp | 7 +++++++ .../traffic_simulator/traffic/traffic_controller.hpp | 2 ++ .../include/traffic_simulator/traffic/traffic_sink.hpp | 3 +++ .../traffic_simulator/src/traffic/traffic_controller.cpp | 5 ++++- simulation/traffic_simulator/src/traffic/traffic_sink.cpp | 2 ++ 5 files changed, 18 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 1d5f44795fa..60daaa9498e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -75,6 +75,13 @@ class API getROS2Parameter("architecture_type", "awf/universe"))), traffic_controller_ptr_(std::make_shared( entity_manager_ptr_->getHdmapUtils(), [this]() { return API::getEntityNames(); }, + [this](const auto & entity_name) { + if (const auto entity = getEntity(entity_name)) { + return entity->getEntityType(); + } else { + THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); + } + }, [this](const auto & entity_name) { if (const auto entity = getEntity(entity_name)) { return entity->getMapPose(); diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index f5696f42d90..0f5832e32ee 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -44,6 +44,7 @@ class TrafficController explicit TrafficController( std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, + const std::function & get_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink = false); @@ -61,6 +62,7 @@ class TrafficController const std::shared_ptr hdmap_utils_; std::vector> modules_; const std::function(void)> get_entity_names; + const std::function get_entity_type; const std::function get_entity_pose; const std::function despawn; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 4409aef8d10..3d51deeb299 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include #include @@ -44,6 +45,7 @@ class TrafficSink : public TrafficModuleBase explicit TrafficSink( const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, + const std::function & get_entity_type, const std::function & get_entity_pose, const std::function & despawn); const lanelet::Id lanelet_id; @@ -55,6 +57,7 @@ class TrafficSink : public TrafficModuleBase private: const std::function(void)> get_entity_names; + const std::function get_entity_type; const std::function get_entity_pose; const std::function despawn; }; diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index cc11ab58661..8f29625ec8f 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -39,10 +40,12 @@ namespace traffic TrafficController::TrafficController( std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, + const std::function & get_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink) : hdmap_utils_(hdmap_utils), get_entity_names(get_entity_names), + get_entity_type(get_entity_type), get_entity_pose(get_entity_pose), despawn(despawn), auto_sink(auto_sink) @@ -61,7 +64,7 @@ void TrafficController::autoSink() lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_); addModule( - lanelet_id, 1, pose.position, get_entity_names, get_entity_pose, despawn); + lanelet_id, 1, pose.position, get_entity_names, get_entity_type, get_entity_pose, despawn); } } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 990a439b50b..9a4745217ee 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -40,6 +40,7 @@ namespace traffic TrafficSink::TrafficSink( const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, + const std::function & get_entity_type, const std::function & get_entity_pose, const std::function & despawn) : TrafficModuleBase(), @@ -47,6 +48,7 @@ TrafficSink::TrafficSink( radius(radius), position(position), get_entity_names(get_entity_names), + get_entity_type(get_entity_type), get_entity_pose(get_entity_pose), despawn(despawn) { From 4824083a04afa8203467c9beb9da6679585dc211 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 12:45:46 +0900 Subject: [PATCH 04/34] add sinkable_entity_type variable Signed-off-by: Masaya Kataoka --- .../traffic_simulator/include/traffic_simulator/api/api.hpp | 2 ++ .../include/traffic_simulator/traffic/traffic_controller.hpp | 3 +++ .../include/traffic_simulator/traffic/traffic_sink.hpp | 2 ++ .../traffic_simulator/src/traffic/traffic_controller.cpp | 5 ++++- simulation/traffic_simulator/src/traffic/traffic_sink.cpp | 3 +++ 5 files changed, 14 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 60daaa9498e..e2ef5ed517b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -82,6 +83,7 @@ class API THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); } }, + std::set({}), [this](const auto & entity_name) { if (const auto entity = getEntity(entity_name)) { return entity->getMapPose(); diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 0f5832e32ee..3d2f86a0c44 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -27,6 +27,7 @@ #define TRAFFIC_SIMULATOR__TRAFFIC__TRAFFIC_CONTROLLER_HPP_ #include +#include #include #include #include @@ -45,6 +46,7 @@ class TrafficController std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, const std::function & get_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink = false); @@ -63,6 +65,7 @@ class TrafficController std::vector> modules_; const std::function(void)> get_entity_names; const std::function get_entity_type; + const std::set sinkable_entity_type; const std::function get_entity_pose; const std::function despawn; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 3d51deeb299..0b1a9246252 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -46,6 +46,7 @@ class TrafficSink : public TrafficModuleBase const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, const std::function & get_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn); const lanelet::Id lanelet_id; @@ -58,6 +59,7 @@ class TrafficSink : public TrafficModuleBase private: const std::function(void)> get_entity_names; const std::function get_entity_type; + const std::set sinkable_entity_type; const std::function get_entity_pose; const std::function despawn; }; diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 8f29625ec8f..b06c0670828 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -41,11 +41,13 @@ TrafficController::TrafficController( std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, const std::function & get_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink) : hdmap_utils_(hdmap_utils), get_entity_names(get_entity_names), get_entity_type(get_entity_type), + sinkable_entity_type(sinkable_entity_type), get_entity_pose(get_entity_pose), despawn(despawn), auto_sink(auto_sink) @@ -64,7 +66,8 @@ void TrafficController::autoSink() lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_); addModule( - lanelet_id, 1, pose.position, get_entity_names, get_entity_type, get_entity_pose, despawn); + lanelet_id, 1, pose.position, get_entity_names, get_entity_type, sinkable_entity_type, + get_entity_pose, despawn); } } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 9a4745217ee..4cb1fa7ba78 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,7 @@ TrafficSink::TrafficSink( const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, const std::function & get_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn) : TrafficModuleBase(), @@ -49,6 +51,7 @@ TrafficSink::TrafficSink( position(position), get_entity_names(get_entity_names), get_entity_type(get_entity_type), + sinkable_entity_type(sinkable_entity_type), get_entity_pose(get_entity_pose), despawn(despawn) { From 61577fc51ea4dc019e531da312c5c2f685726e68 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 12:51:45 +0900 Subject: [PATCH 05/34] add sinkable_entity_type to the configuration class Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/api/configuration.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index 155fab255b9..adf967debf0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -21,7 +21,9 @@ #include #include #include +#include #include +#include namespace traffic_simulator { @@ -33,6 +35,8 @@ struct Configuration bool auto_sink = true; + const std::set sinkable_entity_type = {}; + bool verbose = false; bool standalone_mode = false; From 25b59a5dac02e3a48311c328ebcd8c37a8b76dc6 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 13:14:55 +0900 Subject: [PATCH 06/34] add doxygen comment Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/api/api.hpp | 2 +- .../traffic_simulator/traffic/traffic_sink.hpp | 12 ++++++++++++ .../traffic_simulator/src/traffic/traffic_sink.cpp | 3 +++ 3 files changed, 16 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index e2ef5ed517b..a30855dc2cc 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -83,7 +83,7 @@ class API THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); } }, - std::set({}), + configuration.sinkable_entity_type, [this](const auto & entity_name) { if (const auto entity = getEntity(entity_name)) { return entity->getMapPose(); diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 0b1a9246252..679e33b53dd 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -42,6 +42,18 @@ namespace traffic class TrafficSink : public TrafficModuleBase { public: + /** + * @brief Construct a new Traffic Sink object + * @param lanelet_id Lanelet ID for visualization + * @todo lanelet_id value is only used for visualization and its very confusing. So it should be refactor. + * @param radius Entity is despawned when the distance between the entity's coordinates in the Map coordinate system and the TrafficSink's coordinates is less than this value. + * @param position Position of the traffic sink. + * @param get_entity_names Function to get the name of entity + * @param get_entity_type Function to get the type of entity + * @param sinkable_entity_type If this type is applicable, the entity is dewpanned only when it approaches radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. + * @param get_entity_pose Function to get the pose of entity. + * @param despawn Function to despawn entity. + */ explicit TrafficSink( const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 4cb1fa7ba78..0c115120247 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -62,6 +62,9 @@ void TrafficSink::execute( { const auto names = get_entity_names(); for (const auto & name : names) { + const auto is_sinkable_entity = [](const auto) { + + }; const auto pose = get_entity_pose(name); if (math::geometry::getDistance(position, pose) <= radius) { despawn(name); From f7a2b9280da556d55a35bdc8e0fbbdb0a9141bfd Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 13:54:07 +0900 Subject: [PATCH 07/34] enable compare entity type Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/api/configuration.hpp | 2 +- .../traffic_simulator/data_type/entity_status.hpp | 8 ++++++++ .../traffic_simulator/traffic/traffic_controller.hpp | 4 ++-- .../traffic_simulator/traffic/traffic_sink.hpp | 4 ++-- .../src/traffic/traffic_controller.cpp | 2 +- .../traffic_simulator/src/traffic/traffic_sink.cpp | 11 +++++++---- 6 files changed, 21 insertions(+), 10 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index adf967debf0..bebbdc5d049 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -35,7 +35,7 @@ struct Configuration bool auto_sink = true; - const std::set sinkable_entity_type = {}; + const std::set sinkable_entity_type = {}; bool verbose = false; diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp index 68a781808a3..94d13834eba 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp @@ -26,6 +26,14 @@ using EntityStatus = traffic_simulator_msgs::msg::EntityStatus; using EntityType = traffic_simulator_msgs::msg::EntityType; using EntitySubtype = traffic_simulator_msgs::msg::EntitySubtype; +struct EntityTypeComparator +{ + bool operator()(const EntityType & lhs, const EntityType & rhs) const + { + return lhs.type < rhs.type; + } +}; + inline namespace entity_status { class CanonicalizedEntityStatus diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 3d2f86a0c44..0b38cb03352 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -46,7 +46,7 @@ class TrafficController std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink = false); @@ -65,7 +65,7 @@ class TrafficController std::vector> modules_; const std::function(void)> get_entity_names; const std::function get_entity_type; - const std::set sinkable_entity_type; + const std::set sinkable_entity_type; const std::function get_entity_pose; const std::function despawn; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 679e33b53dd..32e85825cc5 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -58,7 +58,7 @@ class TrafficSink : public TrafficModuleBase const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn); const lanelet::Id lanelet_id; @@ -71,7 +71,7 @@ class TrafficSink : public TrafficModuleBase private: const std::function(void)> get_entity_names; const std::function get_entity_type; - const std::set sinkable_entity_type; + const std::set sinkable_entity_type; const std::function get_entity_pose; const std::function despawn; }; diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index b06c0670828..67efe0189cf 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -41,7 +41,7 @@ TrafficController::TrafficController( std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink) : hdmap_utils_(hdmap_utils), diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 0c115120247..d5a38c38bd6 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -42,7 +42,7 @@ TrafficSink::TrafficSink( const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn) : TrafficModuleBase(), @@ -62,11 +62,14 @@ void TrafficSink::execute( { const auto names = get_entity_names(); for (const auto & name : names) { - const auto is_sinkable_entity = [](const auto) { - + const auto is_sinkable_entity = [this](const auto & entity_name) { + return sinkable_entity_type.empty() ? true : [this](const auto & entity_name) { + return sinkable_entity_type.find(get_entity_type(entity_name)) != + sinkable_entity_type.end(); + }(entity_name); }; const auto pose = get_entity_pose(name); - if (math::geometry::getDistance(position, pose) <= radius) { + if (is_sinkable_entity(name) and math::geometry::getDistance(position, pose) <= radius) { despawn(name); } } From 243a1d17362dee41fde1ca57c10c11ed2fe984a7 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 14:09:25 +0900 Subject: [PATCH 08/34] enable set traffic sink in cpp scenario Signed-off-by: Masaya Kataoka --- mock/cpp_mock_scenarios/CMakeLists.txt | 3 +- .../cpp_mock_scenarios/cpp_scenario_node.hpp | 6 +- .../src/traffic_sink/CMakeLists.txt | 14 ++++ .../src/traffic_sink/auto_sink_vehicle.cpp | 71 +++++++++++++++++++ .../traffic_simulator/api/configuration.hpp | 2 +- 5 files changed, 93 insertions(+), 3 deletions(-) create mode 100644 mock/cpp_mock_scenarios/src/traffic_sink/CMakeLists.txt create mode 100644 mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp diff --git a/mock/cpp_mock_scenarios/CMakeLists.txt b/mock/cpp_mock_scenarios/CMakeLists.txt index 669465e6482..883da343c49 100644 --- a/mock/cpp_mock_scenarios/CMakeLists.txt +++ b/mock/cpp_mock_scenarios/CMakeLists.txt @@ -55,8 +55,9 @@ if(BUILD_CPP_MOCK_SCENARIOS) add_subdirectory(src/random_scenario) add_subdirectory(src/spawn) add_subdirectory(src/speed_planning) - add_subdirectory(src/traffic_source) add_subdirectory(src/synchronized_action) + add_subdirectory(src/traffic_sink) + add_subdirectory(src/traffic_source) # add_subdirectory(src/respawn_ego) endif() diff --git a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp index 4d1141c8fc7..925c2661170 100644 --- a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp +++ b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp @@ -71,7 +71,9 @@ class CppScenarioNode : public rclcpp::Node int timeout_; auto configure( const std::string & map_path, const std::string & lanelet2_map_file, - const std::string & scenario_filename, const bool verbose) -> traffic_simulator::Configuration + const std::string & scenario_filename, const bool verbose, const bool auto_sink = false, + const std::set + sinkable_entity_type = {}) -> traffic_simulator::Configuration { auto configuration = traffic_simulator::Configuration(map_path); { @@ -79,6 +81,8 @@ class CppScenarioNode : public rclcpp::Node // configuration.lanelet2_map_file = "lanelet2_map_with_private_road_and_walkway_ele_fix.osm"; configuration.scenario_path = scenario_filename; configuration.verbose = verbose; + configuration.auto_sink = auto_sink; + configuration.sinkable_entity_type = sinkable_entity_type; } checkConfiguration(configuration); return configuration; diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/CMakeLists.txt b/mock/cpp_mock_scenarios/src/traffic_sink/CMakeLists.txt new file mode 100644 index 00000000000..e1966600cb5 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/traffic_sink/CMakeLists.txt @@ -0,0 +1,14 @@ +ament_auto_add_executable(auto_sink_vehicle + auto_sink_vehicle.cpp +) +target_link_libraries(auto_sink_vehicle cpp_scenario_node) + +install(TARGETS + auto_sink_vehicle + DESTINATION lib/cpp_mock_scenarios +) + +if(BUILD_TESTING) + include(../../cmake/add_cpp_mock_scenario_test.cmake) + add_cpp_mock_scenario_test(${PROJECT_NAME} "auto_sink_vehicle" "5.0") +endif() diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp new file mode 100644 index 00000000000..f5c767829e1 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -0,0 +1,71 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cpp_mock_scenarios +{ +class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit AutoSinkVehicleScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "auto_sink_vehicle", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", + "lanelet2_map.osm", __FILE__, false, option) + { + start(); + } + +private: + void onUpdate() override + { + const auto map_pose = traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 120545, 0.0, 0.0, api_.getHdmapUtils())); + if (api_.reachPosition("ego", map_pose, 0.1)) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } else { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + api_.spawn( + "ego", + traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 120545, 0.0, 0.0, api_.getHdmapUtils())), + getVehicleParameters()); + } +}; +} // namespace cpp_mock_scenarios + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index bebbdc5d049..3cfca6510ab 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -35,7 +35,7 @@ struct Configuration bool auto_sink = true; - const std::set sinkable_entity_type = {}; + std::set sinkable_entity_type = {}; bool verbose = false; From 22c447028dd896bdcab18b1c0cee13569dbf70b9 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 14:19:17 +0900 Subject: [PATCH 09/34] enable sink vehicle Signed-off-by: Masaya Kataoka --- .../include/cpp_mock_scenarios/cpp_scenario_node.hpp | 4 +++- mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp | 8 ++++++-- .../src/traffic_sink/auto_sink_vehicle.cpp | 4 +++- 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp index 925c2661170..92246fd7ae8 100644 --- a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp +++ b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp @@ -33,7 +33,9 @@ class CppScenarioNode : public rclcpp::Node explicit CppScenarioNode( const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, - const bool verbose, const rclcpp::NodeOptions & option); + const bool verbose, const rclcpp::NodeOptions & option, const bool auto_sink = false, + const std::set + sinkable_entity_type = {}); void start(); void stop(Result result, const std::string & description = ""); void expectThrow() { exception_expect_ = true; } diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index 7864980bdab..bd2d4ba1b2a 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -20,10 +20,14 @@ namespace cpp_mock_scenarios CppScenarioNode::CppScenarioNode( const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, - const rclcpp::NodeOptions & option) + const rclcpp::NodeOptions & option, const bool auto_sink, + const std::set + sinkable_entity_type) : Node(node_name, option), api_( - this, configure(map_path, lanelet2_map_file, scenario_filename, verbose), + this, + configure( + map_path, lanelet2_map_file, scenario_filename, verbose, auto_sink, sinkable_entity_type), declare_parameter("global_real_time_factor", 1.0), declare_parameter("global_frame_rate", 20.0)), scenario_filename_(scenario_filename), diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index f5c767829e1..cbaa9ca9278 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -30,7 +30,9 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode explicit AutoSinkVehicleScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "auto_sink_vehicle", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option) + "lanelet2_map.osm", __FILE__, false, option, true, + {traffic_simulator_msgs::build().type( + traffic_simulator::EntityType::VEHICLE)}) { start(); } From ff273988855e1929596d9464a71813a0dfc33c68 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 14:29:29 +0900 Subject: [PATCH 10/34] add testcase for autosink Signed-off-by: Masaya Kataoka --- .../src/traffic_sink/auto_sink_vehicle.cpp | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index cbaa9ca9278..4f4a55a7a72 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -40,13 +40,12 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - const auto map_pose = traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils())); - if (api_.reachPosition("ego", map_pose, 0.1)) { - stop(cpp_mock_scenarios::Result::SUCCESS); - } else { - stop(cpp_mock_scenarios::Result::FAILURE); + if (api_.getCurrentTime() >= 0.1) { + if (api_.entityExists("ego") && !api_.entityExists("ego")) { + stop(cpp_mock_scenarios::Result::FAILURE); + } else { + stop(cpp_mock_scenarios::Result::SUCCESS); + } } } @@ -56,8 +55,14 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode "ego", traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils())), + 34774, 11.0, 0.0, api_.getHdmapUtils())), getVehicleParameters()); + api_.spawn( + "bob", + traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34774, 11.0, 0.0, api_.getHdmapUtils())), + getPedestrianParameters()); } }; } // namespace cpp_mock_scenarios From c15b04eedfb474f220f0a349d4ec8a7bbbf72346 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 14:32:26 +0900 Subject: [PATCH 11/34] fix typo Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/traffic/traffic_sink.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 32e85825cc5..c40086dfcf3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -50,7 +50,7 @@ class TrafficSink : public TrafficModuleBase * @param position Position of the traffic sink. * @param get_entity_names Function to get the name of entity * @param get_entity_type Function to get the type of entity - * @param sinkable_entity_type If this type is applicable, the entity is dewpanned only when it approaches radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. + * @param sinkable_entity_type If this type is applicable, the entity is despawned only when it approaches radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. * @param get_entity_pose Function to get the pose of entity. * @param despawn Function to despawn entity. */ From 9d3a6269c67a1e709ed069a45e9aeb16b05fa803 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 14:38:29 +0900 Subject: [PATCH 12/34] fix typo Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/traffic/traffic_sink.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index c40086dfcf3..55e40f6bc70 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -46,11 +46,11 @@ class TrafficSink : public TrafficModuleBase * @brief Construct a new Traffic Sink object * @param lanelet_id Lanelet ID for visualization * @todo lanelet_id value is only used for visualization and its very confusing. So it should be refactor. - * @param radius Entity is despawned when the distance between the entity's coordinates in the Map coordinate system and the TrafficSink's coordinates is less than this value. + * @param radius The entity despawns when the distance between the entity's coordinates in the Map coordinate system and the TrafficSink's coordinates is less than this value. * @param position Position of the traffic sink. * @param get_entity_names Function to get the name of entity * @param get_entity_type Function to get the type of entity - * @param sinkable_entity_type If this type is applicable, the entity is despawned only when it approaches radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. + * @param sinkable_entity_type If this type is applicable, the entity despawn only when it approaches radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. * @param get_entity_pose Function to get the pose of entity. * @param despawn Function to despawn entity. */ From 8dc26c460cdd772a3b460bf72bd555848a07e506 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 14:45:29 +0900 Subject: [PATCH 13/34] fix typo Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/traffic/traffic_sink.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 55e40f6bc70..752f9288a1a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -46,7 +46,7 @@ class TrafficSink : public TrafficModuleBase * @brief Construct a new Traffic Sink object * @param lanelet_id Lanelet ID for visualization * @todo lanelet_id value is only used for visualization and its very confusing. So it should be refactor. - * @param radius The entity despawns when the distance between the entity's coordinates in the Map coordinate system and the TrafficSink's coordinates is less than this value. + * @param radius The entity despawn when the distance between the entity's coordinates in the Map coordinate system and the TrafficSink's coordinates is less than this value. * @param position Position of the traffic sink. * @param get_entity_names Function to get the name of entity * @param get_entity_type Function to get the type of entity From db654279660be5a9385d7b40db2b350918184822 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Mon, 11 Nov 2024 12:26:52 +0900 Subject: [PATCH 14/34] modify launch file Signed-off-by: Masaya Kataoka --- .../launch/mock_test.launch.py | 76 ++++++++++++++----- 1 file changed, 56 insertions(+), 20 deletions(-) diff --git a/mock/cpp_mock_scenarios/launch/mock_test.launch.py b/mock/cpp_mock_scenarios/launch/mock_test.launch.py index 0289e874095..0aa7d1ef1a7 100644 --- a/mock/cpp_mock_scenarios/launch/mock_test.launch.py +++ b/mock/cpp_mock_scenarios/launch/mock_test.launch.py @@ -30,7 +30,13 @@ from launch.events import Shutdown from launch.event_handlers import OnProcessExit, OnProcessIO -from launch.actions import EmitEvent, RegisterEventHandler, LogInfo, TimerAction, OpaqueFunction +from launch.actions import ( + EmitEvent, + RegisterEventHandler, + LogInfo, + TimerAction, + OpaqueFunction, +) from launch.actions.declare_launch_argument import DeclareLaunchArgument from launch.substitutions.launch_configuration import LaunchConfiguration @@ -40,6 +46,7 @@ from pathlib import Path + class Color: BLACK = "\033[30m" RED = "\033[31m" @@ -69,6 +76,7 @@ def on_stdout_output(event: launch.Event) -> None: if lines[0] == "cpp_scenario:success": print(Color.GREEN + "Scenario Succeed" + Color.END) + def architecture_types(): return ["awf/universe", "awf/universe/20230906"] @@ -94,8 +102,13 @@ def default_autoware_launch_file_of(architecture_type): "awf/universe/20230906": "planning_simulator.launch.xml", }[architecture_type] + def default_rviz_config_file(): - return Path(get_package_share_directory("traffic_simulator")) / "config/scenario_simulator_v2.rviz" + return ( + Path(get_package_share_directory("traffic_simulator")) + / "config/scenario_simulator_v2.rviz" + ) + def launch_setup(context, *args, **kwargs): # fmt: off @@ -125,20 +138,38 @@ def launch_setup(context, *args, **kwargs): junit_path = LaunchConfiguration("junit_path", default="/tmp/output.xunit.xml") # fmt: on - print(f"architecture_type := {architecture_type.perform(context)}") - print(f"autoware_launch_file := {autoware_launch_file.perform(context)}") - print(f"autoware_launch_package := {autoware_launch_package.perform(context)}") - print(f"consider_acceleration_by_road_slope := {consider_acceleration_by_road_slope.perform(context)}") - print(f"consider_pose_by_road_slope := {consider_pose_by_road_slope.perform(context)}") - print(f"global_frame_rate := {global_frame_rate.perform(context)}") - print(f"global_real_time_factor := {global_real_time_factor.perform(context)}") + print( + f"architecture_type := {architecture_type.perform(context)}" + ) + print( + f"autoware_launch_file := {autoware_launch_file.perform(context)}" + ) + print( + f"autoware_launch_package := {autoware_launch_package.perform(context)}" + ) + print( + f"consider_acceleration_by_road_slope := {consider_acceleration_by_road_slope.perform(context)}" + ) + print( + f"consider_pose_by_road_slope := {consider_pose_by_road_slope.perform(context)}" + ) + print( + f"global_frame_rate := {global_frame_rate.perform(context)}" + ) + print( + f"global_real_time_factor := {global_real_time_factor.perform(context)}" + ) print(f"global_timeout := {global_timeout.perform(context)}") - print(f"initialize_duration := {initialize_duration.perform(context)}") + print( + f"initialize_duration := {initialize_duration.perform(context)}" + ) print(f"launch_autoware := {launch_autoware.perform(context)}") print(f"launch_rviz := {launch_rviz.perform(context)}") print(f"output_directory := {output_directory.perform(context)}") print(f"port := {port.perform(context)}") - print(f"publish_empty_context := {publish_empty_context.perform(context)}") + print( + f"publish_empty_context := {publish_empty_context.perform(context)}" + ) print(f"record := {record.perform(context)}") print(f"rviz_config := {rviz_config.perform(context)}") print(f"scenario := {scenario.perform(context)}") @@ -154,12 +185,14 @@ def make_parameters(): {"architecture_type": architecture_type}, {"autoware_launch_file": autoware_launch_file}, {"autoware_launch_package": autoware_launch_package}, - {"consider_acceleration_by_road_slope": consider_acceleration_by_road_slope}, + { + "consider_acceleration_by_road_slope": consider_acceleration_by_road_slope + }, {"consider_pose_by_road_slope": consider_pose_by_road_slope}, {"initialize_duration": initialize_duration}, {"launch_autoware": launch_autoware}, {"port": port}, - {"publish_empty_context" : publish_empty_context}, + {"publish_empty_context": publish_empty_context}, {"record": record}, {"rviz_config": rviz_config}, {"sensor_model": sensor_model}, @@ -187,13 +220,13 @@ def description(): return parameters cpp_scenario_node = Node( - package=scenario_package, - executable=scenario, - name="scenario_node", - output="screen", - arguments=[("__log_level:=info")], - parameters=make_parameters() + [{"use_sim_time": use_sim_time}], - ) + package=scenario_package, + executable=scenario, + name="scenario_node", + output="screen", + arguments=[("__log_level:=info")], + parameters=make_parameters() + [{"use_sim_time": use_sim_time}], + ) io_handler = OnProcessIO( target_action=cpp_scenario_node, on_stderr=on_stderr_output, @@ -241,6 +274,7 @@ def description(): namespace="simulation", name="visualizer", output="screen", + remappings=[("/simulation/entity/status", "/entity/status")], ), Node( package="rviz2", @@ -249,10 +283,12 @@ def description(): output={"stderr": "log", "stdout": "log"}, condition=IfCondition(launch_rviz), arguments=["-d", str(default_rviz_config_file())], + remappings=[("/simulation/lanelet/marker", "/lanelet/marker")], ), RegisterEventHandler(event_handler=io_handler), RegisterEventHandler(event_handler=shutdown_handler), ] + def generate_launch_description(): return LaunchDescription([OpaqueFunction(function=launch_setup)]) From 91b19d35e3ac19763738911807104f1a37554f3a Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Mon, 11 Nov 2024 12:43:55 +0900 Subject: [PATCH 15/34] remap debug marker Signed-off-by: Masaya Kataoka --- mock/cpp_mock_scenarios/launch/mock_test.launch.py | 5 ++++- mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/mock/cpp_mock_scenarios/launch/mock_test.launch.py b/mock/cpp_mock_scenarios/launch/mock_test.launch.py index 0aa7d1ef1a7..b618c0e56ca 100644 --- a/mock/cpp_mock_scenarios/launch/mock_test.launch.py +++ b/mock/cpp_mock_scenarios/launch/mock_test.launch.py @@ -283,7 +283,10 @@ def description(): output={"stderr": "log", "stdout": "log"}, condition=IfCondition(launch_rviz), arguments=["-d", str(default_rviz_config_file())], - remappings=[("/simulation/lanelet/marker", "/lanelet/marker")], + remappings=[ + ("/simulation/lanelet/marker", "/lanelet/marker"), + ("/simulation/debug_marker", "/debug_marker"), + ], ), RegisterEventHandler(event_handler=io_handler), RegisterEventHandler(event_handler=shutdown_handler), diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index 74dbf25598b..c0d70ef03e7 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -30,7 +30,7 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode explicit StopAtCrosswalkScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "stop_at_crosswalk", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option) + "lanelet2_map.osm", __FILE__, false, option, true) { start(); } From ba75cc2570933a4d36803d8b66e519b19a930617 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Tue, 12 Nov 2024 15:16:01 +0900 Subject: [PATCH 16/34] use uint8_t instead of traffic_simulator_msgs::msg::EntityType Signed-off-by: Masaya Kataoka --- .../cpp_mock_scenarios/cpp_scenario_node.hpp | 6 ++---- mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp | 3 +-- .../src/traffic_sink/auto_sink_vehicle.cpp | 4 +--- .../include/simulation_interface/conversions.hpp | 15 ++++++--------- .../traffic_simulator/api/configuration.hpp | 2 +- .../behavior/behavior_plugin_base.hpp | 14 +++++++------- .../traffic_simulator/data_type/entity_status.hpp | 8 -------- .../traffic/traffic_controller.hpp | 4 ++-- .../traffic_simulator/traffic/traffic_sink.hpp | 4 ++-- .../src/data_type/entity_status.cpp | 1 + .../src/traffic/traffic_controller.cpp | 2 +- .../src/traffic/traffic_sink.cpp | 4 ++-- 12 files changed, 26 insertions(+), 41 deletions(-) diff --git a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp index 92246fd7ae8..680e4e128ef 100644 --- a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp +++ b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp @@ -34,8 +34,7 @@ class CppScenarioNode : public rclcpp::Node const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, const rclcpp::NodeOptions & option, const bool auto_sink = false, - const std::set - sinkable_entity_type = {}); + const std::set sinkable_entity_type = {}); void start(); void stop(Result result, const std::string & description = ""); void expectThrow() { exception_expect_ = true; } @@ -74,8 +73,7 @@ class CppScenarioNode : public rclcpp::Node auto configure( const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, const bool auto_sink = false, - const std::set - sinkable_entity_type = {}) -> traffic_simulator::Configuration + const std::set sinkable_entity_type = {}) -> traffic_simulator::Configuration { auto configuration = traffic_simulator::Configuration(map_path); { diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index bd2d4ba1b2a..d49605402be 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -21,8 +21,7 @@ CppScenarioNode::CppScenarioNode( const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, const rclcpp::NodeOptions & option, const bool auto_sink, - const std::set - sinkable_entity_type) + const std::set sinkable_entity_type) : Node(node_name, option), api_( this, diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index 4f4a55a7a72..479860c3382 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -30,9 +30,7 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode explicit AutoSinkVehicleScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "auto_sink_vehicle", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option, true, - {traffic_simulator_msgs::build().type( - traffic_simulator::EntityType::VEHICLE)}) + "lanelet2_map.osm", __FILE__, false, option, true, {traffic_simulator::EntityType::VEHICLE}) { start(); } diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 4ec8320616b..aca2245de55 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -168,9 +168,9 @@ void toMsg(const rosgraph_msgs::Clock & proto, rosgraph_msgs::msg::Clock & time) void toProto(const std_msgs::msg::Header & header, std_msgs::Header & proto); void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header); -#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ - auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \ - auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void +#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ + auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &) -> void; \ + auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &) -> void DEFINE_CONVERSION(autoware_auto_control_msgs, AckermannLateralCommand); DEFINE_CONVERSION(autoware_auto_control_msgs, LongitudinalCommand); @@ -192,8 +192,7 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr - { + auto convert_color = [](auto color) constexpr { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -208,8 +207,7 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr - { + auto convert_shape = [](auto shape) constexpr { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -237,8 +235,7 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr - { + auto convert_status = [](auto status) constexpr { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index 3cfca6510ab..c2b17c99d5e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -35,7 +35,7 @@ struct Configuration bool auto_sink = true; - std::set sinkable_entity_type = {}; + std::set sinkable_entity_type = {}; bool verbose = false; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index f4162f3e9b0..c4dac740c60 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -44,13 +44,13 @@ class BehaviorPluginBase virtual auto update(const double current_time, const double step_time) -> void = 0; virtual const std::string & getCurrentAction() const = 0; -#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ - virtual TYPE get##NAME() = 0; \ - virtual void set##NAME(const TYPE & value) = 0; \ - auto get##NAME##Key() const->const std::string & \ - { \ - static const std::string key = KEY; \ - return key; \ +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + virtual TYPE get##NAME() = 0; \ + virtual void set##NAME(const TYPE & value) = 0; \ + auto get##NAME##Key() const -> const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ } // clang-format off diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp index 94d13834eba..68a781808a3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp @@ -26,14 +26,6 @@ using EntityStatus = traffic_simulator_msgs::msg::EntityStatus; using EntityType = traffic_simulator_msgs::msg::EntityType; using EntitySubtype = traffic_simulator_msgs::msg::EntitySubtype; -struct EntityTypeComparator -{ - bool operator()(const EntityType & lhs, const EntityType & rhs) const - { - return lhs.type < rhs.type; - } -}; - inline namespace entity_status { class CanonicalizedEntityStatus diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 0b38cb03352..1dac416f036 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -46,7 +46,7 @@ class TrafficController std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink = false); @@ -65,7 +65,7 @@ class TrafficController std::vector> modules_; const std::function(void)> get_entity_names; const std::function get_entity_type; - const std::set sinkable_entity_type; + const std::set sinkable_entity_type; const std::function get_entity_pose; const std::function despawn; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 752f9288a1a..81fbc8f5ad8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -58,7 +58,7 @@ class TrafficSink : public TrafficModuleBase const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn); const lanelet::Id lanelet_id; @@ -71,7 +71,7 @@ class TrafficSink : public TrafficModuleBase private: const std::function(void)> get_entity_names; const std::function get_entity_type; - const std::set sinkable_entity_type; + const std::set sinkable_entity_type; const std::function get_entity_pose; const std::function despawn; }; diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index b2bfb78432e..6420e77aec3 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -17,6 +17,7 @@ namespace traffic_simulator { + inline namespace entity_status { diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 67efe0189cf..1510323a4f8 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -41,7 +41,7 @@ TrafficController::TrafficController( std::shared_ptr hdmap_utils, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn, bool auto_sink) : hdmap_utils_(hdmap_utils), diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index d5a38c38bd6..9636a4d54d3 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -42,7 +42,7 @@ TrafficSink::TrafficSink( const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names, const std::function & get_entity_type, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_type, const std::function & get_entity_pose, const std::function & despawn) : TrafficModuleBase(), @@ -64,7 +64,7 @@ void TrafficSink::execute( for (const auto & name : names) { const auto is_sinkable_entity = [this](const auto & entity_name) { return sinkable_entity_type.empty() ? true : [this](const auto & entity_name) { - return sinkable_entity_type.find(get_entity_type(entity_name)) != + return sinkable_entity_type.find(get_entity_type(entity_name).type) != sinkable_entity_type.end(); }(entity_name); }; From 83409529a20d62f223b761e9cfde956fbe07044c Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Tue, 12 Nov 2024 21:40:16 +0900 Subject: [PATCH 17/34] apply reformat Signed-off-by: Masaya Kataoka --- .../include/simulation_interface/conversions.hpp | 15 +++++++++------ .../behavior/behavior_plugin_base.hpp | 14 +++++++------- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index aca2245de55..4ec8320616b 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -168,9 +168,9 @@ void toMsg(const rosgraph_msgs::Clock & proto, rosgraph_msgs::msg::Clock & time) void toProto(const std_msgs::msg::Header & header, std_msgs::Header & proto); void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header); -#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ - auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &) -> void; \ - auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &) -> void +#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ + auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \ + auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void DEFINE_CONVERSION(autoware_auto_control_msgs, AckermannLateralCommand); DEFINE_CONVERSION(autoware_auto_control_msgs, LongitudinalCommand); @@ -192,7 +192,8 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr { + auto convert_color = [](auto color) constexpr + { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -207,7 +208,8 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr { + auto convert_shape = [](auto shape) constexpr + { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -235,7 +237,8 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr { + auto convert_status = [](auto status) constexpr + { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index c4dac740c60..f4162f3e9b0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -44,13 +44,13 @@ class BehaviorPluginBase virtual auto update(const double current_time, const double step_time) -> void = 0; virtual const std::string & getCurrentAction() const = 0; -#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ - virtual TYPE get##NAME() = 0; \ - virtual void set##NAME(const TYPE & value) = 0; \ - auto get##NAME##Key() const -> const std::string & \ - { \ - static const std::string key = KEY; \ - return key; \ +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + virtual TYPE get##NAME() = 0; \ + virtual void set##NAME(const TYPE & value) = 0; \ + auto get##NAME##Key() const->const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ } // clang-format off From 6d3b4bf18ca8ed54afbaf497e46b66af3f9b766a Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 13 Nov 2024 10:17:30 +0900 Subject: [PATCH 18/34] remove sonarcloud warning Signed-off-by: Masaya Kataoka --- simulation/traffic_simulator/src/traffic/traffic_sink.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 9636a4d54d3..d9f46d2097a 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -63,9 +63,8 @@ void TrafficSink::execute( const auto names = get_entity_names(); for (const auto & name : names) { const auto is_sinkable_entity = [this](const auto & entity_name) { - return sinkable_entity_type.empty() ? true : [this](const auto & entity_name) { - return sinkable_entity_type.find(get_entity_type(entity_name).type) != - sinkable_entity_type.end(); + return sinkable_entity_type.empty() ? true : [this](const auto & name) { + return sinkable_entity_type.find(get_entity_type(name).type) != sinkable_entity_type.end(); }(entity_name); }; const auto pose = get_entity_pose(name); From 56f31ccac4e5d54050dc0d2585696e304666d494 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 13 Nov 2024 12:50:28 +0900 Subject: [PATCH 19/34] remove warning Signed-off-by: Masaya Kataoka --- simulation/traffic_simulator/src/traffic/traffic_sink.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index d9f46d2097a..82096d5bb02 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -63,9 +63,10 @@ void TrafficSink::execute( const auto names = get_entity_names(); for (const auto & name : names) { const auto is_sinkable_entity = [this](const auto & entity_name) { - return sinkable_entity_type.empty() ? true : [this](const auto & name) { - return sinkable_entity_type.find(get_entity_type(name).type) != sinkable_entity_type.end(); - }(entity_name); + return sinkable_entity_type.empty() + ? true + : sinkable_entity_type.find(get_entity_type(entity_name).type) != + sinkable_entity_type.end(); }; const auto pose = get_entity_pose(name); if (is_sinkable_entity(name) and math::geometry::getDistance(position, pose) <= radius) { From 66ea1250eb711798a8d13c94f0167ff888344a16 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Thu, 14 Nov 2024 13:46:42 +0900 Subject: [PATCH 20/34] fix check condition Signed-off-by: Masaya Kataoka --- mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index 479860c3382..fe1b2110b33 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -39,7 +39,7 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { if (api_.getCurrentTime() >= 0.1) { - if (api_.entityExists("ego") && !api_.entityExists("ego")) { + if (api_.entityExists("bob") && !api_.entityExists("ego")) { stop(cpp_mock_scenarios::Result::FAILURE); } else { stop(cpp_mock_scenarios::Result::SUCCESS); From 2095cba08bdb189327c736c69127991c7b8709b1 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 28 Nov 2024 17:27:52 +0100 Subject: [PATCH 21/34] TrafficSink refactor with despawn functionality --- .../src/traffic_sink/auto_sink_vehicle.cpp | 9 +-- .../include/traffic_simulator/api/api.hpp | 19 +----- .../entity/entity_manager.hpp | 1 - .../traffic/traffic_controller.hpp | 16 ++--- .../traffic/traffic_sink.hpp | 35 ++++++----- .../src/traffic/traffic_controller.cpp | 27 ++++----- .../src/traffic/traffic_sink.cpp | 58 ++++++++++++++----- 7 files changed, 88 insertions(+), 77 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index fe1b2110b33..ccdcdef41ff 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -30,7 +30,8 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode explicit AutoSinkVehicleScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "auto_sink_vehicle", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option, true, {traffic_simulator::EntityType::VEHICLE}) + "lanelet2_map.osm", __FILE__, false, option, true, + {traffic_simulator::EntityType::PEDESTRIAN}) { start(); } @@ -39,10 +40,10 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { if (api_.getCurrentTime() >= 0.1) { - if (api_.entityExists("bob") && !api_.entityExists("ego")) { - stop(cpp_mock_scenarios::Result::FAILURE); - } else { + if (api_.entityExists("ego") and not api_.entityExists("bob")) { stop(cpp_mock_scenarios::Result::SUCCESS); + } else { + stop(cpp_mock_scenarios::Result::FAILURE); } } } diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index a30855dc2cc..604e1d648be 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -75,23 +76,7 @@ class API node, entity_manager_ptr_->getHdmapUtils(), getROS2Parameter("architecture_type", "awf/universe"))), traffic_controller_ptr_(std::make_shared( - entity_manager_ptr_->getHdmapUtils(), [this]() { return API::getEntityNames(); }, - [this](const auto & entity_name) { - if (const auto entity = getEntity(entity_name)) { - return entity->getEntityType(); - } else { - THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); - } - }, - configuration.sinkable_entity_type, - [this](const auto & entity_name) { - if (const auto entity = getEntity(entity_name)) { - return entity->getMapPose(); - } else { - THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); - } - }, - [this](const auto & name) { return API::despawn(name); }, configuration.auto_sink)), + entity_manager_ptr_, configuration.sinkable_entity_type, configuration.auto_sink)), clock_pub_(rclcpp::create_publisher( node, "/clock", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), rclcpp::PublisherOptionsWithAllocator())), diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 89c2bbec9d3..75af7e4972a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -37,7 +37,6 @@ #include #include #include -#include #include #include #include diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 1dac416f036..50d0414ad3d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -29,9 +29,9 @@ #include #include #include +#include #include #include -#include #include #include @@ -43,12 +43,8 @@ class TrafficController { public: explicit TrafficController( - std::shared_ptr hdmap_utils, - const std::function(void)> & get_entity_names, - const std::function & get_entity_type, - const std::set & sinkable_entity_type, - const std::function & get_entity_pose, - const std::function & despawn, bool auto_sink = false); + const std::shared_ptr entity_manager_ptr, + const std::set & sinkable_entity_type, bool auto_sink = false); template void addModule(Ts &&... xs) @@ -61,13 +57,9 @@ class TrafficController private: void autoSink(); - const std::shared_ptr hdmap_utils_; + const std::shared_ptr entity_manager_ptr; std::vector> modules_; - const std::function(void)> get_entity_names; - const std::function get_entity_type; const std::set sinkable_entity_type; - const std::function get_entity_pose; - const std::function despawn; public: const bool auto_sink; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 81fbc8f5ad8..3b3ed35348d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -32,6 +32,7 @@ #include #include #include +#include #include #include @@ -50,30 +51,38 @@ class TrafficSink : public TrafficModuleBase * @param position Position of the traffic sink. * @param get_entity_names Function to get the name of entity * @param get_entity_type Function to get the type of entity - * @param sinkable_entity_type If this type is applicable, the entity despawn only when it approaches radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. + * @param sinkable_entity_type If this type is applicable, the entity despawn only when it approaches + * radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. * @param get_entity_pose Function to get the pose of entity. * @param despawn Function to despawn entity. */ explicit TrafficSink( + const std::shared_ptr entity_manager_ptr, const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, - const std::function(void)> & get_entity_names, - const std::function & get_entity_type, - const std::set & sinkable_entity_type, - const std::function & get_entity_pose, - const std::function & despawn); - const lanelet::Id lanelet_id; - const double radius; - const geometry_msgs::msg::Point position; + const std::set & sinkable_entity_type); void execute(const double current_time, const double step_time) override; auto appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void override; + const lanelet::Id lanelet_id; + const double radius; + const geometry_msgs::msg::Point position; + private: - const std::function(void)> get_entity_names; - const std::function get_entity_type; + auto getEntityNames() const -> std::vector; + auto getEntityType(const std::string & entity_name) const noexcept(false) + -> traffic_simulator::EntityType; + auto getEntityPose(const std::string & entity_name) const noexcept(false) + -> geometry_msgs::msg::Pose; + /** + * @note Despawns the entity only when both: + * 1. Its distance from the TrafficSink is <= radius [m]. + * 2. Its EntityType is in sinkable_entity_type or sinkable_entity_type is empty. + */ + auto despawn(const std::string & entity_name) const -> void; + + const std::shared_ptr entity_manager_ptr; const std::set sinkable_entity_type; - const std::function get_entity_pose; - const std::function despawn; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 1510323a4f8..f2f220b848c 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -38,18 +39,10 @@ namespace traffic_simulator namespace traffic { TrafficController::TrafficController( - std::shared_ptr hdmap_utils, - const std::function(void)> & get_entity_names, - const std::function & get_entity_type, - const std::set & sinkable_entity_type, - const std::function & get_entity_pose, - const std::function & despawn, bool auto_sink) -: hdmap_utils_(hdmap_utils), - get_entity_names(get_entity_names), - get_entity_type(get_entity_type), + const std::shared_ptr entity_manager_ptr, + const std::set & sinkable_entity_type, bool auto_sink) +: entity_manager_ptr(entity_manager_ptr), sinkable_entity_type(sinkable_entity_type), - get_entity_pose(get_entity_pose), - despawn(despawn), auto_sink(auto_sink) { if (auto_sink) { @@ -59,15 +52,15 @@ TrafficController::TrafficController( void TrafficController::autoSink() { - for (const auto & lanelet_id : hdmap_utils_->getLaneletIds()) { - if (hdmap_utils_->getNextLaneletIds(lanelet_id).empty()) { + const auto hdmap_utils_ptr = entity_manager_ptr->getHdmapUtils(); + for (const auto & lanelet_id : hdmap_utils_ptr->getLaneletIds()) { + if (hdmap_utils_ptr->getNextLaneletIds(lanelet_id).empty()) { LaneletPose lanelet_pose; lanelet_pose.lanelet_id = lanelet_id; - lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_); - const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_); + lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_ptr); + const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_ptr); addModule( - lanelet_id, 1, pose.position, get_entity_names, get_entity_type, sinkable_entity_type, - get_entity_pose, despawn); + entity_manager_ptr, lanelet_id, 1, pose.position, sinkable_entity_type); } } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 82096d5bb02..5c1809cf03b 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -39,36 +40,30 @@ namespace traffic_simulator namespace traffic { TrafficSink::TrafficSink( + const std::shared_ptr entity_manager_ptr, const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, - const std::function(void)> & get_entity_names, - const std::function & get_entity_type, - const std::set & sinkable_entity_type, - const std::function & get_entity_pose, - const std::function & despawn) + const std::set & sinkable_entity_type) : TrafficModuleBase(), + entity_manager_ptr(entity_manager_ptr), lanelet_id(lanelet_id), radius(radius), position(position), - get_entity_names(get_entity_names), - get_entity_type(get_entity_type), - sinkable_entity_type(sinkable_entity_type), - get_entity_pose(get_entity_pose), - despawn(despawn) + sinkable_entity_type(sinkable_entity_type) { } void TrafficSink::execute( [[maybe_unused]] const double current_time, [[maybe_unused]] const double step_time) { - const auto names = get_entity_names(); + const auto names = getEntityNames(); for (const auto & name : names) { const auto is_sinkable_entity = [this](const auto & entity_name) { return sinkable_entity_type.empty() ? true - : sinkable_entity_type.find(get_entity_type(entity_name).type) != + : sinkable_entity_type.find(getEntityType(entity_name).type) != sinkable_entity_type.end(); }; - const auto pose = get_entity_pose(name); + const auto pose = getEntityPose(name); if (is_sinkable_entity(name) and math::geometry::getDistance(position, pose) <= radius) { despawn(name); } @@ -103,5 +98,42 @@ auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marke text_marker.scale.z = 0.6; marker_array.markers.emplace_back(text_marker); } + +auto TrafficSink::getEntityNames() const -> std::vector +{ + return entity_manager_ptr->getEntityNames(); +} + +auto TrafficSink::getEntityType(const std::string & entity_name) const noexcept(false) + -> traffic_simulator::EntityType +{ + if (const auto entity = entity_manager_ptr->getEntity(entity_name)) { + return entity->getEntityType(); + } else { + THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); + } +} +auto TrafficSink::getEntityPose(const std::string & entity_name) const noexcept(false) + -> geometry_msgs::msg::Pose +{ + if (const auto entity = entity_manager_ptr->getEntity(entity_name)) { + return entity->getMapPose(); + } else { + THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); + } +} +auto TrafficSink::despawn(const std::string & entity_name) const -> void +{ + const auto entity_position = getEntityPose(entity_name).position; + const bool in_despawn_proximity = math::geometry::hypot(entity_position, position) <= radius; + + const std::uint8_t entity_type = getEntityType(entity_name).type; + const bool is_despawn_candidate = + sinkable_entity_type.empty() or + sinkable_entity_type.find(entity_type) != sinkable_entity_type.cend(); + if (is_despawn_candidate and in_despawn_proximity) { + entity_manager_ptr->despawnEntity(entity_name); + } +} } // namespace traffic } // namespace traffic_simulator From ebec87979308c977c4286e4504281193d1a7c78d Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 28 Nov 2024 17:36:55 +0100 Subject: [PATCH 22/34] spell-check happy --- .../include/traffic_simulator/traffic/traffic_sink.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 3b3ed35348d..27e4a7fa24d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -75,7 +75,7 @@ class TrafficSink : public TrafficModuleBase auto getEntityPose(const std::string & entity_name) const noexcept(false) -> geometry_msgs::msg::Pose; /** - * @note Despawns the entity only when both: + * @note Despawn the entity only when both: * 1. Its distance from the TrafficSink is <= radius [m]. * 2. Its EntityType is in sinkable_entity_type or sinkable_entity_type is empty. */ From 2f1da242bc5232201d8430d3d1891f73caf51409 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 3 Dec 2024 16:43:12 +0100 Subject: [PATCH 23/34] reorder fix --- simulation/traffic_simulator/src/traffic/traffic_sink.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 5c1809cf03b..dbf60347d63 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -44,10 +44,10 @@ TrafficSink::TrafficSink( const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, const std::set & sinkable_entity_type) : TrafficModuleBase(), - entity_manager_ptr(entity_manager_ptr), lanelet_id(lanelet_id), radius(radius), position(position), + entity_manager_ptr(entity_manager_ptr), sinkable_entity_type(sinkable_entity_type) { } From 8a5d5577e674cee5b4df7c685984d97343855ab4 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 9 Dec 2024 13:46:48 +0100 Subject: [PATCH 24/34] remove lanelet_id from the constructor --- .../traffic_simulator/traffic/traffic_sink.hpp | 12 ++++-------- .../src/traffic/traffic_controller.cpp | 2 +- .../traffic_simulator/src/traffic/traffic_sink.cpp | 6 +++--- 3 files changed, 8 insertions(+), 12 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 27e4a7fa24d..a6b53a05b8f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -45,26 +45,21 @@ class TrafficSink : public TrafficModuleBase public: /** * @brief Construct a new Traffic Sink object - * @param lanelet_id Lanelet ID for visualization - * @todo lanelet_id value is only used for visualization and its very confusing. So it should be refactor. + * @param entity_manager_ptr Shared pointer, refers to the EntityManager * @param radius The entity despawn when the distance between the entity's coordinates in the Map coordinate system and the TrafficSink's coordinates is less than this value. * @param position Position of the traffic sink. - * @param get_entity_names Function to get the name of entity - * @param get_entity_type Function to get the type of entity * @param sinkable_entity_type If this type is applicable, the entity despawn only when it approaches * radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. - * @param get_entity_pose Function to get the pose of entity. - * @param despawn Function to despawn entity. */ explicit TrafficSink( const std::shared_ptr entity_manager_ptr, - const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, + const double radius, const geometry_msgs::msg::Point & position, const std::set & sinkable_entity_type); void execute(const double current_time, const double step_time) override; auto appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void override; - const lanelet::Id lanelet_id; + const std::size_t unique_id; const double radius; const geometry_msgs::msg::Point position; @@ -83,6 +78,7 @@ class TrafficSink : public TrafficModuleBase const std::shared_ptr entity_manager_ptr; const std::set sinkable_entity_type; + inline static std::size_t unique_id_counter = 0UL; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index f2f220b848c..4f3c452061b 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -60,7 +60,7 @@ void TrafficController::autoSink() lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_ptr); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_ptr); addModule( - entity_manager_ptr, lanelet_id, 1, pose.position, sinkable_entity_type); + entity_manager_ptr, 1, pose.position, sinkable_entity_type); } } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index dbf60347d63..c73cb2360b1 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -41,10 +41,10 @@ namespace traffic { TrafficSink::TrafficSink( const std::shared_ptr entity_manager_ptr, - const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, + const double radius, const geometry_msgs::msg::Point & position, const std::set & sinkable_entity_type) : TrafficModuleBase(), - lanelet_id(lanelet_id), + unique_id(unique_id_counter++), radius(radius), position(position), entity_manager_ptr(entity_manager_ptr), @@ -73,7 +73,7 @@ void TrafficSink::execute( auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void { - const auto lanelet_text = std::to_string(lanelet_id); + const auto lanelet_text = std::string("sink_") + std::to_string(unique_id); visualization_msgs::msg::Marker traffic_sink_marker; traffic_sink_marker.header.frame_id = "map"; traffic_sink_marker.ns = "traffic_controller/traffic_sink/" + lanelet_text; From 4aefb2acba9996c884b37d3f21cbb646e5c626a5 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 9 Dec 2024 14:43:28 +0100 Subject: [PATCH 25/34] review suggestions --- .../src/crosswalk/stop_at_crosswalk.cpp | 2 +- .../src/traffic_sink/auto_sink_vehicle.cpp | 13 +++++-------- .../traffic/traffic_controller.hpp | 6 +++--- .../traffic_simulator/traffic/traffic_sink.hpp | 14 ++++++-------- .../src/traffic/traffic_controller.cpp | 5 ++--- .../traffic_simulator/src/traffic/traffic_sink.cpp | 8 +++----- 6 files changed, 20 insertions(+), 28 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index c0d70ef03e7..74dbf25598b 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -30,7 +30,7 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode explicit StopAtCrosswalkScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "stop_at_crosswalk", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option, true) + "lanelet2_map.osm", __FILE__, false, option) { start(); } diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index ccdcdef41ff..59b2ad42b21 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -50,17 +50,14 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { + const auto canonicalized_lanelet_pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34774, 11.0, 0.0, api_.getHdmapUtils()); api_.spawn( - "ego", - traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34774, 11.0, 0.0, api_.getHdmapUtils())), + "ego", traffic_simulator::pose::toMapPose(canonicalized_lanelet_pose), getVehicleParameters()); api_.spawn( - "bob", - traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34774, 11.0, 0.0, api_.getHdmapUtils())), + "bob", traffic_simulator::pose::toMapPose(canonicalized_lanelet_pose), getPedestrianParameters()); } }; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 50d0414ad3d..43b11d15b12 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -43,7 +43,7 @@ class TrafficController { public: explicit TrafficController( - const std::shared_ptr entity_manager_ptr, + const std::shared_ptr entity_manager_ptr, const std::set & sinkable_entity_type, bool auto_sink = false); template @@ -57,8 +57,8 @@ class TrafficController private: void autoSink(); - const std::shared_ptr entity_manager_ptr; - std::vector> modules_; + const std::shared_ptr entity_manager_ptr; + std::vector> modules_; const std::set sinkable_entity_type; public: diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index a6b53a05b8f..7ac9c9062bb 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -46,14 +46,13 @@ class TrafficSink : public TrafficModuleBase /** * @brief Construct a new Traffic Sink object * @param entity_manager_ptr Shared pointer, refers to the EntityManager - * @param radius The entity despawn when the distance between the entity's coordinates in the Map coordinate system and the TrafficSink's coordinates is less than this value. + * @param radius Radius of the sink * @param position Position of the traffic sink. - * @param sinkable_entity_type If this type is applicable, the entity despawn only when it approaches - * radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. + * @param sinkable_entity_type Candidates for despawn. If empty, all entities are sinkable */ explicit TrafficSink( - const std::shared_ptr entity_manager_ptr, - const double radius, const geometry_msgs::msg::Point & position, + const std::shared_ptr entity_manager_ptr, const double radius, + const geometry_msgs::msg::Point & position, const std::set & sinkable_entity_type); void execute(const double current_time, const double step_time) override; auto appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const @@ -65,8 +64,7 @@ class TrafficSink : public TrafficModuleBase private: auto getEntityNames() const -> std::vector; - auto getEntityType(const std::string & entity_name) const noexcept(false) - -> traffic_simulator::EntityType; + auto getEntityType(const std::string & entity_name) const noexcept(false) -> EntityType; auto getEntityPose(const std::string & entity_name) const noexcept(false) -> geometry_msgs::msg::Pose; /** @@ -76,7 +74,7 @@ class TrafficSink : public TrafficModuleBase */ auto despawn(const std::string & entity_name) const -> void; - const std::shared_ptr entity_manager_ptr; + const std::shared_ptr entity_manager_ptr; const std::set sinkable_entity_type; inline static std::size_t unique_id_counter = 0UL; }; diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 4f3c452061b..3db9b7aabde 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -39,7 +39,7 @@ namespace traffic_simulator namespace traffic { TrafficController::TrafficController( - const std::shared_ptr entity_manager_ptr, + const std::shared_ptr entity_manager_ptr, const std::set & sinkable_entity_type, bool auto_sink) : entity_manager_ptr(entity_manager_ptr), sinkable_entity_type(sinkable_entity_type), @@ -59,8 +59,7 @@ void TrafficController::autoSink() lanelet_pose.lanelet_id = lanelet_id; lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_ptr); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_ptr); - addModule( - entity_manager_ptr, 1, pose.position, sinkable_entity_type); + addModule(entity_manager_ptr, 1, pose.position, sinkable_entity_type); } } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index c73cb2360b1..a3373b2363d 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -40,9 +40,8 @@ namespace traffic_simulator namespace traffic { TrafficSink::TrafficSink( - const std::shared_ptr entity_manager_ptr, - const double radius, const geometry_msgs::msg::Point & position, - const std::set & sinkable_entity_type) + const std::shared_ptr entity_manager_ptr, const double radius, + const geometry_msgs::msg::Point & position, const std::set & sinkable_entity_type) : TrafficModuleBase(), unique_id(unique_id_counter++), radius(radius), @@ -104,8 +103,7 @@ auto TrafficSink::getEntityNames() const -> std::vector return entity_manager_ptr->getEntityNames(); } -auto TrafficSink::getEntityType(const std::string & entity_name) const noexcept(false) - -> traffic_simulator::EntityType +auto TrafficSink::getEntityType(const std::string & entity_name) const noexcept(false) -> EntityType { if (const auto entity = entity_manager_ptr->getEntity(entity_name)) { return entity->getEntityType(); From 7be6fcee435a381d5aae18164af50bc483d23244 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 9 Dec 2024 16:35:44 +0100 Subject: [PATCH 26/34] AutoSinkConfig --- .../cpp_mock_scenarios/cpp_scenario_node.hpp | 13 +++++++------ .../src/cpp_scenario_node.cpp | 8 +++----- .../src/traffic_sink/auto_sink_vehicle.cpp | 7 ++++--- .../src/openscenario_interpreter.cpp | 2 +- .../include/traffic_simulator/api/api.hpp | 4 +++- .../traffic_simulator/api/configuration.hpp | 4 ++-- .../traffic/traffic_controller.hpp | 11 ++++++++--- .../traffic_simulator/traffic/traffic_sink.hpp | 8 ++++---- .../src/traffic/traffic_controller.cpp | 14 +++++++------- .../src/traffic/traffic_sink.cpp | 18 +++++++++++++----- 10 files changed, 52 insertions(+), 37 deletions(-) diff --git a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp index 680e4e128ef..70bea75da81 100644 --- a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp +++ b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp @@ -33,8 +33,8 @@ class CppScenarioNode : public rclcpp::Node explicit CppScenarioNode( const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, - const bool verbose, const rclcpp::NodeOptions & option, const bool auto_sink = false, - const std::set sinkable_entity_type = {}); + const bool verbose, const rclcpp::NodeOptions & option, + const traffic_simulator::traffic::AutoSinkConfig & auto_sink_config = {false, {}}); void start(); void stop(Result result, const std::string & description = ""); void expectThrow() { exception_expect_ = true; } @@ -72,8 +72,9 @@ class CppScenarioNode : public rclcpp::Node int timeout_; auto configure( const std::string & map_path, const std::string & lanelet2_map_file, - const std::string & scenario_filename, const bool verbose, const bool auto_sink = false, - const std::set sinkable_entity_type = {}) -> traffic_simulator::Configuration + const std::string & scenario_filename, const bool verbose, + const traffic_simulator::traffic::AutoSinkConfig & auto_sink_config = {false, {}}) + -> traffic_simulator::Configuration { auto configuration = traffic_simulator::Configuration(map_path); { @@ -81,8 +82,8 @@ class CppScenarioNode : public rclcpp::Node // configuration.lanelet2_map_file = "lanelet2_map_with_private_road_and_walkway_ele_fix.osm"; configuration.scenario_path = scenario_filename; configuration.verbose = verbose; - configuration.auto_sink = auto_sink; - configuration.sinkable_entity_type = sinkable_entity_type; + configuration.generate_auto_sink = auto_sink_config.generate_auto_sink; + configuration.default_sinkable_entity_type = auto_sink_config.default_sinkable_entity_type; } checkConfiguration(configuration); return configuration; diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index d49605402be..ecabc7967ed 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -20,13 +20,11 @@ namespace cpp_mock_scenarios CppScenarioNode::CppScenarioNode( const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, - const rclcpp::NodeOptions & option, const bool auto_sink, - const std::set sinkable_entity_type) + const rclcpp::NodeOptions & option, + const traffic_simulator::traffic::AutoSinkConfig & auto_sink_config /*= {false, {}}*/) : Node(node_name, option), api_( - this, - configure( - map_path, lanelet2_map_file, scenario_filename, verbose, auto_sink, sinkable_entity_type), + this, configure(map_path, lanelet2_map_file, scenario_filename, verbose, auto_sink_config), declare_parameter("global_real_time_factor", 1.0), declare_parameter("global_frame_rate", 20.0)), scenario_filename_(scenario_filename), diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index 59b2ad42b21..3e67930bfb4 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -30,8 +31,8 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode explicit AutoSinkVehicleScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "auto_sink_vehicle", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option, true, - {traffic_simulator::EntityType::PEDESTRIAN}) + "lanelet2_map.osm", __FILE__, false, option, + traffic_simulator::traffic::AutoSinkConfig{true, {traffic_simulator::EntityType::PEDESTRIAN}}) { start(); } @@ -39,7 +40,7 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - if (api_.getCurrentTime() >= 0.1) { + if (api_.getCurrentTime() >= 10.1) { if (api_.entityExists("ego") and not api_.entityExists("bob")) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index d2f7c910cc0..c1597f52251 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -70,7 +70,7 @@ auto Interpreter::makeCurrentConfiguration() const -> traffic_simulator::Configu auto configuration = traffic_simulator::Configuration( logic_file.isDirectory() ? logic_file : logic_file.filepath.parent_path()); { - configuration.auto_sink = false; + configuration.generate_auto_sink = false; configuration.scenario_path = osc_path; // XXX DIRTY HACK!!! diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 604e1d648be..300b9589e78 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -76,7 +76,9 @@ class API node, entity_manager_ptr_->getHdmapUtils(), getROS2Parameter("architecture_type", "awf/universe"))), traffic_controller_ptr_(std::make_shared( - entity_manager_ptr_, configuration.sinkable_entity_type, configuration.auto_sink)), + entity_manager_ptr_, + traffic_simulator::traffic::AutoSinkConfig{ + configuration.generate_auto_sink, configuration.default_sinkable_entity_type})), clock_pub_(rclcpp::create_publisher( node, "/clock", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), rclcpp::PublisherOptionsWithAllocator())), diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index c2b17c99d5e..07dbd4ff012 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -33,9 +33,9 @@ struct Configuration using Pathname = boost::filesystem::path; - bool auto_sink = true; + bool generate_auto_sink = true; - std::set sinkable_entity_type = {}; + std::unordered_set default_sinkable_entity_type = {}; bool verbose = false; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 43b11d15b12..9783161929f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -39,12 +39,18 @@ namespace traffic_simulator { namespace traffic { +struct AutoSinkConfig +{ + bool generate_auto_sink = true; + std::unordered_set default_sinkable_entity_type = {}; + static constexpr double radius = 1.0; +}; class TrafficController { public: explicit TrafficController( const std::shared_ptr entity_manager_ptr, - const std::set & sinkable_entity_type, bool auto_sink = false); + const AutoSinkConfig & auto_sink_config = {false, {}}); template void addModule(Ts &&... xs) @@ -59,10 +65,9 @@ class TrafficController void autoSink(); const std::shared_ptr entity_manager_ptr; std::vector> modules_; - const std::set sinkable_entity_type; public: - const bool auto_sink; + const AutoSinkConfig auto_sink_config; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 7ac9c9062bb..2d0acde43bd 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -53,12 +53,13 @@ class TrafficSink : public TrafficModuleBase explicit TrafficSink( const std::shared_ptr entity_manager_ptr, const double radius, const geometry_msgs::msg::Point & position, - const std::set & sinkable_entity_type); + const std::unordered_set & sinkable_entity_type, + const std::optional lanelet_id_opt = std::nullopt); void execute(const double current_time, const double step_time) override; auto appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void override; - const std::size_t unique_id; + const std::string description; const double radius; const geometry_msgs::msg::Point position; @@ -75,8 +76,7 @@ class TrafficSink : public TrafficModuleBase auto despawn(const std::string & entity_name) const -> void; const std::shared_ptr entity_manager_ptr; - const std::set sinkable_entity_type; - inline static std::size_t unique_id_counter = 0UL; + const std::unordered_set sinkable_entity_type; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 3db9b7aabde..a07e19305ec 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -40,12 +40,10 @@ namespace traffic { TrafficController::TrafficController( const std::shared_ptr entity_manager_ptr, - const std::set & sinkable_entity_type, bool auto_sink) -: entity_manager_ptr(entity_manager_ptr), - sinkable_entity_type(sinkable_entity_type), - auto_sink(auto_sink) + const AutoSinkConfig & auto_sink_config /* = {false, {}}*/) +: entity_manager_ptr(entity_manager_ptr), auto_sink_config(auto_sink_config) { - if (auto_sink) { + if (auto_sink_config.generate_auto_sink) { autoSink(); } } @@ -59,7 +57,9 @@ void TrafficController::autoSink() lanelet_pose.lanelet_id = lanelet_id; lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_ptr); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_ptr); - addModule(entity_manager_ptr, 1, pose.position, sinkable_entity_type); + addModule( + entity_manager_ptr, auto_sink_config.radius, pose.position, + auto_sink_config.default_sinkable_entity_type, std::make_optional(lanelet_id)); } } } @@ -75,7 +75,7 @@ auto TrafficController::makeDebugMarker() const -> const visualization_msgs::msg { static const auto marker_array = [&]() { visualization_msgs::msg::MarkerArray marker_array; - for (size_t i = 0; i < modules_.size(); ++i) { + for (size_t i = 0UL; i < modules_.size(); ++i) { modules_[i]->appendDebugMarker(marker_array); } return marker_array; diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index a3373b2363d..7c85b19b434 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -41,9 +41,18 @@ namespace traffic { TrafficSink::TrafficSink( const std::shared_ptr entity_manager_ptr, const double radius, - const geometry_msgs::msg::Point & position, const std::set & sinkable_entity_type) + const geometry_msgs::msg::Point & position, + const std::unordered_set & sinkable_entity_type, + const std::optional lanelet_id_opt /*= std::nullopt*/) : TrafficModuleBase(), - unique_id(unique_id_counter++), + description([](std::optional lanelet_id_opt) -> std::string { + static long unique_id = 0L; + if (lanelet_id_opt.has_value()) { + return std::string("auto_") + std::to_string(lanelet_id_opt.value()); + } else { + return std::string("custom_") + std::to_string(unique_id++); + } + }(lanelet_id_opt)), radius(radius), position(position), entity_manager_ptr(entity_manager_ptr), @@ -72,10 +81,9 @@ void TrafficSink::execute( auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void { - const auto lanelet_text = std::string("sink_") + std::to_string(unique_id); visualization_msgs::msg::Marker traffic_sink_marker; traffic_sink_marker.header.frame_id = "map"; - traffic_sink_marker.ns = "traffic_controller/traffic_sink/" + lanelet_text; + traffic_sink_marker.ns = "traffic_controller/traffic_sink/" + description; traffic_sink_marker.id = 0; traffic_sink_marker.action = traffic_sink_marker.ADD; traffic_sink_marker.type = 3; // cylinder @@ -92,7 +100,7 @@ auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marke text_marker = traffic_sink_marker; text_marker.id = 1; text_marker.type = 9; //text - text_marker.text = lanelet_text; + text_marker.text = description; text_marker.color = color_names::makeColorMsg("white", 0.99); text_marker.scale.z = 0.6; marker_array.markers.emplace_back(text_marker); From 69918d7652db2f704cd59285a0f4a0410dd7103e Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 9 Dec 2024 17:28:14 +0100 Subject: [PATCH 27/34] TrafficSinkConfig --- .../src/traffic_sink/auto_sink_vehicle.cpp | 2 +- .../traffic_simulator/api/configuration.hpp | 2 +- .../traffic/traffic_controller.hpp | 4 +- .../traffic/traffic_sink.hpp | 47 +++++++++++++---- .../src/traffic/traffic_controller.cpp | 9 ++-- .../src/traffic/traffic_sink.cpp | 50 +++++++------------ 6 files changed, 64 insertions(+), 50 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index 3e67930bfb4..ae39e75c7c3 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -40,7 +40,7 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - if (api_.getCurrentTime() >= 10.1) { + if (api_.getCurrentTime() >= 0.1) { if (api_.entityExists("ego") and not api_.entityExists("bob")) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index 07dbd4ff012..cfd33d0f306 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -35,7 +35,7 @@ struct Configuration bool generate_auto_sink = true; - std::unordered_set default_sinkable_entity_type = {}; + std::set default_sinkable_entity_type = {}; bool verbose = false; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 9783161929f..323073114bb 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -41,8 +41,8 @@ namespace traffic { struct AutoSinkConfig { - bool generate_auto_sink = true; - std::unordered_set default_sinkable_entity_type = {}; + const bool generate_auto_sink = true; + const std::set default_sinkable_entity_type = {}; static constexpr double radius = 1.0; }; class TrafficController diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 2d0acde43bd..3d80a8bc042 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -40,28 +40,54 @@ namespace traffic_simulator { namespace traffic { +struct TrafficSinkConfig +{ + /** + * @brief Construct a new TrafficSinkConfig object + * @param radius Radius of the traffic sink + * @param position Position of the traffic sink + * @param sinkable_entity_type Candidates for despawn. If empty, all entities are sinkable + */ + explicit TrafficSinkConfig( + const double radius, const geometry_msgs::msg::Point & position, + const std::set & sinkable_entity_type, + const std::optional lanelet_id_opt) + : radius(radius), + position(position), + sinkable_entity_type(sinkable_entity_type), + description([](std::optional lanelet_id_opt) -> std::string { + static long unique_id = 0L; + if (lanelet_id_opt.has_value()) { + return std::string("auto_") + std::to_string(lanelet_id_opt.value()); + } else { + return std::string("custom_") + std::to_string(unique_id++); + } + }(lanelet_id_opt)) + { + } + + const double radius; + const geometry_msgs::msg::Point position; + const std::set sinkable_entity_type; + const std::string description; +}; + class TrafficSink : public TrafficModuleBase { public: /** * @brief Construct a new Traffic Sink object * @param entity_manager_ptr Shared pointer, refers to the EntityManager - * @param radius Radius of the sink - * @param position Position of the traffic sink. - * @param sinkable_entity_type Candidates for despawn. If empty, all entities are sinkable + * @param config TrafficSink configuration */ explicit TrafficSink( - const std::shared_ptr entity_manager_ptr, const double radius, - const geometry_msgs::msg::Point & position, - const std::unordered_set & sinkable_entity_type, - const std::optional lanelet_id_opt = std::nullopt); + const std::shared_ptr entity_manager_ptr, + const TrafficSinkConfig & config); void execute(const double current_time, const double step_time) override; auto appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void override; - const std::string description; - const double radius; - const geometry_msgs::msg::Point position; + const TrafficSinkConfig config; private: auto getEntityNames() const -> std::vector; @@ -76,7 +102,6 @@ class TrafficSink : public TrafficModuleBase auto despawn(const std::string & entity_name) const -> void; const std::shared_ptr entity_manager_ptr; - const std::unordered_set sinkable_entity_type; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index a07e19305ec..28fa9b214f6 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -41,7 +41,7 @@ namespace traffic TrafficController::TrafficController( const std::shared_ptr entity_manager_ptr, const AutoSinkConfig & auto_sink_config /* = {false, {}}*/) -: entity_manager_ptr(entity_manager_ptr), auto_sink_config(auto_sink_config) +: entity_manager_ptr(entity_manager_ptr), modules_(), auto_sink_config(auto_sink_config) { if (auto_sink_config.generate_auto_sink) { autoSink(); @@ -57,9 +57,10 @@ void TrafficController::autoSink() lanelet_pose.lanelet_id = lanelet_id; lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_ptr); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_ptr); - addModule( - entity_manager_ptr, auto_sink_config.radius, pose.position, - auto_sink_config.default_sinkable_entity_type, std::make_optional(lanelet_id)); + const auto traffic_sink_config = TrafficSinkConfig( + auto_sink_config.radius, pose.position, auto_sink_config.default_sinkable_entity_type, + std::make_optional(lanelet_id)); + addModule(entity_manager_ptr, traffic_sink_config); } } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 7c85b19b434..c50d14680ee 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -40,23 +40,8 @@ namespace traffic_simulator namespace traffic { TrafficSink::TrafficSink( - const std::shared_ptr entity_manager_ptr, const double radius, - const geometry_msgs::msg::Point & position, - const std::unordered_set & sinkable_entity_type, - const std::optional lanelet_id_opt /*= std::nullopt*/) -: TrafficModuleBase(), - description([](std::optional lanelet_id_opt) -> std::string { - static long unique_id = 0L; - if (lanelet_id_opt.has_value()) { - return std::string("auto_") + std::to_string(lanelet_id_opt.value()); - } else { - return std::string("custom_") + std::to_string(unique_id++); - } - }(lanelet_id_opt)), - radius(radius), - position(position), - entity_manager_ptr(entity_manager_ptr), - sinkable_entity_type(sinkable_entity_type) + const std::shared_ptr entity_manager_ptr, const TrafficSinkConfig & config) +: TrafficModuleBase(), config(config), entity_manager_ptr(entity_manager_ptr) { } @@ -66,13 +51,14 @@ void TrafficSink::execute( const auto names = getEntityNames(); for (const auto & name : names) { const auto is_sinkable_entity = [this](const auto & entity_name) { - return sinkable_entity_type.empty() - ? true - : sinkable_entity_type.find(getEntityType(entity_name).type) != - sinkable_entity_type.end(); + return config.sinkable_entity_type.empty() or + config.sinkable_entity_type.find(getEntityType(entity_name).type) != + config.sinkable_entity_type.end(); }; const auto pose = getEntityPose(name); - if (is_sinkable_entity(name) and math::geometry::getDistance(position, pose) <= radius) { + if ( + is_sinkable_entity(name) and + math::geometry::getDistance(config.position, pose) <= config.radius) { despawn(name); } } @@ -83,16 +69,17 @@ auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marke { visualization_msgs::msg::Marker traffic_sink_marker; traffic_sink_marker.header.frame_id = "map"; - traffic_sink_marker.ns = "traffic_controller/traffic_sink/" + description; + traffic_sink_marker.ns = "traffic_controller/traffic_sink/" + config.description; traffic_sink_marker.id = 0; traffic_sink_marker.action = traffic_sink_marker.ADD; traffic_sink_marker.type = 3; // cylinder traffic_sink_marker.pose = - geometry_msgs::build().position(position).orientation( - geometry_msgs::build().x(0).y(0).z(0).w(1)); + geometry_msgs::build() + .position(config.position) + .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); traffic_sink_marker.color = color_names::makeColorMsg("firebrick", 0.99); - traffic_sink_marker.scale.x = radius * 2; - traffic_sink_marker.scale.y = radius * 2; + traffic_sink_marker.scale.x = config.radius * 2.0; + traffic_sink_marker.scale.y = config.radius * 2.0; traffic_sink_marker.scale.z = 1.0; marker_array.markers.emplace_back(traffic_sink_marker); @@ -100,7 +87,7 @@ auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marke text_marker = traffic_sink_marker; text_marker.id = 1; text_marker.type = 9; //text - text_marker.text = description; + text_marker.text = config.description; text_marker.color = color_names::makeColorMsg("white", 0.99); text_marker.scale.z = 0.6; marker_array.markers.emplace_back(text_marker); @@ -131,12 +118,13 @@ auto TrafficSink::getEntityPose(const std::string & entity_name) const noexcept( auto TrafficSink::despawn(const std::string & entity_name) const -> void { const auto entity_position = getEntityPose(entity_name).position; - const bool in_despawn_proximity = math::geometry::hypot(entity_position, position) <= radius; + const bool in_despawn_proximity = + math::geometry::hypot(entity_position, config.position) <= config.radius; const std::uint8_t entity_type = getEntityType(entity_name).type; const bool is_despawn_candidate = - sinkable_entity_type.empty() or - sinkable_entity_type.find(entity_type) != sinkable_entity_type.cend(); + config.sinkable_entity_type.empty() or + config.sinkable_entity_type.find(entity_type) != config.sinkable_entity_type.cend(); if (is_despawn_candidate and in_despawn_proximity) { entity_manager_ptr->despawnEntity(entity_name); } From 2d56a8b3a0f1136e0778095732f422057a574fc5 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 9 Dec 2024 18:54:12 +0100 Subject: [PATCH 28/34] simplify auto_sink logic --- .../cpp_mock_scenarios/cpp_scenario_node.hpp | 8 ++--- .../src/cpp_scenario_node.cpp | 5 +-- .../src/traffic_sink/auto_sink_vehicle.cpp | 7 ++-- .../src/openscenario_interpreter.cpp | 1 - .../include/traffic_simulator/api/api.hpp | 4 +-- .../traffic_simulator/api/configuration.hpp | 4 +-- .../traffic/traffic_controller.hpp | 13 ++----- .../traffic/traffic_sink.hpp | 21 ++++++----- .../src/traffic/traffic_controller.cpp | 14 ++++---- .../src/traffic/traffic_sink.cpp | 35 +++++-------------- 10 files changed, 39 insertions(+), 73 deletions(-) diff --git a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp index 70bea75da81..45c2aa6962e 100644 --- a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp +++ b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp @@ -34,7 +34,7 @@ class CppScenarioNode : public rclcpp::Node const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, const rclcpp::NodeOptions & option, - const traffic_simulator::traffic::AutoSinkConfig & auto_sink_config = {false, {}}); + const std::set & auto_sink_entity_types = {}); void start(); void stop(Result result, const std::string & description = ""); void expectThrow() { exception_expect_ = true; } @@ -73,8 +73,7 @@ class CppScenarioNode : public rclcpp::Node auto configure( const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, - const traffic_simulator::traffic::AutoSinkConfig & auto_sink_config = {false, {}}) - -> traffic_simulator::Configuration + const std::set & auto_sink_entity_types = {}) -> traffic_simulator::Configuration { auto configuration = traffic_simulator::Configuration(map_path); { @@ -82,8 +81,7 @@ class CppScenarioNode : public rclcpp::Node // configuration.lanelet2_map_file = "lanelet2_map_with_private_road_and_walkway_ele_fix.osm"; configuration.scenario_path = scenario_filename; configuration.verbose = verbose; - configuration.generate_auto_sink = auto_sink_config.generate_auto_sink; - configuration.default_sinkable_entity_type = auto_sink_config.default_sinkable_entity_type; + configuration.auto_sink_entity_types = auto_sink_entity_types; } checkConfiguration(configuration); return configuration; diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index ecabc7967ed..362203733c9 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -21,10 +21,11 @@ CppScenarioNode::CppScenarioNode( const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, const rclcpp::NodeOptions & option, - const traffic_simulator::traffic::AutoSinkConfig & auto_sink_config /*= {false, {}}*/) + const std::set & auto_sink_entity_types /*= {}*/) : Node(node_name, option), api_( - this, configure(map_path, lanelet2_map_file, scenario_filename, verbose, auto_sink_config), + this, + configure(map_path, lanelet2_map_file, scenario_filename, verbose, auto_sink_entity_types), declare_parameter("global_real_time_factor", 1.0), declare_parameter("global_frame_rate", 20.0)), scenario_filename_(scenario_filename), diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index ae39e75c7c3..951684a587d 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -31,8 +31,7 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode explicit AutoSinkVehicleScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "auto_sink_vehicle", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option, - traffic_simulator::traffic::AutoSinkConfig{true, {traffic_simulator::EntityType::PEDESTRIAN}}) + "lanelet2_map.osm", __FILE__, false, option, {traffic_simulator::EntityType::PEDESTRIAN}) { start(); } @@ -41,7 +40,7 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { if (api_.getCurrentTime() >= 0.1) { - if (api_.entityExists("ego") and not api_.entityExists("bob")) { + if (api_.entityExists("car") and not api_.entityExists("bob")) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); @@ -55,7 +54,7 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34774, 11.0, 0.0, api_.getHdmapUtils()); api_.spawn( - "ego", traffic_simulator::pose::toMapPose(canonicalized_lanelet_pose), + "car", traffic_simulator::pose::toMapPose(canonicalized_lanelet_pose), getVehicleParameters()); api_.spawn( "bob", traffic_simulator::pose::toMapPose(canonicalized_lanelet_pose), diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index c1597f52251..c82d04b4601 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -70,7 +70,6 @@ auto Interpreter::makeCurrentConfiguration() const -> traffic_simulator::Configu auto configuration = traffic_simulator::Configuration( logic_file.isDirectory() ? logic_file : logic_file.filepath.parent_path()); { - configuration.generate_auto_sink = false; configuration.scenario_path = osc_path; // XXX DIRTY HACK!!! diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 300b9589e78..7001b325a8e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -76,9 +76,7 @@ class API node, entity_manager_ptr_->getHdmapUtils(), getROS2Parameter("architecture_type", "awf/universe"))), traffic_controller_ptr_(std::make_shared( - entity_manager_ptr_, - traffic_simulator::traffic::AutoSinkConfig{ - configuration.generate_auto_sink, configuration.default_sinkable_entity_type})), + entity_manager_ptr_, configuration.auto_sink_entity_types)), clock_pub_(rclcpp::create_publisher( node, "/clock", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), rclcpp::PublisherOptionsWithAllocator())), diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index cfd33d0f306..ea84ee189f8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -33,9 +33,7 @@ struct Configuration using Pathname = boost::filesystem::path; - bool generate_auto_sink = true; - - std::set default_sinkable_entity_type = {}; + std::set auto_sink_entity_types = {}; bool verbose = false; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 323073114bb..02de243fdb8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -39,18 +39,12 @@ namespace traffic_simulator { namespace traffic { -struct AutoSinkConfig -{ - const bool generate_auto_sink = true; - const std::set default_sinkable_entity_type = {}; - static constexpr double radius = 1.0; -}; class TrafficController { public: explicit TrafficController( const std::shared_ptr entity_manager_ptr, - const AutoSinkConfig & auto_sink_config = {false, {}}); + const std::set auto_sink_entity_types /*= {}*/); template void addModule(Ts &&... xs) @@ -62,12 +56,9 @@ class TrafficController auto makeDebugMarker() const -> const visualization_msgs::msg::MarkerArray; private: - void autoSink(); + void generateAutoSinks(const std::set & auto_sink_entity_types); const std::shared_ptr entity_manager_ptr; std::vector> modules_; - -public: - const AutoSinkConfig auto_sink_config; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 3d80a8bc042..5135b19497a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -46,16 +46,16 @@ struct TrafficSinkConfig * @brief Construct a new TrafficSinkConfig object * @param radius Radius of the traffic sink * @param position Position of the traffic sink - * @param sinkable_entity_type Candidates for despawn. If empty, all entities are sinkable + * @param sinkable_entity_types Candidates for despawn. */ explicit TrafficSinkConfig( const double radius, const geometry_msgs::msg::Point & position, - const std::set & sinkable_entity_type, + const std::set & sinkable_entity_types, const std::optional lanelet_id_opt) : radius(radius), position(position), - sinkable_entity_type(sinkable_entity_type), - description([](std::optional lanelet_id_opt) -> std::string { + sinkable_entity_types(sinkable_entity_types), + description([](const std::optional lanelet_id_opt) -> std::string { static long unique_id = 0L; if (lanelet_id_opt.has_value()) { return std::string("auto_") + std::to_string(lanelet_id_opt.value()); @@ -68,7 +68,7 @@ struct TrafficSinkConfig const double radius; const geometry_msgs::msg::Point position; - const std::set sinkable_entity_type; + const std::set sinkable_entity_types; const std::string description; }; @@ -83,6 +83,11 @@ class TrafficSink : public TrafficModuleBase explicit TrafficSink( const std::shared_ptr entity_manager_ptr, const TrafficSinkConfig & config); + /** + * @note execute calls despawn on each entity only when both: + * 1. Its distance from the TrafficSink is <= config.radius [m]. + * 2. Its EntityType is in config.sinkable_entity_types. + */ void execute(const double current_time, const double step_time) override; auto appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void override; @@ -94,12 +99,6 @@ class TrafficSink : public TrafficModuleBase auto getEntityType(const std::string & entity_name) const noexcept(false) -> EntityType; auto getEntityPose(const std::string & entity_name) const noexcept(false) -> geometry_msgs::msg::Pose; - /** - * @note Despawn the entity only when both: - * 1. Its distance from the TrafficSink is <= radius [m]. - * 2. Its EntityType is in sinkable_entity_type or sinkable_entity_type is empty. - */ - auto despawn(const std::string & entity_name) const -> void; const std::shared_ptr entity_manager_ptr; }; diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 28fa9b214f6..f069f4bd9a5 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -40,15 +40,15 @@ namespace traffic { TrafficController::TrafficController( const std::shared_ptr entity_manager_ptr, - const AutoSinkConfig & auto_sink_config /* = {false, {}}*/) -: entity_manager_ptr(entity_manager_ptr), modules_(), auto_sink_config(auto_sink_config) + const std::set auto_sink_entity_types) +: entity_manager_ptr(entity_manager_ptr), modules_() { - if (auto_sink_config.generate_auto_sink) { - autoSink(); + if (not auto_sink_entity_types.empty()) { + generateAutoSinks(auto_sink_entity_types); } } -void TrafficController::autoSink() +void TrafficController::generateAutoSinks(const std::set & auto_sink_entity_types) { const auto hdmap_utils_ptr = entity_manager_ptr->getHdmapUtils(); for (const auto & lanelet_id : hdmap_utils_ptr->getLaneletIds()) { @@ -57,9 +57,9 @@ void TrafficController::autoSink() lanelet_pose.lanelet_id = lanelet_id; lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_ptr); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_ptr); + static constexpr double sink_radius = 1.0; const auto traffic_sink_config = TrafficSinkConfig( - auto_sink_config.radius, pose.position, auto_sink_config.default_sinkable_entity_type, - std::make_optional(lanelet_id)); + sink_radius, pose.position, auto_sink_entity_types, std::make_optional(lanelet_id)); addModule(entity_manager_ptr, traffic_sink_config); } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index c50d14680ee..24494e5b0bf 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -48,18 +48,15 @@ TrafficSink::TrafficSink( void TrafficSink::execute( [[maybe_unused]] const double current_time, [[maybe_unused]] const double step_time) { - const auto names = getEntityNames(); - for (const auto & name : names) { - const auto is_sinkable_entity = [this](const auto & entity_name) { - return config.sinkable_entity_type.empty() or - config.sinkable_entity_type.find(getEntityType(entity_name).type) != - config.sinkable_entity_type.end(); - }; - const auto pose = getEntityPose(name); - if ( - is_sinkable_entity(name) and - math::geometry::getDistance(config.position, pose) <= config.radius) { - despawn(name); + const auto entity_names = getEntityNames(); + for (const auto & entity_name : entity_names) { + const bool is_in_sinkable_radius = + math::geometry::getDistance(config.position, getEntityPose(entity_name)) <= config.radius; + const bool has_sinkable_entity_type = + config.sinkable_entity_types.find(getEntityType(entity_name).type) != + config.sinkable_entity_types.end(); + if (has_sinkable_entity_type and is_in_sinkable_radius) { + entity_manager_ptr->despawnEntity(entity_name); } } } @@ -115,19 +112,5 @@ auto TrafficSink::getEntityPose(const std::string & entity_name) const noexcept( THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); } } -auto TrafficSink::despawn(const std::string & entity_name) const -> void -{ - const auto entity_position = getEntityPose(entity_name).position; - const bool in_despawn_proximity = - math::geometry::hypot(entity_position, config.position) <= config.radius; - - const std::uint8_t entity_type = getEntityType(entity_name).type; - const bool is_despawn_candidate = - config.sinkable_entity_type.empty() or - config.sinkable_entity_type.find(entity_type) != config.sinkable_entity_type.cend(); - if (is_despawn_candidate and in_despawn_proximity) { - entity_manager_ptr->despawnEntity(entity_name); - } -} } // namespace traffic } // namespace traffic_simulator From c2d4f1e324e32dd1f63e2af9f3d05fdc05808848 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 9 Dec 2024 19:13:38 +0100 Subject: [PATCH 29/34] sink pedestrian test fix --- mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index 74dbf25598b..4b94b5ee5d2 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -30,7 +30,7 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode explicit StopAtCrosswalkScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "stop_at_crosswalk", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option) + "lanelet2_map.osm", __FILE__, false, option, {traffic_simulator::EntityType::PEDESTRIAN}) { start(); } From ce85613d3f17af93294158fc9de3e93449235bcd Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 10 Dec 2024 12:25:39 +0100 Subject: [PATCH 30/34] code style --- .../src/traffic_sink/auto_sink_vehicle.cpp | 12 ++++------- .../traffic/traffic_controller.hpp | 8 +++---- .../src/traffic/traffic_controller.cpp | 21 ++++++++++--------- 3 files changed, 19 insertions(+), 22 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index 951684a587d..8d3a89c2ae7 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -50,15 +50,11 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { - const auto canonicalized_lanelet_pose = + const auto map_pose = traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34774, 11.0, 0.0, api_.getHdmapUtils()); - api_.spawn( - "car", traffic_simulator::pose::toMapPose(canonicalized_lanelet_pose), - getVehicleParameters()); - api_.spawn( - "bob", traffic_simulator::pose::toMapPose(canonicalized_lanelet_pose), - getPedestrianParameters()); + 34774, 11.0, 0.0, api_.getHdmapUtils())); + api_.spawn("car", map_pose, getVehicleParameters()); + api_.spawn("bob", map_pose, getPedestrianParameters()); } }; } // namespace cpp_mock_scenarios diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 02de243fdb8..3ed3561f75a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -52,12 +52,12 @@ class TrafficController auto module_ptr = std::make_shared(std::forward(xs)...); modules_.emplace_back(module_ptr); } - void execute(const double current_time, const double step_time); - auto makeDebugMarker() const -> const visualization_msgs::msg::MarkerArray; + auto execute(const double current_time, const double step_time) -> void; + auto makeDebugMarker() const -> visualization_msgs::msg::MarkerArray; private: - void generateAutoSinks(const std::set & auto_sink_entity_types); - const std::shared_ptr entity_manager_ptr; + auto appendAutoSinks(const std::set & auto_sink_entity_types) -> void; + const std::shared_ptr entity_manager_ptr_; std::vector> modules_; }; } // namespace traffic diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index f069f4bd9a5..042cca131d9 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -41,42 +41,43 @@ namespace traffic TrafficController::TrafficController( const std::shared_ptr entity_manager_ptr, const std::set auto_sink_entity_types) -: entity_manager_ptr(entity_manager_ptr), modules_() +: entity_manager_ptr_(entity_manager_ptr), modules_() { if (not auto_sink_entity_types.empty()) { - generateAutoSinks(auto_sink_entity_types); + appendAutoSinks(auto_sink_entity_types); } } -void TrafficController::generateAutoSinks(const std::set & auto_sink_entity_types) +auto TrafficController::appendAutoSinks(const std::set & auto_sink_entity_types) + -> void { - const auto hdmap_utils_ptr = entity_manager_ptr->getHdmapUtils(); + static constexpr double sink_radius = 1.0; + const auto hdmap_utils_ptr = entity_manager_ptr_->getHdmapUtils(); for (const auto & lanelet_id : hdmap_utils_ptr->getLaneletIds()) { if (hdmap_utils_ptr->getNextLaneletIds(lanelet_id).empty()) { LaneletPose lanelet_pose; lanelet_pose.lanelet_id = lanelet_id; lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_ptr); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_ptr); - static constexpr double sink_radius = 1.0; const auto traffic_sink_config = TrafficSinkConfig( sink_radius, pose.position, auto_sink_entity_types, std::make_optional(lanelet_id)); - addModule(entity_manager_ptr, traffic_sink_config); + addModule(entity_manager_ptr_, traffic_sink_config); } } } -void TrafficController::execute(const double current_time, const double step_time) +auto TrafficController::execute(const double current_time, const double step_time) -> void { for (const auto & module : modules_) { module->execute(current_time, step_time); } } -auto TrafficController::makeDebugMarker() const -> const visualization_msgs::msg::MarkerArray +auto TrafficController::makeDebugMarker() const -> visualization_msgs::msg::MarkerArray { - static const auto marker_array = [&]() { + static const auto marker_array = [this]() { visualization_msgs::msg::MarkerArray marker_array; - for (size_t i = 0UL; i < modules_.size(); ++i) { + for (std::size_t i = 0UL; i < modules_.size(); ++i) { modules_[i]->appendDebugMarker(marker_array); } return marker_array; From 562b1ad691c32f07f4f3b9521f9624ac483c3bf7 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 10 Dec 2024 12:32:42 +0100 Subject: [PATCH 31/34] code style --- .../include/traffic_simulator/traffic/traffic_sink.hpp | 2 +- .../traffic_simulator/src/traffic/traffic_sink.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 5135b19497a..55c70576c6d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -100,7 +100,7 @@ class TrafficSink : public TrafficModuleBase auto getEntityPose(const std::string & entity_name) const noexcept(false) -> geometry_msgs::msg::Pose; - const std::shared_ptr entity_manager_ptr; + const std::shared_ptr entity_manager_ptr_; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 24494e5b0bf..33323b7aa16 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -41,7 +41,7 @@ namespace traffic { TrafficSink::TrafficSink( const std::shared_ptr entity_manager_ptr, const TrafficSinkConfig & config) -: TrafficModuleBase(), config(config), entity_manager_ptr(entity_manager_ptr) +: TrafficModuleBase(), config(config), entity_manager_ptr_(entity_manager_ptr) { } @@ -56,7 +56,7 @@ void TrafficSink::execute( config.sinkable_entity_types.find(getEntityType(entity_name).type) != config.sinkable_entity_types.end(); if (has_sinkable_entity_type and is_in_sinkable_radius) { - entity_manager_ptr->despawnEntity(entity_name); + entity_manager_ptr_->despawnEntity(entity_name); } } } @@ -92,12 +92,12 @@ auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marke auto TrafficSink::getEntityNames() const -> std::vector { - return entity_manager_ptr->getEntityNames(); + return entity_manager_ptr_->getEntityNames(); } auto TrafficSink::getEntityType(const std::string & entity_name) const noexcept(false) -> EntityType { - if (const auto entity = entity_manager_ptr->getEntity(entity_name)) { + if (const auto entity = entity_manager_ptr_->getEntity(entity_name)) { return entity->getEntityType(); } else { THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); @@ -106,7 +106,7 @@ auto TrafficSink::getEntityType(const std::string & entity_name) const noexcept( auto TrafficSink::getEntityPose(const std::string & entity_name) const noexcept(false) -> geometry_msgs::msg::Pose { - if (const auto entity = entity_manager_ptr->getEntity(entity_name)) { + if (const auto entity = entity_manager_ptr_->getEntity(entity_name)) { return entity->getMapPose(); } else { THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); From 0cca11022dfe97a10cfc1c56f37d1fcd73ed610f Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 10 Dec 2024 14:54:28 +0100 Subject: [PATCH 32/34] remove unnecessary funcs --- .../include/traffic_simulator/api/api.hpp | 1 - .../traffic/traffic_module_base.hpp | 2 +- .../traffic/traffic_sink.hpp | 10 ++-- .../src/traffic/traffic_sink.cpp | 54 ++++++++----------- 4 files changed, 25 insertions(+), 42 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index d485964cb68..be13d11637e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include #include diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_module_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_module_base.hpp index 41e52aaf0e4..b660cb0a281 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_module_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_module_base.hpp @@ -36,7 +36,7 @@ class TrafficModuleBase { public: TrafficModuleBase() {} - virtual void execute(const double current_time, const double step_time) = 0; + virtual auto execute(const double current_time, const double step_time) -> void = 0; virtual auto appendDebugMarker(visualization_msgs::msg::MarkerArray &) const -> void{}; }; } // namespace traffic diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 55c70576c6d..e3a99f23a86 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -88,18 +88,14 @@ class TrafficSink : public TrafficModuleBase * 1. Its distance from the TrafficSink is <= config.radius [m]. * 2. Its EntityType is in config.sinkable_entity_types. */ - void execute(const double current_time, const double step_time) override; + auto execute(const double current_time, const double step_time) -> void override; auto appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const -> void override; - const TrafficSinkConfig config; - private: - auto getEntityNames() const -> std::vector; - auto getEntityType(const std::string & entity_name) const noexcept(false) -> EntityType; - auto getEntityPose(const std::string & entity_name) const noexcept(false) - -> geometry_msgs::msg::Pose; + auto isEntitySinkable(const std::string & entity_name) const noexcept(false) -> bool; + const TrafficSinkConfig config_; const std::shared_ptr entity_manager_ptr_; }; } // namespace traffic diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 33323b7aa16..1c8238f1b7b 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -41,21 +41,16 @@ namespace traffic { TrafficSink::TrafficSink( const std::shared_ptr entity_manager_ptr, const TrafficSinkConfig & config) -: TrafficModuleBase(), config(config), entity_manager_ptr_(entity_manager_ptr) +: TrafficModuleBase(), config_(config), entity_manager_ptr_(entity_manager_ptr) { } -void TrafficSink::execute( - [[maybe_unused]] const double current_time, [[maybe_unused]] const double step_time) +auto TrafficSink::execute( + [[maybe_unused]] const double current_time, [[maybe_unused]] const double step_time) -> void { - const auto entity_names = getEntityNames(); + const auto entity_names = entity_manager_ptr_->getEntityNames(); for (const auto & entity_name : entity_names) { - const bool is_in_sinkable_radius = - math::geometry::getDistance(config.position, getEntityPose(entity_name)) <= config.radius; - const bool has_sinkable_entity_type = - config.sinkable_entity_types.find(getEntityType(entity_name).type) != - config.sinkable_entity_types.end(); - if (has_sinkable_entity_type and is_in_sinkable_radius) { + if (isEntitySinkable(entity_name)) { entity_manager_ptr_->despawnEntity(entity_name); } } @@ -66,51 +61,44 @@ auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marke { visualization_msgs::msg::Marker traffic_sink_marker; traffic_sink_marker.header.frame_id = "map"; - traffic_sink_marker.ns = "traffic_controller/traffic_sink/" + config.description; + traffic_sink_marker.ns = "traffic_controller/traffic_sink/" + config_.description; traffic_sink_marker.id = 0; traffic_sink_marker.action = traffic_sink_marker.ADD; traffic_sink_marker.type = 3; // cylinder traffic_sink_marker.pose = geometry_msgs::build() - .position(config.position) + .position(config_.position) .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1)); traffic_sink_marker.color = color_names::makeColorMsg("firebrick", 0.99); - traffic_sink_marker.scale.x = config.radius * 2.0; - traffic_sink_marker.scale.y = config.radius * 2.0; + traffic_sink_marker.scale.x = config_.radius * 2.0; + traffic_sink_marker.scale.y = config_.radius * 2.0; traffic_sink_marker.scale.z = 1.0; marker_array.markers.emplace_back(traffic_sink_marker); visualization_msgs::msg::Marker text_marker; text_marker = traffic_sink_marker; text_marker.id = 1; - text_marker.type = 9; //text - text_marker.text = config.description; + text_marker.type = 9; // text + text_marker.text = config_.description; text_marker.color = color_names::makeColorMsg("white", 0.99); text_marker.scale.z = 0.6; marker_array.markers.emplace_back(text_marker); } -auto TrafficSink::getEntityNames() const -> std::vector +auto TrafficSink::isEntitySinkable(const std::string & entity_name) const noexcept(false) -> bool { - return entity_manager_ptr_->getEntityNames(); -} - -auto TrafficSink::getEntityType(const std::string & entity_name) const noexcept(false) -> EntityType -{ - if (const auto entity = entity_manager_ptr_->getEntity(entity_name)) { - return entity->getEntityType(); - } else { + if (const auto entity = entity_manager_ptr_->getEntity(entity_name); entity == nullptr) { THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); - } -} -auto TrafficSink::getEntityPose(const std::string & entity_name) const noexcept(false) - -> geometry_msgs::msg::Pose -{ - if (const auto entity = entity_manager_ptr_->getEntity(entity_name)) { - return entity->getMapPose(); + } else if ( + config_.sinkable_entity_types.find(entity->getEntityType().type) == + config_.sinkable_entity_types.end()) { + return false; + } else if (math::geometry::getDistance(config_.position, entity->getMapPose()) > config_.radius) { + return false; } else { - THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); + return true; } } + } // namespace traffic } // namespace traffic_simulator From d0403a4440fad854e750ddb784b5610952c53292 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 10 Dec 2024 15:05:14 +0100 Subject: [PATCH 33/34] remove unnecessary include directives --- .../include/traffic_simulator/api/configuration.hpp | 1 - simulation/traffic_simulator/src/data_type/entity_status.cpp | 1 - simulation/traffic_simulator/src/traffic/traffic_controller.cpp | 2 -- 3 files changed, 4 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index ea84ee189f8..22007c7c6bc 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -23,7 +23,6 @@ #include #include #include -#include namespace traffic_simulator { diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index 6420e77aec3..b2bfb78432e 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -17,7 +17,6 @@ namespace traffic_simulator { - inline namespace entity_status { diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 042cca131d9..5e764e933c1 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -25,11 +25,9 @@ #include #include -#include #include #include #include -#include #include #include #include From 539580da26d5fe076e751481e3e0e6392da9e4cf Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 18 Dec 2024 11:42:41 +0100 Subject: [PATCH 34/34] style fix --- .../launch/mock_test.launch.py | 54 +++++-------------- .../src/traffic/traffic_sink.cpp | 3 +- 2 files changed, 13 insertions(+), 44 deletions(-) diff --git a/mock/cpp_mock_scenarios/launch/mock_test.launch.py b/mock/cpp_mock_scenarios/launch/mock_test.launch.py index ddfbda57f54..0a1f08e45f4 100644 --- a/mock/cpp_mock_scenarios/launch/mock_test.launch.py +++ b/mock/cpp_mock_scenarios/launch/mock_test.launch.py @@ -30,13 +30,7 @@ from launch.events import Shutdown from launch.event_handlers import OnProcessExit, OnProcessIO -from launch.actions import ( - EmitEvent, - RegisterEventHandler, - LogInfo, - TimerAction, - OpaqueFunction, -) +from launch.actions import EmitEvent, RegisterEventHandler, LogInfo, TimerAction, OpaqueFunction from launch.actions.declare_launch_argument import DeclareLaunchArgument from launch.substitutions.launch_configuration import LaunchConfiguration @@ -46,7 +40,6 @@ from pathlib import Path - class Color: BLACK = "\033[30m" RED = "\033[31m" @@ -104,10 +97,7 @@ def default_autoware_launch_file_of(architecture_type): def default_rviz_config_file(): - return ( - Path(get_package_share_directory("traffic_simulator")) - / "config/scenario_simulator_v2.rviz" - ) + return Path(get_package_share_directory("traffic_simulator")) / "config/scenario_simulator_v2.rviz" def launch_setup(context, *args, **kwargs): @@ -138,38 +128,20 @@ def launch_setup(context, *args, **kwargs): junit_path = LaunchConfiguration("junit_path", default="/tmp/output.xunit.xml") # fmt: on - print( - f"architecture_type := {architecture_type.perform(context)}" - ) - print( - f"autoware_launch_file := {autoware_launch_file.perform(context)}" - ) - print( - f"autoware_launch_package := {autoware_launch_package.perform(context)}" - ) - print( - f"consider_acceleration_by_road_slope := {consider_acceleration_by_road_slope.perform(context)}" - ) - print( - f"consider_pose_by_road_slope := {consider_pose_by_road_slope.perform(context)}" - ) - print( - f"global_frame_rate := {global_frame_rate.perform(context)}" - ) - print( - f"global_real_time_factor := {global_real_time_factor.perform(context)}" - ) + print(f"architecture_type := {architecture_type.perform(context)}") + print(f"autoware_launch_file := {autoware_launch_file.perform(context)}") + print(f"autoware_launch_package := {autoware_launch_package.perform(context)}") + print(f"consider_acceleration_by_road_slope := {consider_acceleration_by_road_slope.perform(context)}") + print(f"consider_pose_by_road_slope := {consider_pose_by_road_slope.perform(context)}") + print(f"global_frame_rate := {global_frame_rate.perform(context)}") + print(f"global_real_time_factor := {global_real_time_factor.perform(context)}") print(f"global_timeout := {global_timeout.perform(context)}") - print( - f"initialize_duration := {initialize_duration.perform(context)}" - ) + print(f"initialize_duration := {initialize_duration.perform(context)}") print(f"launch_autoware := {launch_autoware.perform(context)}") print(f"launch_rviz := {launch_rviz.perform(context)}") print(f"output_directory := {output_directory.perform(context)}") print(f"port := {port.perform(context)}") - print( - f"publish_empty_context := {publish_empty_context.perform(context)}" - ) + print(f"publish_empty_context := {publish_empty_context.perform(context)}") print(f"record := {record.perform(context)}") print(f"rviz_config := {rviz_config.perform(context)}") print(f"scenario := {scenario.perform(context)}") @@ -185,9 +157,7 @@ def make_parameters(): {"architecture_type": architecture_type}, {"autoware_launch_file": autoware_launch_file}, {"autoware_launch_package": autoware_launch_package}, - { - "consider_acceleration_by_road_slope": consider_acceleration_by_road_slope - }, + {"consider_acceleration_by_road_slope": consider_acceleration_by_road_slope}, {"consider_pose_by_road_slope": consider_pose_by_road_slope}, {"initialize_duration": initialize_duration}, {"launch_autoware": launch_autoware}, diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index 1c8238f1b7b..dc41a3a3a72 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -48,8 +48,7 @@ TrafficSink::TrafficSink( auto TrafficSink::execute( [[maybe_unused]] const double current_time, [[maybe_unused]] const double step_time) -> void { - const auto entity_names = entity_manager_ptr_->getEntityNames(); - for (const auto & entity_name : entity_names) { + for (const auto & entity_name : entity_manager_ptr_->getEntityNames()) { if (isEntitySinkable(entity_name)) { entity_manager_ptr_->despawnEntity(entity_name); }