Refactor/concealer 2

external/concealer/include/concealer/autoware_universe.hpp

@@ -26,8 +26,8 @@
 #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/publisher_wrapper.hpp>
-#include <concealer/subscriber_wrapper.hpp>
+#include <concealer/publisher.hpp>
+#include <concealer/subscriber.hpp>
 #include <concealer/visibility.hpp>
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/pose.hpp>
@@ -57,19 +57,19 @@ class AutowareUniverse : public rclcpp::Node,
   using TurnIndicatorsReport        = autoware_vehicle_msgs::msg::TurnIndicatorsReport;
   using VelocityReport              = autoware_vehicle_msgs::msg::VelocityReport;
 
-  SubscriberWrapper<Control>               getCommand;
-  SubscriberWrapper<GearCommand>           getGearCommand;
-  SubscriberWrapper<TurnIndicatorsCommand> getTurnIndicatorsCommand;
-  SubscriberWrapper<PathWithLaneId>        getPathWithLaneId;
+  Subscriber<Control>               getCommand;
+  Subscriber<GearCommand>           getGearCommand;
+  Subscriber<TurnIndicatorsCommand> getTurnIndicatorsCommand;
+  Subscriber<PathWithLaneId>        getPathWithLaneId;
 
-  PublisherWrapper<AccelWithCovarianceStamped> setAcceleration;
-  PublisherWrapper<Odometry>                   setOdometry;
-  PublisherWrapper<PoseWithCovarianceStamped>  setPose;
-  PublisherWrapper<SteeringReport>             setSteeringReport;
-  PublisherWrapper<GearReport>                 setGearReport;
-  PublisherWrapper<ControlModeReport>          setControlModeReport;
-  PublisherWrapper<VelocityReport>             setVelocityReport;
-  PublisherWrapper<TurnIndicatorsReport>       setTurnIndicatorsReport;
+  Publisher<AccelWithCovarianceStamped> setAcceleration;
+  Publisher<Odometry>                   setOdometry;
+  Publisher<PoseWithCovarianceStamped>  setPose;
+  Publisher<SteeringReport>             setSteeringReport;
+  Publisher<GearReport>                 setGearReport;
+  Publisher<ControlModeReport>          setControlModeReport;
+  Publisher<VelocityReport>             setVelocityReport;
+  Publisher<TurnIndicatorsReport>       setTurnIndicatorsReport;
 
   std::atomic<geometry_msgs::msg::Accel> current_acceleration;
   std::atomic<geometry_msgs::msg::Pose>  current_pose;
@@ -83,7 +83,7 @@ class AutowareUniverse : public rclcpp::Node,
 
   const rclcpp::TimerBase::SharedPtr vehicle_state_update_timer;
 
-  std::thread localization_and_vehicle_state_update_thread;
+  std::thread spinner;
 
   std::atomic<bool> is_stop_requested = false;
 
@@ -100,10 +100,6 @@ class AutowareUniverse : public rclcpp::Node,
 
   auto rethrow() -> void;
 
-  auto updateLocalization() -> void;
-
-  auto updateVehicleState() -> void;
-
   auto getVehicleCommand() const -> std::tuple<double, double, double, double, int>;
 
   auto getRouteLanelets() const -> std::vector<std::int64_t>;

external/concealer/include/concealer/field_operator_application.hpp

@@ -31,10 +31,9 @@
 #include <autoware_vehicle_msgs/msg/gear_command.hpp>
 #include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
 #include <concealer/autoware_universe.hpp>
-#include <concealer/launch.hpp>
-#include <concealer/publisher_wrapper.hpp>
-#include <concealer/service_with_validation.hpp>
-#include <concealer/subscriber_wrapper.hpp>
+#include <concealer/publisher.hpp>
+#include <concealer/service.hpp>
+#include <concealer/subscriber.hpp>
 #include <concealer/task_queue.hpp>
 #include <concealer/transition_assertion.hpp>
 #include <concealer/visibility.hpp>
@@ -55,19 +54,6 @@
 
 namespace concealer
 {
-/* ---- NOTE -------------------------------------------------------------------
- *
- *  The magic class 'FieldOperatorApplication' is a class that makes it easy to work with
- *  Autoware from C++. The main features of this class are as follows
- *
- *    (1) Launch Autoware in an independent process upon instantiation of the
- *        class.
- *    (2) Properly terminates the Autoware process started by the constructor
- *        upon destruction of the class.
- *    (3) Probably the simplest instructions to Autoware, consisting of
- *        initialize, plan, and engage.
- *
- * -------------------------------------------------------------------------- */
 struct FieldOperatorApplication : public rclcpp::Node,
                                   public TransitionAssertion<FieldOperatorApplication>
 {
@@ -77,11 +63,9 @@ struct FieldOperatorApplication : public rclcpp::Node,
 
   const pid_t process_id = 0;
 
-  bool initialize_was_called = false;
+  bool initialized = false;
 
-  std::string autoware_state;
-
-  tier4_rtc_msgs::msg::CooperateStatusArray latest_cooperate_status_array;
+  std::string autoware_state = "LAUNCHING";
 
   std::string minimum_risk_maneuver_state;
 
@@ -107,26 +91,26 @@ struct FieldOperatorApplication : public rclcpp::Node,
   using SetVelocityLimit                = tier4_external_api_msgs::srv::SetVelocityLimit;
   using ChangeOperationMode             = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
 
-  SubscriberWrapper<AutowareState>                   getAutowareState;
-  SubscriberWrapper<Control>                         getCommand;
-  SubscriberWrapper<CooperateStatusArray>            getCooperateStatusArray;
-  SubscriberWrapper<Emergency>                       getEmergencyState;
+  Subscriber<AutowareState>                   getAutowareState;
+  Subscriber<Control>                         getCommand;
+  Subscriber<CooperateStatusArray>            getCooperateStatusArray;
+  Subscriber<Emergency>                       getEmergencyState;
 #if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
-  SubscriberWrapper<LocalizationInitializationState> getLocalizationState;
+  Subscriber<LocalizationInitializationState> getLocalizationState;
 #endif
-  SubscriberWrapper<MrmState>                        getMrmState;
-  SubscriberWrapper<PathWithLaneId>                  getPathWithLaneId;
-  SubscriberWrapper<Trajectory>                      getTrajectory;
-  SubscriberWrapper<TurnIndicatorsCommand>           getTurnIndicatorsCommand;
-
-  ServiceWithValidation<ClearRoute>                  requestClearRoute;
-  ServiceWithValidation<CooperateCommands>           requestCooperateCommands;
-  ServiceWithValidation<Engage>                      requestEngage;
-  ServiceWithValidation<InitializeLocalization>      requestInitialPose;
-  ServiceWithValidation<SetRoutePoints>              requestSetRoutePoints;
-  ServiceWithValidation<AutoModeWithModule>          requestSetRtcAutoMode;
-  ServiceWithValidation<SetVelocityLimit>            requestSetVelocityLimit;
-  ServiceWithValidation<ChangeOperationMode>         requestEnableAutowareControl;
+  Subscriber<MrmState>                        getMrmState;
+  Subscriber<PathWithLaneId>                  getPathWithLaneId;
+  Subscriber<Trajectory>                      getTrajectory;
+  Subscriber<TurnIndicatorsCommand>           getTurnIndicatorsCommand;
+
+  Service<ClearRoute>             requestClearRoute;
+  Service<CooperateCommands>      requestCooperateCommands;
+  Service<Engage>                 requestEngage;
+  Service<InitializeLocalization> requestInitialPose;
+  Service<SetRoutePoints>         requestSetRoutePoints;
+  Service<AutoModeWithModule>     requestSetRtcAutoMode;
+  Service<SetVelocityLimit>       requestSetVelocityLimit;
+  Service<ChangeOperationMode>    requestEnableAutowareControl;
   // clang-format on
 
   /*
@@ -136,13 +120,7 @@ struct FieldOperatorApplication : public rclcpp::Node,
   */
   TaskQueue task_queue;
 
-  CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t = 0);
-
-  template <typename... Ts>
-  CONCEALER_PUBLIC explicit FieldOperatorApplication(Ts &&... xs)
-  : FieldOperatorApplication(ros2_launch(std::forward<decltype(xs)>(xs)...))
-  {
-  }
+  CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t);
 
   ~FieldOperatorApplication();
 
@@ -160,18 +138,8 @@ struct FieldOperatorApplication : public rclcpp::Node,
 
   auto clearRoute() -> void;
 
-  auto getAutowareStateName() const { return autoware_state; }
-
-  auto getMinimumRiskManeuverBehaviorName() const { return minimum_risk_maneuver_behavior; }
-
-  auto getMinimumRiskManeuverStateName() const { return minimum_risk_maneuver_state; }
-
-  auto getEmergencyStateName() const { return minimum_risk_maneuver_state; }
-
   auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray;
 
-  auto initialized() const noexcept { return initialize_was_called; }
-
   auto requestAutoModeForCooperation(const std::string &, bool) -> void;
 
   auto rethrow() const { task_queue.rethrow(); }

external/concealer/include/concealer/publisher_wrapper.hpp → external/concealer/include/concealer/publisher.hpp

@@ -12,33 +12,32 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef CONCEALER__PUBLISHER_WRAPPER_HPP_
-#define CONCEALER__PUBLISHER_WRAPPER_HPP_
+#ifndef CONCEALER__PUBLISHER_HPP_
+#define CONCEALER__PUBLISHER_HPP_
 
 #include <memory>
 #include <rclcpp/rclcpp.hpp>
 
 namespace concealer
 {
-template <typename MessageType>
-class PublisherWrapper
+template <typename Message>
+class Publisher
 {
-private:
-  typename rclcpp::Publisher<MessageType>::SharedPtr publisher;
+  typename rclcpp::Publisher<Message>::SharedPtr publisher;
 
 public:
-  auto operator()(const MessageType & message) const -> decltype(auto)
+  template <typename Node>
+  explicit Publisher(const std::string & topic, Node & node)
+  : publisher(node.template create_publisher<Message>(topic, rclcpp::QoS(1).reliable()))
   {
-    return publisher->publish(message);
   }
 
-  template <typename NodeInterface>
-  explicit PublisherWrapper(std::string topic, NodeInterface & autoware_interface)
-  : publisher(
-      autoware_interface.template create_publisher<MessageType>(topic, rclcpp::QoS(1).reliable()))
+  template <typename... Ts>
+  auto operator()(Ts &&... xs) const -> decltype(auto)
   {
+    return publisher->publish(std::forward<decltype(xs)>(xs)...);
   }
 };
 }  // namespace concealer
 
-#endif  // CONCEALER__PUBLISHER_WRAPPER_HPP_
+#endif  // CONCEALER__PUBLISHER_HPP_

external/concealer/include/concealer/service_with_validation.hpp → external/concealer/include/concealer/service.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef CONCEALER__SERVICE_WITH_VALIDATION_HPP_
-#define CONCEALER__SERVICE_WITH_VALIDATION_HPP_
+#ifndef CONCEALER__SERVICE_HPP_
+#define CONCEALER__SERVICE_HPP_
 
 #include <autoware_adapi_v1_msgs/msg/response_status.hpp>
 #include <chrono>
@@ -28,7 +28,7 @@
 namespace concealer
 {
 template <typename T>
-class ServiceWithValidation
+class Service
 {
   const std::string service_name;
 
@@ -39,13 +39,13 @@ class ServiceWithValidation
   rclcpp::WallRate validation_rate;
 
 public:
-  template <typename FieldOperatorApplication>
-  explicit ServiceWithValidation(
-    const std::string & service_name, FieldOperatorApplication & autoware,
+  template <typename Node>
+  explicit Service(
+    const std::string & service_name, Node & node,
     const std::chrono::nanoseconds validation_interval = std::chrono::seconds(1))
   : service_name(service_name),
-    logger(autoware.get_logger()),
-    client(autoware.template create_client<T>(service_name, rmw_qos_profile_default)),
+    logger(node.get_logger()),
+    client(node.template create_client<T>(service_name, rmw_qos_profile_default)),
     validation_rate(validation_interval)
   {
   }
@@ -142,4 +142,4 @@ class ServiceWithValidation
 };
 }  // namespace concealer
 
-#endif  //CONCEALER__SERVICE_WITH_VALIDATION_HPP_
+#endif  //CONCEALER__SERVICE_HPP_

external/concealer/include/concealer/subscriber_wrapper.hpp → external/concealer/include/concealer/subscriber.hpp

@@ -12,26 +12,25 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef CONCEALER__SUBSCRIBER_WRAPPER_HPP_
-#define CONCEALER__SUBSCRIBER_WRAPPER_HPP_
+#ifndef CONCEALER__SUBSCRIBER_HPP_
+#define CONCEALER__SUBSCRIBER_HPP_
 
 #include <memory>
 #include <rclcpp/rclcpp.hpp>
 
 namespace concealer
 {
 template <typename Message>
-class SubscriberWrapper
+struct Subscriber
 {
   typename Message::ConstSharedPtr current_value = std::make_shared<const Message>();
 
   typename rclcpp::Subscription<Message>::SharedPtr subscription;
 
-public:
   auto operator()() const -> const auto & { return *std::atomic_load(&current_value); }
 
   template <typename Autoware, typename Callback>
-  explicit SubscriberWrapper(
+  explicit Subscriber(
     const std::string & topic, const rclcpp::QoS & quality_of_service, Autoware & autoware,
     const Callback & callback)
   : subscription(autoware.template create_subscription<Message>(
@@ -45,7 +44,7 @@ class SubscriberWrapper
   }
 
   template <typename Autoware>
-  explicit SubscriberWrapper(
+  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) {
@@ -56,4 +55,4 @@ class SubscriberWrapper
 };
 }  // namespace concealer
 
-#endif  // CONCEALER__SUBSCRIBER_WRAPPER_HPP_
+#endif  // CONCEALER__SUBSCRIBER_HPP_