Merge pull request #1488 from tier4/refactor/concealer-1

Refactor/concealer 1

Author: yamacir-kit

external/concealer/CMakeLists.txt

@@ -14,11 +14,9 @@ find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/autoware.cpp
   src/autoware_universe.cpp
   src/execute.cpp
   src/field_operator_application.cpp
-  src/field_operator_application_for_autoware_universe.cpp
   src/is_package_exists.cpp
   src/task_queue.cpp)
 

external/concealer/include/concealer/autoware.hpp

@@ -15,57 +15,68 @@
 #ifndef CONCEALER__AUTOWARE_UNIVERSE_HPP_
 #define CONCEALER__AUTOWARE_UNIVERSE_HPP_
 
+#include <atomic>
 #include <autoware_control_msgs/msg/control.hpp>
 #include <autoware_vehicle_msgs/msg/control_mode_report.hpp>
+#include <autoware_vehicle_msgs/msg/gear_command.hpp>
 #include <autoware_vehicle_msgs/msg/gear_report.hpp>
 #include <autoware_vehicle_msgs/msg/steering_report.hpp>
+#include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
 #include <autoware_vehicle_msgs/msg/turn_indicators_report.hpp>
 #include <autoware_vehicle_msgs/msg/velocity_report.hpp>
 #include <autoware_vehicle_msgs/srv/control_mode_command.hpp>
-#include <concealer/autoware.hpp>
+#include <concealer/continuous_transform_broadcaster.hpp>
 #include <concealer/publisher_wrapper.hpp>
 #include <concealer/subscriber_wrapper.hpp>
+#include <concealer/visibility.hpp>
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#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
 {
-/*
- * Implements Autoware interface for Autoware Universe
- * NOTE: This class is intended to be move to simple_sensor_simulator
- */
-class AutowareUniverse : public Autoware
+class AutowareUniverse : public rclcpp::Node,
+                         public ContinuousTransformBroadcaster<AutowareUniverse>
 {
+public:
   // clang-format off
   using AccelWithCovarianceStamped  = geometry_msgs::msg::AccelWithCovarianceStamped;
   using Control                     = autoware_control_msgs::msg::Control;
   using ControlModeCommand          = autoware_vehicle_msgs::srv::ControlModeCommand;
   using ControlModeReport           = autoware_vehicle_msgs::msg::ControlModeReport;
   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;
 
-  SubscriberWrapper<Control,                ThreadSafety::safe> getCommand;
-  SubscriberWrapper<GearCommand,            ThreadSafety::safe> getGearCommandImpl;
-  SubscriberWrapper<TurnIndicatorsCommand,  ThreadSafety::safe> getTurnIndicatorsCommand;
-  SubscriberWrapper<PathWithLaneId,         ThreadSafety::safe> getPathWithLaneId;
+  SubscriberWrapper<Control>               getCommand;
+  SubscriberWrapper<GearCommand>           getGearCommand;
+  SubscriberWrapper<TurnIndicatorsCommand> getTurnIndicatorsCommand;
+  SubscriberWrapper<PathWithLaneId>        getPathWithLaneId;
 
   PublisherWrapper<AccelWithCovarianceStamped> setAcceleration;
-  PublisherWrapper<nav_msgs::msg::Odometry>    setOdometry;
+  PublisherWrapper<Odometry>                   setOdometry;
   PublisherWrapper<PoseWithCovarianceStamped>  setPose;
   PublisherWrapper<SteeringReport>             setSteeringReport;
   PublisherWrapper<GearReport>                 setGearReport;
   PublisherWrapper<ControlModeReport>          setControlModeReport;
   PublisherWrapper<VelocityReport>             setVelocityReport;
   PublisherWrapper<TurnIndicatorsReport>       setTurnIndicatorsReport;
+
+  std::atomic<geometry_msgs::msg::Accel> current_acceleration;
+  std::atomic<geometry_msgs::msg::Pose>  current_pose;
+  std::atomic<geometry_msgs::msg::Twist> current_twist;
   // clang-format on
 
+private:
   rclcpp::Service<ControlModeCommand>::SharedPtr control_mode_request_server;
 
   const rclcpp::TimerBase::SharedPtr localization_update_timer;
@@ -82,38 +93,25 @@ class AutowareUniverse : public Autoware
 
   std::exception_ptr thrown;
 
-  auto stopAndJoin() -> void;
-
 public:
   CONCEALER_PUBLIC explicit AutowareUniverse(bool);
 
   ~AutowareUniverse();
 
-  auto rethrow() -> void override;
-
-  auto getAcceleration() const -> double override;
-
-  auto getSteeringAngle() const -> double override;
-
-  auto getVelocity() const -> double override;
+  auto rethrow() -> void;
 
   auto updateLocalization() -> void;
 
   auto updateVehicleState() -> void;
 
-  auto getGearCommand() const -> GearCommand override;
-
-  auto getGearSign() const -> double override;
+  auto getVehicleCommand() const -> std::tuple<double, double, double, double, int>;
 
-  auto getVehicleCommand() const -> std::tuple<Control, GearCommand> override;
+  auto getRouteLanelets() const -> std::vector<std::int64_t>;
 
-  auto getRouteLanelets() const -> std::vector<std::int64_t> override;
+  auto getControlModeReport() const -> ControlModeReport;
 
-  auto getControlModeReport() const -> ControlModeReport override;
-
-  auto setManualMode() -> void override;
+  auto setManualMode() -> void;
 };
-
 }  // namespace concealer
 
 #endif  // CONCEALER__AUTOWARE_UNIVERSE_HPP_

external/concealer/include/concealer/autoware_stream.hpp

@@ -24,8 +24,6 @@ auto execute(const std::vector<std::string> &) -> int;
 
 // Emulates shell's $(...) expression.
 auto dollar(const std::string & command) -> std::string;
-
-void sudokill(const pid_t process_id);
 }  // namespace concealer
 
 #endif  // CONCEALER__EXECUTE_HPP_