draft

draft

draft

Author: brkay54

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -29,7 +29,17 @@
 #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>
@@ -120,15 +130,107 @@ struct NodeParams
   bool run_from_bag;
 };
 
+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,
+};
 
-struct ConjugatePointclouds
+template <typename MessageType>
+struct PublisherVariables
 {
-  PointCloud2::SharedPtr empty_area_pointcloud;
-  PointCloud2::SharedPtr object_spawned_pointcloud;
-  rclcpp::Publisher<PointCloud2>::SharedPtr publisher;
+  double period_ns;
+  typename MessageType::SharedPtr empty_area_message;
+  typename MessageType::SharedPtr object_spawned_message;
+  typename rclcpp::Publisher<MessageType>::SharedPtr publisher;
   rclcpp::TimerBase::SharedPtr timer;
 };
 
+struct PublisherVarAccessor {
+
+  // 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;
+  }
+
+  // Set Empty Area Message
+  template <typename T, typename Message>
+  void setEmptyAreaMessage(T& publisherVar, typename Message::SharedPtr message) {
+    publisherVar.empty_area_message = message;
+  }
+
+  // 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);
+  }
+
+  // Set Object Spawned Message
+  template <typename T, typename Message>
+  void setObjectSpawnedMessage(T& publisherVar, typename Message::SharedPtr message) {
+    publisherVar.object_spawned_message = 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);
+  }
+
+  // Set Publisher
+  template <typename T, typename PublisherType>
+  void setPublisher(T& publisherVar, typename rclcpp::Publisher<PublisherType>::SharedPtr publisher) {
+    publisherVar.publisher = publisher;
+  }
+
+  // Get Publisher
+  template <typename T>
+  std::shared_ptr<void> getPublisher(const T& publisherVar) const {
+    return std::static_pointer_cast<void>(publisherVar.publisher);
+  }
+
+  // Set Timer
+  template <typename T>
+  void setTimer(T& publisherVar, rclcpp::TimerBase::SharedPtr newTimer) {
+    publisherVar.timer = newTimer;
+  }
+
+  // Get Timer
+  template <typename T>
+  std::shared_ptr<void> getTimer(const T& publisherVar) const {
+    return std::static_pointer_cast<void>(publisherVar.timer);
+  }
+};
+
+
+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:
@@ -147,11 +249,8 @@ class ReactionAnalyzerNode : public rclcpp::Node
   double entity_search_radius_;
 
   // TEMP
-  void genericPointcloudPublisherTimer(const std::string & topic_name);
-  std::vector<rclcpp::TimerBase::SharedPtr> pointcloud_pub_timers_;
-  std::unordered_map<std::string, ConjugatePointclouds> pointclouds_map_;
-
-
+  void genericMessagePublisherTimer(const std::string & topic_name);
+  std::unordered_map<std::string, PublisherVariablesVariant> topic_publisher_map_;
 
   // Initialization Variables
   geometry_msgs::msg::Pose entity_pose_;