structure change and add reset function

Signed-off-by: Berkay Karaman <brkay54@gmail.com>

Author: brkay54

tools/reaction_analyzer/CMakeLists.txt

@@ -10,6 +10,8 @@ find_package(Eigen3 REQUIRED)
 ament_auto_add_library(reaction_analyzer SHARED
   include/reaction_analyzer_node.hpp
   src/reaction_analyzer_node.cpp
+  include/topic_publisher.hpp
+  src/topic_publisher.cpp
 )
 
 target_include_directories(reaction_analyzer

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -20,6 +20,7 @@
 #include <rosbag2_cpp/reader.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/math/unit_conversion.hpp>
+#include <topic_publisher.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
@@ -29,18 +30,8 @@
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
-#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
-#include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
-#include <autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp>
-#include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
-#include <autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp>
-#include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
-#include <sensor_msgs/msg/camera_info.hpp>
-#include <sensor_msgs/msg/image.hpp>
-#include <sensor_msgs/msg/imu.hpp>
-#include <sensor_msgs/msg/point_cloud2.hpp>
 
 #include <pcl/common/distances.h>
 #include <pcl/point_types.h>
@@ -75,6 +66,7 @@ using TrajectoryBuffer = std::optional<Trajectory>;
 using PointCloud2Buffer = std::optional<PointCloud2>;
 using PredictedObjectsBuffer = std::optional<PredictedObjects>;
 using DetectedObjectsBuffer = std::optional<DetectedObjects>;
+
 using BufferVariant = std::variant<
   ControlCommandBuffer, TrajectoryBuffer, PointCloud2Buffer, PredictedObjectsBuffer,
   DetectedObjectsBuffer>;
@@ -88,21 +80,6 @@ enum class SubscriberMessageType {
   PredictedObjects = 5,
 };
 
-enum class PublisherMessageType {
-  Unknown = 0,
-  CameraInfo = 1,
-  Image = 2,
-  PointCloud2 = 3,
-  PoseWithCovarianceStamped = 4,
-  Imu = 5,
-  ControlModeReport = 6,
-  GearReport = 7,
-  HazardLightsReport = 8,
-  SteeringReport = 9,
-  TurnIndicatorsReport = 10,
-  VelocityReport = 11,
-};
-
 enum class RunningMode {
   PerceptionPlanning = 0,
   PlanningControl = 1,
@@ -140,124 +117,23 @@ struct EntityParams
   double z_l;
 };
 
-struct GeneralParams
+struct NodeParams
 {
   std::string running_mode;
   double timer_period;
   double object_search_radius_offset;
-  EntityParams entity_params;
-  PoseParams goal_pose;
-};
-
-struct PerceptionPlanningModeParams
-{
-  std::string path_bag_without_object;
-  std::string path_bag_with_object;
-  bool publish_pointcloud_synchronically;
-  double synchronic_pointcloud_publishing_period;
   double spawn_time_after_init;
-};
-
-struct PlanningControlModeParams
-{
   double spawn_distance_threshold;
   double spawned_pointcloud_sampling_distance;
+  double dummy_perception_publisher_period;
   double control_cmd_buffer_time_interval;
   double min_jerk_for_brake_cmd;
   size_t min_number_descending_order_control_cmd;
   PoseParams initial_pose;
+  PoseParams goal_pose;
+  EntityParams entity_params;
 };
 
-template <typename MessageType>
-struct PublisherVariables
-{
-  double period_ns{0.0};
-  typename MessageType::SharedPtr empty_area_message;
-  typename MessageType::SharedPtr object_spawned_message;
-  typename rclcpp::Publisher<MessageType>::SharedPtr publisher;
-  rclcpp::TimerBase::SharedPtr timer;
-};
-
-struct PublisherVarAccessor
-{
-  template <typename T, typename = std::void_t<>>
-  struct has_header : std::false_type
-  {
-  };
-
-  template <typename T>
-  struct has_header<T, std::void_t<decltype(T::header)>> : std::true_type
-  {
-  };
-  template <typename T, typename = std::void_t<>>
-  struct has_stamp : std::false_type
-  {
-  };
-
-  template <typename T>
-  struct has_stamp<T, std::void_t<decltype(T::stamp)>> : std::true_type
-  {
-  };
-  template <typename MessageType>
-  void publishWithCurrentTime(
-    const PublisherVariables<MessageType> & publisherVar, const rclcpp::Time & current_time,
-    const bool is_object_spawned) const
-  {
-    std::unique_ptr<MessageType> msg_to_be_published = std::make_unique<MessageType>();
-
-    if (is_object_spawned) {
-      *msg_to_be_published = *publisherVar.object_spawned_message;
-    } else {
-      *msg_to_be_published = *publisherVar.empty_area_message;
-    }
-    if constexpr (has_header<MessageType>::value) {
-      msg_to_be_published->header.stamp = current_time;
-    } else if constexpr (has_stamp<MessageType>::value) {
-      msg_to_be_published->stamp = current_time;
-    }
-    publisherVar.publisher->publish(std::move(msg_to_be_published));
-  }
-  // Set Period
-  template <typename T>
-  void setPeriod(T & publisherVar, double newPeriod)
-  {
-    publisherVar.period_ns = newPeriod;
-  }
-
-  // Get Period
-  template <typename T>
-  double getPeriod(const T & publisherVar) const
-  {
-    return publisherVar.period_ns;
-  }
-
-  // Get Empty Area Message
-  template <typename T>
-  std::shared_ptr<void> getEmptyAreaMessage(const T & publisherVar) const
-  {
-    return std::static_pointer_cast<void>(publisherVar.empty_area_message);
-  }
-
-  // Get Object Spawned Message
-  template <typename T>
-  std::shared_ptr<void> getObjectSpawnedMessage(const T & publisherVar) const
-  {
-    return std::static_pointer_cast<void>(publisherVar.object_spawned_message);
-  }
-};
-
-using PublisherVariablesVariant = std::variant<
-  PublisherVariables<PointCloud2>, PublisherVariables<sensor_msgs::msg::CameraInfo>,
-  PublisherVariables<sensor_msgs::msg::Image>,
-  PublisherVariables<geometry_msgs::msg::PoseWithCovarianceStamped>,
-  PublisherVariables<sensor_msgs::msg::Imu>,
-  PublisherVariables<autoware_auto_vehicle_msgs::msg::ControlModeReport>,
-  PublisherVariables<autoware_auto_vehicle_msgs::msg::GearReport>,
-  PublisherVariables<autoware_auto_vehicle_msgs::msg::HazardLightsReport>,
-  PublisherVariables<autoware_auto_vehicle_msgs::msg::SteeringReport>,
-  PublisherVariables<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>,
-  PublisherVariables<autoware_auto_vehicle_msgs::msg::VelocityReport>>;
-
 class ReactionAnalyzerNode : public rclcpp::Node
 {
 public:
@@ -270,17 +146,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
   RunningMode node_running_mode_;
 
   // Parameters
-  GeneralParams general_params_;
-  PerceptionPlanningModeParams perception_planning_mode_params_;
-  PlanningControlModeParams planning_control_mode_params_;
-
-  // TEMP
-  void genericMessagePublisher(const std::string & topic_name);
-  std::unordered_map<std::string, PublisherVariablesVariant> topic_publisher_map_;
-  std::vector<PublisherVariables<PointCloud2>> sync_pointcloud_publishers_;
-  void syncPointcloudPublisher();
-  rclcpp::TimerBase::SharedPtr sync_pointcloud_publish_timer_;
-  void initRosbagPublishers();
+  NodeParams node_params_;
 
   // Initialization Variables
   geometry_msgs::msg::Pose entity_pose_;
@@ -324,8 +190,9 @@ class ReactionAnalyzerNode : public rclcpp::Node
   void initPredictedObjects();
 
   void initEgoForTest(
-    LocalizationInitializationState::ConstSharedPtr initialization_state_ptr,
-    RouteState::ConstSharedPtr route_state_ptr);
+    const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr,
+    const RouteState::ConstSharedPtr & route_state_ptr,
+    const OperationModeState::ConstSharedPtr & operation_mode_ptr);
 
   void setControlCommandToBuffer(
     std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd);
@@ -334,13 +201,16 @@ class ReactionAnalyzerNode : public rclcpp::Node
     const std::vector<AckermannControlCommand> & cmd_array,
     const std::optional<rclcpp::Time> & spawn_cmd_time);
 
-  void spawnObstacle(
-    const geometry_msgs::msg::Point & ego_pose, std::optional<rclcpp::Time> & spawn_cmd_time);
+  void spawnObstacle(const geometry_msgs::msg::Point & ego_pose);
 
   void printResults(const std::unordered_map<std::string, BufferVariant> & message_buffers);
 
   void onTimer();
 
+  void dummyPerceptionPublisher();
+
+  void reset();
+
   // Callbacks
   void vehiclePoseCallback(Odometry::ConstSharedPtr msg_ptr);
 
@@ -350,7 +220,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
 
   void operationModeCallback(OperationModeState::ConstSharedPtr msg_ptr);
 
-  // Callbacks for modules are observed
+  // Callbacks for modules are subscribed
   void controlCommandOutputCallback(
     const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr);
 
@@ -369,23 +239,26 @@ class ReactionAnalyzerNode : public rclcpp::Node
   // Variables
   std::vector<rclcpp::SubscriptionBase::SharedPtr> subscribers_;
   std::unordered_map<std::string, BufferVariant> message_buffers_;
-  std::optional<rclcpp::Time> last_test_environment_init_time_;
+  std::optional<rclcpp::Time> last_test_environment_init_request_time_;
+  std::optional<rclcpp::Time> test_environment_init_time_;
   std::optional<rclcpp::Time> spawn_cmd_time_;
-  std::atomic<bool> is_object_spawned_{false};
-  bool is_test_environment_created_{false};
-  bool is_ego_initialized_{false};
-  bool is_route_set_{false};
+  std::atomic<bool> spawn_object_cmd_{false};
+  std::atomic<bool> is_object_spawned_message_published_{false};
   bool is_output_printed_{false};
-
+  bool is_vehicle_initialized_{false};
+  bool is_route_set_{false};
   // Timer
   rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::TimerBase::SharedPtr dummy_perception_timer_;
 
   // Client
   rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_autonomous_;
 
   void callOperationModeServiceWithoutResponse();
 
   // Pointers
+  std::shared_ptr<topic_publisher::TopicPublisher> topic_publisher_ptr_;
+
   PointCloud2::SharedPtr entity_pointcloud_ptr_;
   PredictedObjects::SharedPtr predicted_objects_ptr_;
   Odometry::ConstSharedPtr odometry_;