chain can get from parameters

Author: brkay54

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -39,6 +39,7 @@
 #include <memory>
 #include <mutex>
 #include <utility>
+#include <variant>
 
 namespace reaction_analyzer
 {
@@ -54,12 +55,24 @@ using autoware_auto_planning_msgs::msg::Trajectory;
 using geometry_msgs::msg::Pose;
 using nav_msgs::msg::Odometry;
 using sensor_msgs::msg::PointCloud2;
+using ControlBuffer = std::pair<std::vector<AckermannControlCommand>, std::optional<AckermannControlCommand>>;
+
+using BufferVariant = std::variant<
+  ControlBuffer
+  // , std::vector<OtherMessageType> // Add other message types here
+  >;
+
+enum class MessageType
+{
+  Unknown = 0,
+  AckermannControlCommand = 1,
+};
 
 struct TopicConfig
 {
   std::string node_name;
   std::string topic_address;
-  std::string message_type;
+  MessageType message_type;
 };
 
 using ChainModules = std::vector<TopicConfig>;
@@ -96,24 +109,19 @@ struct NodeParams
   int min_number_descending_order_control_cmd;
 };
 
-struct ControlCommandWithSeenTime
-{
-  explicit ControlCommandWithSeenTime(
-    const rclcpp::Time & time, const AckermannControlCommand & cmd)
-  : seen_time(time), control_cmd(cmd)
-  {
-  }
-  rclcpp::Time seen_time;
-  AckermannControlCommand control_cmd;
-};
-
 class ReactionAnalyzerNode : public rclcpp::Node
 {
 public:
   explicit ReactionAnalyzerNode(rclcpp::NodeOptions options);
   ~ReactionAnalyzerNode() = default;
 
 private:
+  void initObservers(const ChainModules & modules);
+  std::vector<rclcpp::SubscriptionBase::SharedPtr> subscribers_;
+  void controlCommandOutputCallback(const std::string & node_name, const AckermannControlCommand::ConstSharedPtr msg_ptr);
+
+  std::unordered_map<std::string, BufferVariant> messageBuffers_;
+
   std::mutex mutex_;
   // Module list in chain
   ChainModules chain_modules;
@@ -135,10 +143,6 @@ class ReactionAnalyzerNode : public rclcpp::Node
   rclcpp::Subscription<LocalizationInitializationState>::SharedPtr sub_localization_init_state_;
   rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
 
-  // Observer Subscribers
-  rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_trajectory_follower_output;
-  rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_vehicle_cmd_gate_output;
-
   // Publishers
   rclcpp::Publisher<PointCloud2>::SharedPtr pub_pointcloud_;
   rclcpp::Publisher<PredictedObjects>::SharedPtr pub_predicted_objects_;
@@ -159,32 +163,28 @@ class ReactionAnalyzerNode : public rclcpp::Node
     RouteState::ConstSharedPtr route_state_ptr);
   void onTimer();
   void setControlCommandToBuffer(
-    std::vector<ControlCommandWithSeenTime> & buffer, const AckermannControlCommand & cmd);
+    std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd);
   std::optional<size_t> findFirstBrakeIdx(
-    const std::vector<ControlCommandWithSeenTime> & cmd_array);
+    const std::vector<AckermannControlCommand> & cmd_array);
 
   // Callbacks
   void vehiclePoseCallback(const Odometry::ConstSharedPtr msg_ptr);
   void initializationStateCallback(const LocalizationInitializationState::ConstSharedPtr msg_ptr);
   void routeStateCallback(const RouteState::ConstSharedPtr msg);
   void operationModeCallback(const OperationModeState::ConstSharedPtr msg_ptr);
 
-  // Observer Callbacks
-  void trajectoryFollowerOutputCallback(const AckermannControlCommand::ConstSharedPtr msg_ptr);
-  void vehicleCmdGateOutputCallback(const AckermannControlCommand::ConstSharedPtr msg_ptr);
-
   // Variables
   std::optional<rclcpp::Time> spawn_cmd_time;
   bool is_test_environment_created_{false};
   bool is_ego_initialized_{false};
   bool is_route_set_{false};
   bool is_output_printed{false};
+  bool all_topics_reacted_{false};
   std::optional<rclcpp::Time> last_initialize_test_time_;
+
   // measurement for control cmd
-  std::vector<ControlCommandWithSeenTime> trajectory_follower_output_buffer_;
-  std::vector<ControlCommandWithSeenTime> vehicle_cmd_gate_output_buffer_;
-  std::optional<ControlCommandWithSeenTime> trajectory_follower_brake_cmd;
-  std::optional<ControlCommandWithSeenTime> vehicle_cmd_gate_brake_cmd;
+//  std::optional<ControlCommandWithSeenTime> trajectory_follower_brake_cmd;
+//  std::optional<ControlCommandWithSeenTime> vehicle_cmd_gate_brake_cmd;
 
   // Timer
   rclcpp::TimerBase::SharedPtr timer_;