fix: ci/cd

Signed-off-by: Berkay Karaman <[email protected]>

tools/reaction_analyzer/CMakeLists.txt

@@ -33,9 +33,6 @@ rclcpp_components_register_node(reaction_analyzer
   EXECUTABLE reaction_analyzer_exe
 )
 
-if (BUILD_TESTING)
-endif ()
-
 ament_auto_package(
   INSTALL_TO_SHARE
   param

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -28,10 +28,10 @@
 #include <nav_msgs/msg/odometry.hpp>
 
 #include <algorithm>
+#include <map>
 #include <memory>
 #include <mutex>
 #include <string>
-#include <unordered_map>
 #include <utility>
 #include <variant>
 #include <vector>
@@ -66,8 +66,7 @@ struct NodeParams
 class ReactionAnalyzerNode : public rclcpp::Node
 {
 public:
-  explicit ReactionAnalyzerNode(rclcpp::NodeOptions options);
-  ~ReactionAnalyzerNode() = default;
+  explicit ReactionAnalyzerNode(rclcpp::NodeOptions node_options);
 
   Odometry::ConstSharedPtr odometry_ptr_;
 
@@ -107,16 +106,24 @@ class ReactionAnalyzerNode : public rclcpp::Node
 
   // Functions
   void init_analyzer_variables();
-  void init_ego(
+  void init_test_env(
     const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr,
     const RouteState::ConstSharedPtr & route_state_ptr,
     const OperationModeState::ConstSharedPtr & operation_mode_ptr,
     const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr,
     const Odometry::ConstSharedPtr & odometry_ptr);
+  bool init_ego(
+    const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr,
+    const Odometry::ConstSharedPtr & odometry_ptr, const rclcpp::Time & current_time);
+  bool set_route(
+    const RouteState::ConstSharedPtr & route_state_ptr, const rclcpp::Time & current_time);
+  bool check_ego_init_correctly(
+    const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr,
+    const Odometry::ConstSharedPtr & odometry_ptr);
   void call_operation_mode_service_without_response();
   void spawn_obstacle(const geometry_msgs::msg::Point & ego_pose);
   void calculate_results(
-    const std::unordered_map<std::string, subscriber::MessageBufferVariant> & message_buffers,
+    const std::map<std::string, subscriber::MessageBufferVariant> & message_buffers,
     const rclcpp::Time & spawn_cmd_time);
   void on_timer();
   void reset();

tools/reaction_analyzer/include/subscriber.hpp

@@ -29,10 +29,10 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
+#include <map>
 #include <memory>
 #include <mutex>
 #include <string>
-#include <unordered_map>
 #include <utility>
 #include <variant>
 #include <vector>
@@ -77,25 +77,6 @@ using SubscriberVariablesVariant = std::variant<
   SubscriberVariables<TrackedObjects>, SubscriberVariables<PredictedObjects>,
   SubscriberVariables<Trajectory>, SubscriberVariables<AckermannControlCommand>>;
 
-// The supported message types
-enum class SubscriberMessageType {
-  UNKNOWN = 0,
-  ACKERMANN_CONTROL_COMMAND = 1,
-  TRAJECTORY = 2,
-  POINTCLOUD2 = 3,
-  DETECTED_OBJECTS = 4,
-  PREDICTED_OBJECTS = 5,
-  TRACKED_OBJECTS = 6,
-};
-
-// Reaction Types
-enum class ReactionType {
-  UNKNOWN = 0,
-  FIRST_BRAKE = 1,
-  SEARCH_ZERO_VEL = 2,
-  SEARCH_ENTITY = 3,
-};
-
 // The configuration of the topic to be subscribed which are defined in reaction_chain
 struct TopicConfig
 {
@@ -141,13 +122,7 @@ class SubscriberBase
     rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, std::atomic<bool> & spawn_object_cmd,
     const EntityParams & entity_params);
 
-  // Instances of SubscriberBase cannot be copied
-  SubscriberBase(const SubscriberBase &) = delete;
-  SubscriberBase & operator=(const SubscriberBase &) = delete;
-
-  ~SubscriberBase() = default;
-
-  std::optional<std::unordered_map<std::string, MessageBufferVariant>> getMessageBuffersMap();
+  std::optional<std::map<std::string, MessageBufferVariant>> get_message_buffers_map();
   void reset();
 
 private:
@@ -160,14 +135,14 @@ class SubscriberBase
   EntityParams entity_params_;
 
   // Variables to be initialized in constructor
-  ChainModules chain_modules_;
+  ChainModules chain_modules_{};
   ReactionParams reaction_params_{};
-  geometry_msgs::msg::Pose entity_pose_;
-  double entity_search_radius_;
+  geometry_msgs::msg::Pose entity_pose_{};
+  double entity_search_radius_{0.0};
 
   // Variants
-  std::unordered_map<std::string, SubscriberVariablesVariant> subscriber_variables_map_;
-  std::unordered_map<std::string, MessageBufferVariant> message_buffers_;
+  std::map<std::string, SubscriberVariablesVariant> subscriber_variables_map_;
+  std::map<std::string, MessageBufferVariant> message_buffers_;
 
   // tf
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
@@ -176,10 +151,12 @@ class SubscriberBase
   // Functions
   void init_reaction_chains_and_params();
   bool init_subscribers();
+  std::optional<SubscriberVariablesVariant> get_subscriber_variable(
+    const TopicConfig & topic_config);
   std::optional<size_t> find_first_brake_idx(
     const std::vector<AckermannControlCommand> & cmd_array);
   void set_control_command_to_buffer(
-    std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd);
+    std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd) const;
 
   // Callbacks for modules are subscribed
   void on_control_command(

tools/reaction_analyzer/include/topic_publisher.hpp

@@ -35,10 +35,10 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
+#include <map>
 #include <memory>
 #include <mutex>
 #include <string>
-#include <unordered_map>
 #include <utility>
 #include <variant>
 #include <vector>
@@ -55,23 +55,6 @@ using geometry_msgs::msg::Pose;
 using nav_msgs::msg::Odometry;
 using sensor_msgs::msg::PointCloud2;
 
-enum class PublisherMessageType {
-  UNKNOWN = 0,
-  CAMERA_INFO = 1,
-  IMAGE = 2,
-  POINTCLOUD2 = 3,
-  POSE_WITH_COVARIANCE_STAMPED = 4,
-  POSE_STAMPED = 5,
-  ODOMETRY = 6,
-  IMU = 7,
-  CONTROL_MODE_REPORT = 8,
-  GEAR_REPORT = 9,
-  HAZARD_LIGHTS_REPORT = 10,
-  STEERING_REPORT = 11,
-  TURN_INDICATORS_REPORT = 12,
-  VELOCITY_REPORT = 13,
-};
-
 struct TopicPublisherParams
 {
   double dummy_perception_publisher_period;  // Only for planning_control mode
@@ -110,68 +93,68 @@ struct PublisherVarAccessor
 {
   // Template struct to check if a type has a header member.
   template <typename T, typename = std::void_t<>>
-  struct has_header : std::false_type
+  struct HasHeader : std::false_type
   {
   };
 
   template <typename T>
-  struct has_header<T, std::void_t<decltype(T::header)>> : std::true_type
+  struct HasHeader<T, std::void_t<decltype(T::header)>> : std::true_type
   {
   };
 
   // Template struct to check if a type has a stamp member.
   template <typename T, typename = std::void_t<>>
-  struct has_stamp : std::false_type
+  struct HasStamp : std::false_type
   {
   };
 
   template <typename T>
-  struct has_stamp<T, std::void_t<decltype(T::stamp)>> : std::true_type
+  struct HasStamp<T, std::void_t<decltype(T::stamp)>> : std::true_type
   {
   };
 
-  template <typename MessageType>
+  template <typename T>
   void publish_with_current_time(
-    const PublisherVariables<MessageType> & publisherVar, const rclcpp::Time & current_time,
+    const PublisherVariables<T> & publisher_var, const rclcpp::Time & current_time,
     const bool is_object_spawned) const
   {
-    std::unique_ptr<MessageType> msg_to_be_published = std::make_unique<MessageType>();
+    std::unique_ptr<T> msg_to_be_published = std::make_unique<T>();
 
     if (is_object_spawned) {
-      *msg_to_be_published = *publisherVar.object_spawned_message;
+      *msg_to_be_published = *publisher_var.object_spawned_message;
     } else {
-      *msg_to_be_published = *publisherVar.empty_area_message;
+      *msg_to_be_published = *publisher_var.empty_area_message;
     }
-    if constexpr (has_header<MessageType>::value) {
+    if constexpr (HasHeader<T>::value) {
       msg_to_be_published->header.stamp = current_time;
-    } else if constexpr (has_stamp<MessageType>::value) {
+    } else if constexpr (HasStamp<T>::value) {
       msg_to_be_published->stamp = current_time;
     }
-    publisherVar.publisher->publish(std::move(msg_to_be_published));
+    publisher_var.publisher->publish(std::move(msg_to_be_published));
   }
 
   template <typename T>
-  void set_period(T & publisherVar, std::chrono::milliseconds new_period)
+  void set_period(T & publisher_var, std::chrono::milliseconds new_period)
   {
-    publisherVar.period_ms = new_period;
+    publisher_var.period_ms = new_period;
   }
 
   template <typename T>
-  std::chrono::milliseconds get_period(const T & publisherVar) const
+  std::chrono::milliseconds get_period(const T & publisher_var) const
   {
-    return publisherVar.period_ms;
+    return publisher_var.period_ms;
   }
 
   template <typename T>
-  std::shared_ptr<void> get_empty_area_message(const T & publisherVar) const
+  std::shared_ptr<void> get_empty_area_message(const T & publisher_var) const
   {
-    return std::static_pointer_cast<void>(publisherVar.empty_area_message);
+    return std::static_pointer_cast<void>(publisher_var.empty_area_message);
   }
 
   template <typename T>
-  std::shared_ptr<void> get_object_spawned_message(const T & publisherVar) const
+  std::shared_ptr<void> get_object_spawned_message(const T & publisher_var) const
   {
-    return std::static_pointer_cast<void>(publisherVar.object_spawned_message);
+    return std::static_pointer_cast<void>(publisher_var.object_spawned_message);
   }
 };
 
@@ -200,7 +183,6 @@ class TopicPublisher
     std::optional<rclcpp::Time> & spawn_cmd_time, const RunningMode & node_running_mode,
     const EntityParams & entity_params);
 
-  ~TopicPublisher() = default;
   void reset();
 
 private:
@@ -229,21 +211,25 @@ class TopicPublisher
 
   // Variables perception_planning mode
   PointcloudPublisherType pointcloud_publisher_type_;
-  std::unordered_map<std::string, PublisherVariablesVariant> topic_publisher_map_;
-  std::unordered_map<std::string, LidarOutputPair>
+  std::map<std::string, PublisherVariablesVariant> topic_publisher_map_;
+  std::map<std::string, LidarOutputPair>
     lidar_pub_variable_pair_map_;  // used to publish pointcloud_raw and pointcloud_raw_ex
   bool is_object_spawned_message_published_{false};
   std::shared_ptr<rclcpp::TimerBase> one_shot_timer_shared_ptr_;
 
   // Functions
-  void set_message_to_variable_map(
-    const PublisherMessageType & message_type, const std::string & topic_name,
-    rosbag2_storage::SerializedBagMessage & bag_message, const bool is_empty_area_message);
-  void set_period_to_variable_map(
-    const std::unordered_map<std::string, std::vector<rclcpp::Time>> & time_map);
-  bool set_publishers_and_timers_to_variable_map();
+  template <typename MessageType>
+  void set_message(
+    const std::string & topic_name, const rosbag2_storage::SerializedBagMessage & bag_message,
+    const bool is_empty_area_message);
+  void set_period(const std::map<std::string, std::vector<rclcpp::Time>> & time_map);
+  std::map<std::string, PublisherVariables<PointCloud2>> set_general_publishers_and_timers();
+  void set_pointcloud_publishers_and_timers(
+    const std::map<std::string, PublisherVariables<PointCloud2>> & pointcloud_variables_map);
   bool check_publishers_initialized_correctly();
   void init_rosbag_publishers();
+  void init_rosbag_publishers_with_object(const std::string & path_bag_with_object);
+  void init_rosbag_publishers_without_object(const std::string & path_bag_without_object);
   void pointcloud_messages_sync_publisher(const PointcloudPublisherType type);
   void pointcloud_messages_async_publisher(
     const std::pair<
@@ -253,7 +239,7 @@ class TopicPublisher
   void dummy_perception_publisher();  // Only for planning_control mode
 
   // Timers
-  std::unordered_map<std::string, rclcpp::TimerBase::SharedPtr> pointcloud_publish_timers_map_;
+  std::map<std::string, rclcpp::TimerBase::SharedPtr> pointcloud_publish_timers_map_;
   rclcpp::TimerBase::SharedPtr pointcloud_sync_publish_timer_;
   rclcpp::TimerBase::SharedPtr dummy_perception_timer_;
 };