Skip to content

Commit

Permalink
DetectedObjects msg type added
Browse files Browse the repository at this point in the history
  • Loading branch information
brkay54 committed Jan 21, 2024
1 parent 2aa4704 commit a60d6df
Show file tree
Hide file tree
Showing 2 changed files with 69 additions and 7 deletions.
16 changes: 10 additions & 6 deletions tools/reaction_analyzer/include/reaction_analyzer_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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,
Expand Down Expand Up @@ -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);
Expand All @@ -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();
Expand All @@ -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_;
Expand Down
60 changes: 59 additions & 1 deletion tools/reaction_analyzer/src/reaction_analyzer_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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;
}
}
}

Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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..",
Expand Down Expand Up @@ -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>
Expand Down

0 comments on commit a60d6df

Please sign in to comment.