diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp index fc04230c02f12..82313aae1433f 100644 --- a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp +++ b/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; using PointCloud2Buffer = std::optional; using PredictedObjectsBuffer = std::optional; -using BufferVariant = - std::variant; +using DetectedObjectsBuffer = std::optional; +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 & spawn_cmd_time); - // void searchForReactions( - // const std::unordered_map & message_buffers, - // const geometry_msgs::msg::Point & ego_pose); - void printResults(const std::unordered_map & 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 subscribers_; std::unordered_map message_buffers_; diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp index 2ee167747173e..88c93dca75322 100644 --- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp +++ b/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(variant)) { + // If the variant doesn't hold a Trajectory message yet, initialize it + DetectedObjectsBuffer buffer(std::nullopt); + variant = buffer; + } else if (std::get(variant).has_value()) { + // reacted + return; + } + if (searchDetectedObjectsNearEntity(*msg_ptr)) { + std::get(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 lock(mutex_); @@ -320,7 +345,6 @@ void ReactionAnalyzerNode::onTimer() void ReactionAnalyzerNode::spawnObstacle( const geometry_msgs::msg::Point & ego_pose, std::optional & 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(&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( + 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