From 09de4c5df200b333133ffc293cd4c82880f7b169 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 8 Nov 2024 12:12:16 +0900 Subject: [PATCH 01/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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);