From 2aa47040c6199864ca7caf21edb2f4722ce2b023 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Tue, 16 Jan 2024 15:27:56 +0300 Subject: [PATCH] PredictedObjects msg type added --- .../include/reaction_analyzer_node.hpp | 9 ++- .../param/reaction_analyzer.param.yaml | 3 + .../src/reaction_analyzer_node.cpp | 64 ++++++++++++++++++- 3 files changed, 74 insertions(+), 2 deletions(-) diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp index 1fa239228beca..fc04230c02f12 100644 --- a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp +++ b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp @@ -61,7 +61,9 @@ using ControlCommandBuffer = std::pair, std::optional>; using TrajectoryBuffer = std::optional; using PointCloud2Buffer = std::optional; -using BufferVariant = std::variant; +using PredictedObjectsBuffer = std::optional; +using BufferVariant = + std::variant; enum class MessageType { Unknown = 0, @@ -156,6 +158,8 @@ class ReactionAnalyzerNode : public rclcpp::Node bool searchPointcloudNearEntity(const pcl::PointCloud & 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 subscribers_; std::unordered_map message_buffers_; diff --git a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml index f472835e92319..02ae004a49b81 100644 --- a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml +++ b/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 diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp index 459d47028095d..2ee167747173e 100644 --- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp +++ b/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(variant)) { + // If the variant doesn't hold a Trajectory message yet, initialize it + PredictedObjectsBuffer buffer(std::nullopt); + variant = buffer; + } else if (std::get(variant).has_value()) { + // reacted + return; + } + if (searchPredictedObjectsNearEntity(*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_); @@ -295,12 +320,12 @@ void ReactionAnalyzerNode::onTimer() void ReactionAnalyzerNode::spawnObstacle( const geometry_msgs::msg::Point & ego_pose, std::optional & 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_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(&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( + 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