draft

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

Author: brkay54

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -171,48 +171,52 @@ struct PlanningControlModeParams
 template <typename MessageType>
 struct PublisherVariables
 {
-  double period_ns;
+  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;
 };
 
-template <typename T>
-struct PublisherVariablesMessageTypeExtractor;
-
-template <typename MessageType>
-struct PublisherVariablesMessageTypeExtractor<PublisherVariables<MessageType>>
-{
-  using Type = MessageType;
-};
-
-template <typename T, typename = std::void_t<>>
-struct has_header : std::false_type
+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>
+  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
+  {
+  };
 
-struct PublisherVarAccessor
-{
-  // Method to set header time if available
   template <typename T>
-  void setHeaderTimeIfAvailable(T & publisherVar, const rclcpp::Time & time)
+  struct has_stamp<T, std::void_t<decltype(T::stamp)>> : std::true_type
   {
-    if constexpr (has_header<typename T::MessageType>::value) {
-      if (publisherVar.empty_area_message) {
-        publisherVar.empty_area_message->header.stamp = time;
-      }
-      if (publisherVar.object_spawned_message) {
-        publisherVar.object_spawned_message->header.stamp = time;
-      }
+  };
+  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)
@@ -227,62 +231,19 @@ struct PublisherVarAccessor
     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<
@@ -410,6 +371,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
   std::unordered_map<std::string, BufferVariant> message_buffers_;
   std::optional<rclcpp::Time> last_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};

tools/reaction_analyzer/launch/reaction_analyzer.launch.xml

@@ -7,23 +7,23 @@
   <arg name="vehicle_model" default="sample_vehicle" description="vehicle model name"/>
   <arg name="sensor_model" default="sample_sensor_kit" description="sensor model name"/>
 
-  <group if="$(eval &quot;'$(var running_mode)'=='perception_planning'&quot;)">
-    <include file="$(find-pkg-share autoware_launch)/launch/e2e_simulator.launch.xml">
-      <arg name="map_path" value="$(var map_path)"/>
-      <arg name="vehicle_model" value="$(var vehicle_model)"/>
-      <arg name="sensor_model" value="$(var sensor_model)"/>
-      <arg name="use_sim_time" value="false"/>
-      <arg name="enable_image_decompressor" value="false"/>
-    </include>
-  </group>
-  <group if="$(eval &quot;'$(var running_mode)'=='planning_control'&quot;)">
-    <include file="$(find-pkg-share autoware_launch)/launch/planning_simulator.launch.xml">
-      <arg name="map_path" value="$(var map_path)"/>
-      <arg name="vehicle_model" value="$(var vehicle_model)"/>
-      <arg name="sensor_model" value="$(var sensor_model)"/>
-      <arg name="use_sim_time" value="false"/>
-    </include>
-  </group>
+<!--  <group if="$(eval &quot;'$(var running_mode)'=='perception_planning'&quot;)">-->
+<!--    <include file="$(find-pkg-share autoware_launch)/launch/e2e_simulator.launch.xml">-->
+<!--      <arg name="map_path" value="$(var map_path)"/>-->
+<!--      <arg name="vehicle_model" value="$(var vehicle_model)"/>-->
+<!--      <arg name="sensor_model" value="$(var sensor_model)"/>-->
+<!--      <arg name="use_sim_time" value="false"/>-->
+<!--      <arg name="enable_image_decompressor" value="false"/>-->
+<!--    </include>-->
+<!--  </group>-->
+<!--  <group if="$(eval &quot;'$(var running_mode)'=='planning_control'&quot;)">-->
+<!--    <include file="$(find-pkg-share autoware_launch)/launch/planning_simulator.launch.xml">-->
+<!--      <arg name="map_path" value="$(var map_path)"/>-->
+<!--      <arg name="vehicle_model" value="$(var vehicle_model)"/>-->
+<!--      <arg name="sensor_model" value="$(var sensor_model)"/>-->
+<!--      <arg name="use_sim_time" value="false"/>-->
+<!--    </include>-->
+<!--  </group>-->
 
   <node_container pkg="rclcpp_components" exec="component_container_mt" name="reaction_analyzer_container" namespace="" args="" output="screen">
     <composable_node pkg="reaction_analyzer" plugin="reaction_analyzer::ReactionAnalyzerNode" name="reaction_analyzer" namespace="">

tools/reaction_analyzer/param/reaction_analyzer.param.yaml

@@ -40,7 +40,7 @@
     perception_planning:
       path_bag_without_object: /home/berkay/projects/bags/awsim-bag/bag/lexus_bag_without_car/rosbag2_2024_01_30-13_50_45_0.db3
       path_bag_with_object: /home/berkay/projects/bags/awsim-bag/bag/lexus_bag_with_car/rosbag2_2024_01_30-13_50_17_0.db3
-      publish_pointcloud_synchronically: false
+      publish_pointcloud_synchronically: true
       synchronic_pointcloud_publishing_period: 0.1 # s only used if publish_pointcloud_synchronically is true
       spawn_time_after_init: 10.0 # s
     planning_control: