DetectedObjects msg type added

Author: brkay54

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -49,6 +49,7 @@ using autoware_adapi_v1_msgs::msg::OperationModeState;
 using autoware_adapi_v1_msgs::msg::RouteState;
 using autoware_adapi_v1_msgs::srv::ChangeOperationMode;
 using autoware_auto_control_msgs::msg::AckermannControlCommand;
+using autoware_auto_perception_msgs::msg::DetectedObject;
 using autoware_auto_perception_msgs::msg::DetectedObjects;
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
@@ -62,8 +63,10 @@ using ControlCommandBuffer =
 using TrajectoryBuffer = std::optional<Trajectory>;
 using PointCloud2Buffer = std::optional<PointCloud2>;
 using PredictedObjectsBuffer = std::optional<PredictedObjects>;
-using BufferVariant =
-  std::variant<ControlCommandBuffer, TrajectoryBuffer, PointCloud2Buffer, PredictedObjectsBuffer>;
+using DetectedObjectsBuffer = std::optional<DetectedObjects>;
+using BufferVariant = std::variant<
+  ControlCommandBuffer, TrajectoryBuffer, PointCloud2Buffer, PredictedObjectsBuffer,
+  DetectedObjectsBuffer>;
 
 enum class MessageType {
   Unknown = 0,
@@ -160,6 +163,8 @@ class ReactionAnalyzerNode : public rclcpp::Node
 
   bool searchPredictedObjectsNearEntity(const PredictedObjects & predicted_objects);
 
+  bool searchDetectedObjectsNearEntity(const DetectedObjects & detected_objects);
+
   bool loadChainModules();
 
   bool initObservers(const ChainModules & modules);
@@ -184,10 +189,6 @@ class ReactionAnalyzerNode : public rclcpp::Node
   void spawnObstacle(
     const geometry_msgs::msg::Point & ego_pose, std::optional<rclcpp::Time> & spawn_cmd_time);
 
-  //  void searchForReactions(
-  //    const std::unordered_map<std::string, BufferVariant> & message_buffers,
-  //    const geometry_msgs::msg::Point & ego_pose);
-
   void printResults(const std::unordered_map<std::string, BufferVariant> & message_buffers);
 
   void onTimer();
@@ -214,6 +215,9 @@ class ReactionAnalyzerNode : public rclcpp::Node
   void predictedObjectsOutputCallback(
     const std::string & node_name, const PredictedObjects::ConstSharedPtr msg_ptr);
 
+  void detectedObjectsOutputCallback(
+    const std::string & node_name, const DetectedObjects::ConstSharedPtr msg_ptr);
+
   // Variables
   std::vector<rclcpp::SubscriptionBase::SharedPtr> subscribers_;
   std::unordered_map<std::string, BufferVariant> message_buffers_;

tools/reaction_analyzer/src/reaction_analyzer_node.cpp

@@ -175,6 +175,31 @@ void ReactionAnalyzerNode::predictedObjectsOutputCallback(
   mutex_.unlock();
 }
 
+void ReactionAnalyzerNode::detectedObjectsOutputCallback(
+  const std::string & node_name, const DetectedObjects::ConstSharedPtr msg_ptr)
+{
+  mutex_.lock();
+  auto variant = message_buffers_[node_name];
+  //  const auto is_spawned = spawn_cmd_time_;
+  mutex_.unlock();
+
+  if (!std::holds_alternative<DetectedObjectsBuffer>(variant)) {
+    // If the variant doesn't hold a Trajectory message yet, initialize it
+    DetectedObjectsBuffer buffer(std::nullopt);
+    variant = buffer;
+  } else if (std::get<DetectedObjectsBuffer>(variant).has_value()) {
+    // reacted
+    return;
+  }
+  if (searchDetectedObjectsNearEntity(*msg_ptr)) {
+    std::get<DetectedObjectsBuffer>(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_);
@@ -320,7 +345,6 @@ void ReactionAnalyzerNode::onTimer()
 void ReactionAnalyzerNode::spawnObstacle(
   const geometry_msgs::msg::Point & ego_pose, std::optional<rclcpp::Time> & spawn_cmd_time)
 {
-
   if (
     tier4_autoware_utils::calcDistance3d(ego_pose, entity_pose_.position) >=
     node_params_.spawn_distance_threshold) {
@@ -410,6 +434,13 @@ void ReactionAnalyzerNode::printResults(
       } else {
         all_reacted = false;
       }
+    } else if (auto * detected_objects_message = std::get_if<DetectedObjectsBuffer>(&variant)) {
+      if (detected_objects_message->has_value()) {
+        reaction_times.emplace_back(
+          key, rclcpp::Time(detected_objects_message->value().header.stamp));
+      } else {
+        all_reacted = false;
+      }
     }
   }
 
@@ -460,6 +491,8 @@ bool ReactionAnalyzerNode::loadChainModules()
       return MessageType::PointCloud2;
     } else if (input == "autoware_auto_perception_msgs::msg::PredictedObjects") {
       return MessageType::PredictedObjects;
+    } else if (input == "autoware_auto_perception_msgs::msg::DetectedObjects") {
+      return MessageType::DetectedObjects;
     } else {
       return MessageType::Unknown;
     }
@@ -625,6 +658,15 @@ bool ReactionAnalyzerNode::initObservers(const reaction_analyzer::ChainModules &
         subscribers_.push_back(subscriber);
         break;
       }
+      case MessageType::DetectedObjects: {
+        auto callback = [this, module](const DetectedObjects::ConstSharedPtr msg) {
+          this->detectedObjectsOutputCallback(module.node_name, msg);
+        };
+        auto subscriber = this->create_subscription<DetectedObjects>(
+          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..",
@@ -849,6 +891,22 @@ bool ReactionAnalyzerNode::searchPredictedObjectsNearEntity(
   return false;
 }
 
+bool ReactionAnalyzerNode::searchDetectedObjectsNearEntity(const DetectedObjects & detected_objects)
+{
+  bool isAnyObjectWithinRadius = std::any_of(
+    detected_objects.objects.begin(), detected_objects.objects.end(),
+    [this](const DetectedObject & object) {
+      return tier4_autoware_utils::calcDistance3d(
+               entity_pose_.position, object.kinematics.pose_with_covariance.pose.position) <=
+             entity_search_radius_;
+    });
+
+  if (isAnyObjectWithinRadius) {
+    return true;
+  }
+  return false;
+}
+
 }  // namespace reaction_analyzer
 
 #include <rclcpp_components/register_node_macro.hpp>