Update function template available<T> to prepare its own node

Signed-off-by: yamacir-kit <[email protected]>
tier4 · Jan 31, 2025 · c80fe28 · c80fe28
1 parent 31c27dc
commit c80fe28
Show file tree

Hide file tree

Showing 6 changed files with 84 additions and 50 deletions.
diff --git a/external/concealer/include/concealer/available.hpp b/external/concealer/include/concealer/available.hpp
@@ -15,13 +15,10 @@
 #ifndef CONCEALER__AVAILABLE_HPP_
 #define CONCEALER__AVAILABLE_HPP_
 
-#include <rclcpp/rclcpp.hpp>
-
 namespace concealer
 {
 template <typename T>
-constexpr auto available(const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &)
-  -> bool
+constexpr auto available() -> bool
 {
   return true;
 }

diff --git a/external/concealer/include/concealer/get_parameter.hpp b/external/concealer/include/concealer/get_parameter.hpp
@@ -29,6 +29,13 @@ auto getParameter(
   }
   return node->get_parameter(name).get_value<T>();
 }
+
+template <typename T>
+auto getParameter(const std::string & name, T value = {})
+{
+  auto node = rclcpp::Node("get_parameter", "simulation");
+  return getParameter(node.get_node_parameters_interface(), name, value);
+}
 }  // namespace concealer
 
 #endif  // CONCEALER__GET_PARAMETER_HPP_
diff --git a/external/concealer/include/concealer/path_with_lane_id.hpp b/external/concealer/include/concealer/path_with_lane_id.hpp
@@ -47,14 +47,12 @@ static_assert(0 < std::tuple_size_v<PathWithLaneId>);
 
 #if __has_include(<autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>)
 template <>
-auto available<autoware_internal_planning_msgs::msg::PathWithLaneId>(
-  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &) -> bool;
+auto available<autoware_internal_planning_msgs::msg::PathWithLaneId>() -> bool;
 #endif
 
 #if __has_include(<tier4_planning_msgs/msg/path_with_lane_id.hpp>)
 template <>
-auto available<tier4_planning_msgs::msg::PathWithLaneId>(
-  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &) -> bool;
+auto available<tier4_planning_msgs::msg::PathWithLaneId>() -> bool;
 #endif
 
 #if __has_include(<autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>) and \

diff --git a/external/concealer/include/concealer/subscriber.hpp b/external/concealer/include/concealer/subscriber.hpp
@@ -16,6 +16,7 @@
 #define CONCEALER__SUBSCRIBER_HPP_
 
 #include <concealer/convert.hpp>
+#include <concealer/get_parameter.hpp>
 #include <memory>
 #include <rclcpp/rclcpp.hpp>
 #include <scenario_simulator_exception/exception.hpp>
@@ -39,29 +40,27 @@ struct Subscriber<Message>
     const std::string & topic, const rclcpp::QoS & quality_of_service, Autoware & autoware,
     const Callback & callback)
   : subscription(
-      available<Message>(autoware.get_node_parameters_interface())
-        ? autoware.template create_subscription<Message>(
-            topic, quality_of_service,
-            [this, callback](const typename Message::ConstSharedPtr & message) {
-              if (std::atomic_store(&current_value, message); current_value) {
-                callback((*this)());
-              }
-            })
-        : nullptr)
+      available<Message>() ? autoware.template create_subscription<Message>(
+                               topic, quality_of_service,
+                               [this, callback](const typename Message::ConstSharedPtr & message) {
+                                 if (std::atomic_store(&current_value, message); current_value) {
+                                   callback((*this)());
+                                 }
+                               })
+                           : nullptr)
   {
   }
 
   template <typename Autoware>
   explicit Subscriber(
     const std::string & topic, const rclcpp::QoS & quality_of_service, Autoware & autoware)
   : subscription(
-      available<Message>(autoware.get_node_parameters_interface())
-        ? autoware.template create_subscription<Message>(
-            topic, quality_of_service,
-            [this](const typename Message::ConstSharedPtr & message) {
-              std::atomic_store(&current_value, message);
-            })
-        : nullptr)
+      available<Message>() ? autoware.template create_subscription<Message>(
+                               topic, quality_of_service,
+                               [this](const typename Message::ConstSharedPtr & message) {
+                                 std::atomic_store(&current_value, message);
+                               })
+                           : nullptr)
   {
   }
 };
@@ -88,15 +87,14 @@ struct Subscriber<Message, T, Ts...> : public Subscriber<T, Ts...>
     const Callback & callback)
   : Subscriber<T, Ts...>(topic, quality_of_service, autoware, callback),
     subscription(
-      available<Message>(autoware.get_node_parameters_interface())
-        ? autoware.template create_subscription<Message>(
-            topic, quality_of_service,
-            [this, callback](const typename Message::ConstSharedPtr & message) {
-              if (std::atomic_store(&current_value, message); current_value) {
-                callback((*this)());
-              }
-            })
-        : nullptr)
+      available<Message>() ? autoware.template create_subscription<Message>(
+                               topic, quality_of_service,
+                               [this, callback](const typename Message::ConstSharedPtr & message) {
+                                 if (std::atomic_store(&current_value, message); current_value) {
+                                   callback((*this)());
+                                 }
+                               })
+                           : nullptr)
   {
   }
 
@@ -105,21 +103,45 @@ struct Subscriber<Message, T, Ts...> : public Subscriber<T, Ts...>
     const std::string & topic, const rclcpp::QoS & quality_of_service, Autoware & autoware)
   : Subscriber<T, Ts...>(topic, quality_of_service, autoware),
     subscription(
-      available<Message>(autoware.get_node_parameters_interface())
-        ? autoware.template create_subscription<Message>(
-            topic, quality_of_service,
-            [this](const typename Message::ConstSharedPtr & message) {
-              std::atomic_store(&current_value, message);
-            })
-        : nullptr)
+      available<Message>() ? autoware.template create_subscription<Message>(
+                               topic, quality_of_service,
+                               [this](const typename Message::ConstSharedPtr & message) {
+                                 std::atomic_store(&current_value, message);
+                               })
+                           : nullptr)
   {
   }
 };
 
-template <typename... Ts>
-struct Subscriber<std::tuple<Ts...>> : public Subscriber<Ts...>
+template <typename... Messages>
+struct Subscriber<std::tuple<Messages...>> : public Subscriber<Messages...>
 {
-  using Subscriber<Ts...>::Subscriber;
+  using type = std::tuple<Messages...>;
+
+  using primary_type = std::tuple_element_t<0, type>;
+
+  template <typename F, typename T, typename... Ts>
+  constexpr auto any(F f, const Subscriber<T, Ts...> & x) -> bool
+  {
+    if constexpr (0 < sizeof...(Ts)) {
+      return f(x) or any(f, static_cast<const Subscriber<Ts...> &>(x));
+    } else {
+      return f(x);
+    }
+  }
+
+  template <typename... Ts>
+  explicit Subscriber(const std::string & topic, Ts &&... xs)
+  : Subscriber<Messages...>(topic, std::forward<decltype(xs)>(xs)...)
+  {
+    auto subscription_available = [](const auto & x) { return static_cast<bool>(x.subscription); };
+
+    if (not any(subscription_available, static_cast<const Subscriber<Messages...> &>(*this))) {
+      throw common::scenario_simulator_exception::Error(
+        "No viable subscription for topic ", std::quoted(topic), " in ",
+        getParameter<std::string>("architecture_type"), ".");
+    }
+  }
 };
 }  // namespace concealer
 

diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp
@@ -16,7 +16,13 @@
 
 namespace concealer
 {
-AutowareUniverse::AutowareUniverse(bool simulate_localization)
+/*
+   clang-format seems not to understand the constructor function try-block: it
+   misidentifies the try as a jump label and the formatting afterwards falls
+   apart.
+*/
+// clang-format off
+AutowareUniverse::AutowareUniverse(bool simulate_localization) try
 : rclcpp::Node("concealer", "simulation"),
   getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this),
   getGearCommand("/control/command/gear_cmd", rclcpp::QoS(1), *this),
@@ -161,6 +167,12 @@ AutowareUniverse::AutowareUniverse(bool simulate_localization)
   }))
 {
 }
+catch (...)
+{
+  thrown = std::current_exception();
+  is_thrown.store(true);
+}
+// clang-format on
 
 AutowareUniverse::~AutowareUniverse()
 {

diff --git a/external/concealer/src/path_with_lane_id.cpp b/external/concealer/src/path_with_lane_id.cpp
@@ -19,10 +19,9 @@ namespace concealer
 {
 #if __has_include(<autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>)
 template <>
-auto available<autoware_internal_planning_msgs::msg::PathWithLaneId>(
-  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node) -> bool
+auto available<autoware_internal_planning_msgs::msg::PathWithLaneId>() -> bool
 {
-  if (const auto architecture_type = getParameter<std::string>(node, "architecture_type");
+  if (const auto architecture_type = getParameter<std::string>("architecture_type");
       architecture_type.find("awf/universe") != std::string::npos) {
     return "awf/universe/20250130" <= architecture_type;
   } else {
@@ -33,10 +32,9 @@ auto available<autoware_internal_planning_msgs::msg::PathWithLaneId>(
 
 #if __has_include(<tier4_planning_msgs/msg/path_with_lane_id.hpp>)
 template <>
-auto available<tier4_planning_msgs::msg::PathWithLaneId>(
-  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node) -> bool
+auto available<tier4_planning_msgs::msg::PathWithLaneId>() -> bool
 {
-  if (const auto architecture_type = getParameter<std::string>(node, "architecture_type");
+  if (const auto architecture_type = getParameter<std::string>("architecture_type");
       architecture_type.find("awf/universe") != std::string::npos) {
     return architecture_type < "awf/universe/20250130";
   } else {