Merge pull request #1518 from tier4/feature/support-latest-autoware-m…

…essage-type

Support message type `autoware_internal_planning_msgs::msg::PathWithLaneId`

external/concealer/CMakeLists.txt

@@ -18,6 +18,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/execute.cpp
   src/field_operator_application.cpp
   src/is_package_exists.cpp
+  src/path_with_lane_id.cpp
   src/task_queue.cpp)
 
 target_link_libraries(${PROJECT_NAME}

external/concealer/include/concealer/autoware_universe.hpp

@@ -26,6 +26,7 @@
 #include <autoware_vehicle_msgs/msg/velocity_report.hpp>
 #include <autoware_vehicle_msgs/srv/control_mode_command.hpp>
 #include <concealer/continuous_transform_broadcaster.hpp>
+#include <concealer/path_with_lane_id.hpp>
 #include <concealer/publisher.hpp>
 #include <concealer/subscriber.hpp>
 #include <concealer/visibility.hpp>
@@ -34,7 +35,6 @@
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
 namespace concealer
 {
@@ -50,17 +50,16 @@ class AutowareUniverse : public rclcpp::Node,
   using GearCommand                 = autoware_vehicle_msgs::msg::GearCommand;
   using GearReport                  = autoware_vehicle_msgs::msg::GearReport;
   using Odometry                    = nav_msgs::msg::Odometry;
-  using PathWithLaneId              = tier4_planning_msgs::msg::PathWithLaneId;
   using PoseWithCovarianceStamped   = geometry_msgs::msg::PoseWithCovarianceStamped;
   using SteeringReport              = autoware_vehicle_msgs::msg::SteeringReport;
   using TurnIndicatorsCommand       = autoware_vehicle_msgs::msg::TurnIndicatorsCommand;
   using TurnIndicatorsReport        = autoware_vehicle_msgs::msg::TurnIndicatorsReport;
   using VelocityReport              = autoware_vehicle_msgs::msg::VelocityReport;
 
-  Subscriber<Control>               getCommand;
-  Subscriber<GearCommand>           getGearCommand;
-  Subscriber<TurnIndicatorsCommand> getTurnIndicatorsCommand;
-  Subscriber<PathWithLaneId>        getPathWithLaneId;
+  Subscriber<Control>                  getCommand;
+  Subscriber<GearCommand>              getGearCommand;
+  Subscriber<TurnIndicatorsCommand>    getTurnIndicatorsCommand;
+  Subscriber<priority::PathWithLaneId> getPathWithLaneId;
 
   Publisher<AccelWithCovarianceStamped> setAcceleration;
   Publisher<Odometry>                   setOdometry;

external/concealer/include/concealer/available.hpp

@@ -0,0 +1,27 @@
+// 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.
+
+#ifndef CONCEALER__AVAILABLE_HPP_
+#define CONCEALER__AVAILABLE_HPP_
+
+namespace concealer
+{
+template <typename T>
+constexpr auto available() -> bool
+{
+  return true;
+}
+}  // namespace concealer
+
+#endif  // CONCEALER__AVAILABLE_HPP_

external/concealer/include/concealer/convert.hpp

@@ -0,0 +1,27 @@
+// 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.
+
+#ifndef CONCEALER__CONVERT_HPP_
+#define CONCEALER__CONVERT_HPP_
+
+namespace concealer
+{
+template <typename To, typename From>
+auto convert(const From & from) -> To
+{
+  return std::forward<decltype(from)>(from);
+}
+}  // namespace concealer
+
+#endif  // CONCEALER__CONVERT_HPP_

external/concealer/include/concealer/field_operator_application.hpp

@@ -31,6 +31,7 @@
 #include <autoware_vehicle_msgs/msg/gear_command.hpp>
 #include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
 #include <concealer/autoware_universe.hpp>
+#include <concealer/path_with_lane_id.hpp>
 #include <concealer/publisher.hpp>
 #include <concealer/service.hpp>
 #include <concealer/subscriber.hpp>
@@ -44,7 +45,6 @@
 #include <tier4_external_api_msgs/msg/emergency.hpp>
 #include <tier4_external_api_msgs/srv/engage.hpp>
 #include <tier4_external_api_msgs/srv/set_velocity_limit.hpp>
-#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 #include <tier4_planning_msgs/msg/trajectory.hpp>
 #include <tier4_rtc_msgs/msg/cooperate_status_array.hpp>
 #include <tier4_rtc_msgs/srv/auto_mode_with_module.hpp>
@@ -78,7 +78,6 @@ struct FieldOperatorApplication : public rclcpp::Node,
   using Emergency                       = tier4_external_api_msgs::msg::Emergency;
   using LocalizationInitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
   using MrmState                        = autoware_adapi_v1_msgs::msg::MrmState;
-  using PathWithLaneId                  = tier4_planning_msgs::msg::PathWithLaneId;
   using Trajectory                      = tier4_planning_msgs::msg::Trajectory;
   using TurnIndicatorsCommand           = autoware_vehicle_msgs::msg::TurnIndicatorsCommand;
 
@@ -98,10 +97,10 @@ struct FieldOperatorApplication : public rclcpp::Node,
 #if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
   Subscriber<LocalizationInitializationState> getLocalizationState;
 #endif
-  Subscriber<MrmState>                        getMrmState;
-  Subscriber<PathWithLaneId>                  getPathWithLaneId;
-  Subscriber<Trajectory>                      getTrajectory;
-  Subscriber<TurnIndicatorsCommand>           getTurnIndicatorsCommand;
+  Subscriber<MrmState>                 getMrmState;
+  Subscriber<priority::PathWithLaneId> getPathWithLaneId;
+  Subscriber<Trajectory>               getTrajectory;
+  Subscriber<TurnIndicatorsCommand>    getTurnIndicatorsCommand;
 
   Service<ClearRoute>             requestClearRoute;
   Service<CooperateCommands>      requestCooperateCommands;

external/concealer/include/concealer/get_parameter.hpp

@@ -0,0 +1,43 @@
+// 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.
+
+#ifndef CONCEALER__GET_PARAMETER_HPP_
+#define CONCEALER__GET_PARAMETER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+namespace concealer
+{
+static constexpr auto default_architecture_type = "awf/universe/20240605";
+
+template <typename T>
+auto getParameter(
+  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node,
+  const std::string & name, T value = {})
+{
+  if (not node->has_parameter(name)) {
+    node->declare_parameter(name, rclcpp::ParameterValue(value));
+  }
+  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_

external/concealer/include/concealer/path_with_lane_id.hpp

@@ -0,0 +1,66 @@
+// 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.
+
+#ifndef CONCEALER__PATH_WITH_LANE_ID_HPP_
+#define CONCEALER__PATH_WITH_LANE_ID_HPP_
+
+#if __has_include(<autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>)
+#include <autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>
+#endif
+
+#if __has_include(<tier4_planning_msgs/msg/path_with_lane_id.hpp>)
+#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
+#endif
+
+#include <concealer/available.hpp>
+#include <concealer/convert.hpp>
+
+namespace concealer
+{
+namespace priority
+{
+using PathWithLaneId = decltype(std::tuple_cat(
+  std::declval<std::tuple<
+#if __has_include(<autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>)
+    autoware_internal_planning_msgs::msg::PathWithLaneId
+#endif
+    > >(),
+  std::declval<std::tuple<
+#if __has_include(<tier4_planning_msgs/msg/path_with_lane_id.hpp>)
+    tier4_planning_msgs::msg::PathWithLaneId
+#endif
+    > >()));
+
+static_assert(0 < std::tuple_size_v<PathWithLaneId>);
+}  // namespace priority
+
+#if __has_include(<autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>)
+template <>
+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>() -> bool;
+#endif
+
+#if __has_include(<autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>) and \
+    __has_include(<tier4_planning_msgs/msg/path_with_lane_id.hpp>)
+template <>
+auto convert(const tier4_planning_msgs::msg::PathWithLaneId & from)
+  -> autoware_internal_planning_msgs::msg::PathWithLaneId;
+#endif
+}  // namespace concealer
+
+#endif  // CONCEALER__PATH_WITH_LANE_ID_HPP_

external/concealer/include/concealer/subscriber.hpp

@@ -15,13 +15,19 @@
 #ifndef CONCEALER__SUBSCRIBER_HPP_
 #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>
 
 namespace concealer
 {
+template <typename...>
+struct Subscriber;
+
 template <typename Message>
-struct Subscriber
+struct Subscriber<Message>
 {
   typename Message::ConstSharedPtr current_value = std::make_shared<const Message>();
 
@@ -33,24 +39,108 @@ struct Subscriber
   explicit Subscriber(
     const std::string & topic, const rclcpp::QoS & quality_of_service, Autoware & autoware,
     const Callback & callback)
-  : subscription(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)());
-        }
-      }))
+  : subscription(
+      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(autoware.template create_subscription<Message>(
-      topic, quality_of_service, [this](const typename Message::ConstSharedPtr & message) {
-        std::atomic_store(&current_value, message);
-      }))
+  : subscription(
+      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 Message, typename T, typename... Ts>
+struct Subscriber<Message, T, Ts...> : public Subscriber<T, Ts...>
+{
+  typename Message::ConstSharedPtr current_value = nullptr;
+
+  typename rclcpp::Subscription<Message>::SharedPtr subscription;
+
+  auto operator()() const -> Message
   {
+    if (auto value = std::atomic_load(&current_value)) {
+      return *value;
+    } else {
+      return convert<Message>(Subscriber<T, Ts...>::operator()());
+    }
+  }
+
+  template <typename Autoware, typename Callback>
+  explicit Subscriber(
+    const std::string & topic, const rclcpp::QoS & quality_of_service, Autoware & autoware,
+    const Callback & callback)
+  : Subscriber<T, Ts...>(topic, quality_of_service, autoware, callback),
+    subscription(
+      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)
+  : Subscriber<T, Ts...>(topic, quality_of_service, autoware),
+    subscription(
+      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... Messages>
+struct Subscriber<std::tuple<Messages...>> : public Subscriber<Messages...>
+{
+  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