PredictedObjects msg type added

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -61,7 +61,9 @@ using ControlCommandBuffer =
   std::pair<std::vector<AckermannControlCommand>, std::optional<AckermannControlCommand>>;
 using TrajectoryBuffer = std::optional<Trajectory>;
 using PointCloud2Buffer = std::optional<PointCloud2>;
-using BufferVariant = std::variant<ControlCommandBuffer, TrajectoryBuffer, PointCloud2Buffer>;
+using PredictedObjectsBuffer = std::optional<PredictedObjects>;
+using BufferVariant =
+  std::variant<ControlCommandBuffer, TrajectoryBuffer, PointCloud2Buffer, PredictedObjectsBuffer>;
 
 enum class MessageType {
   Unknown = 0,
@@ -156,6 +158,8 @@ class ReactionAnalyzerNode : public rclcpp::Node
 
   bool searchPointcloudNearEntity(const pcl::PointCloud<pcl::PointXYZ> & pcl_pointcloud);
 
+  bool searchPredictedObjectsNearEntity(const PredictedObjects & predicted_objects);
+
   bool loadChainModules();
 
   bool initObservers(const ChainModules & modules);
@@ -207,6 +211,9 @@ class ReactionAnalyzerNode : public rclcpp::Node
   void pointcloud2OutputCallback(
     const std::string & node_name, const PointCloud2::ConstSharedPtr msg_ptr);
 
+  void predictedObjectsOutputCallback(
+    const std::string & node_name, const PredictedObjects::ConstSharedPtr msg_ptr);
+
   // Variables
   std::vector<rclcpp::SubscriptionBase::SharedPtr> subscribers_;
   std::unordered_map<std::string, BufferVariant> message_buffers_;

tools/reaction_analyzer/param/reaction_analyzer.param.yaml

@@ -53,3 +53,6 @@
       pointcloud_topic:
         topic_name: /perception/obstacle_segmentation/pointcloud
         message_type: sensor_msgs::msg::PointCloud2
+      predicted_objects_topic:
+        topic_name: /perception/object_recognition/objects
+        message_type: autoware_auto_perception_msgs::msg::PredictedObjects

tools/reaction_analyzer/src/reaction_analyzer_node.cpp

@@ -150,6 +150,31 @@ void ReactionAnalyzerNode::pointcloud2OutputCallback(
   mutex_.unlock();
 }
 
+void ReactionAnalyzerNode::predictedObjectsOutputCallback(
+  const std::string & node_name, const PredictedObjects::ConstSharedPtr msg_ptr)
+{
+  mutex_.lock();
+  auto variant = message_buffers_[node_name];
+  //  const auto is_spawned = spawn_cmd_time_;
+  mutex_.unlock();
+
+  if (!std::holds_alternative<PredictedObjectsBuffer>(variant)) {
+    // If the variant doesn't hold a Trajectory message yet, initialize it
+    PredictedObjectsBuffer buffer(std::nullopt);
+    variant = buffer;
+  } else if (std::get<PredictedObjectsBuffer>(variant).has_value()) {
+    // reacted
+    return;
+  }
+  if (searchPredictedObjectsNearEntity(*msg_ptr)) {
+    std::get<PredictedObjectsBuffer>(variant) = *msg_ptr;
+  }
+  //  if (!is_spawned) return;
+  mutex_.lock();
+  message_buffers_[node_name] = variant;
+  mutex_.unlock();
+}
+
 void ReactionAnalyzerNode::operationModeCallback(const OperationModeState::ConstSharedPtr msg_ptr)
 {
   std::lock_guard<std::mutex> lock(mutex_);
@@ -295,12 +320,12 @@ void ReactionAnalyzerNode::onTimer()
 void ReactionAnalyzerNode::spawnObstacle(
   const geometry_msgs::msg::Point & ego_pose, std::optional<rclcpp::Time> & spawn_cmd_time)
 {
-  const auto current_time = this->now();
 
   if (
     tier4_autoware_utils::calcDistance3d(ego_pose, entity_pose_.position) >=
     node_params_.spawn_distance_threshold) {
     // do not spawn it, send empty pointcloud
+    const auto current_time = this->now();
     pcl::PointCloud<pcl::PointXYZ> pcl_empty;
     PointCloud2 empty_pointcloud;
     pcl::toROSMsg(pcl_empty, empty_pointcloud);
@@ -324,6 +349,7 @@ void ReactionAnalyzerNode::spawnObstacle(
       RCLCPP_ERROR_STREAM(get_logger(), "Failed to look up transform from map to base_link");
       return;
     }
+    const auto current_time = this->now();
 
     // transform by using eigen matrix
     PointCloud2 transformed_points{};
@@ -377,6 +403,13 @@ void ReactionAnalyzerNode::printResults(
       } else {
         all_reacted = false;
       }
+    } else if (auto * predicted_objects_message = std::get_if<PredictedObjectsBuffer>(&variant)) {
+      if (predicted_objects_message->has_value()) {
+        reaction_times.emplace_back(
+          key, rclcpp::Time(predicted_objects_message->value().header.stamp));
+      } else {
+        all_reacted = false;
+      }
     }
   }
 
@@ -425,6 +458,8 @@ bool ReactionAnalyzerNode::loadChainModules()
       return MessageType::Trajectory;
     } else if (input == "sensor_msgs::msg::PointCloud2") {
       return MessageType::PointCloud2;
+    } else if (input == "autoware_auto_perception_msgs::msg::PredictedObjects") {
+      return MessageType::PredictedObjects;
     } else {
       return MessageType::Unknown;
     }
@@ -581,6 +616,15 @@ bool ReactionAnalyzerNode::initObservers(const reaction_analyzer::ChainModules &
         subscribers_.push_back(subscriber);
         break;
       }
+      case MessageType::PredictedObjects: {
+        auto callback = [this, module](const PredictedObjects::ConstSharedPtr msg) {
+          this->predictedObjectsOutputCallback(module.node_name, msg);
+        };
+        auto subscriber = this->create_subscription<PredictedObjects>(
+          module.topic_address, rclcpp::SensorDataQoS(), callback, createSubscriptionOptions());
+        subscribers_.push_back(subscriber);
+        break;
+      }
       case MessageType::Unknown:
         RCLCPP_WARN(
           this->get_logger(), "Unknown message type for module name: %s, skipping..",
@@ -787,6 +831,24 @@ bool ReactionAnalyzerNode::searchPointcloudNearEntity(
   return false;
 }
 
+bool ReactionAnalyzerNode::searchPredictedObjectsNearEntity(
+  const PredictedObjects & predicted_objects)
+{
+  bool isAnyObjectWithinRadius = std::any_of(
+    predicted_objects.objects.begin(), predicted_objects.objects.end(),
+    [this](const PredictedObject & object) {
+      return tier4_autoware_utils::calcDistance3d(
+               entity_pose_.position,
+               object.kinematics.initial_pose_with_covariance.pose.position) <=
+             entity_search_radius_;
+    });
+
+  if (isAnyObjectWithinRadius) {
+    return true;
+  }
+  return false;
+}
+
 }  // namespace reaction_analyzer
 
 #include <rclcpp_components/register_node_macro.hpp>