diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp index 93cd8d00d27c0..ed58ed1e44d50 100644 --- a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp +++ b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp @@ -15,6 +15,7 @@ #ifndef REACTION_ANALYZER_NODE_HPP_ #define REACTION_ANALYZER_NODE_HPP_ +#include #include #include #include @@ -55,17 +56,16 @@ using autoware_auto_planning_msgs::msg::Trajectory; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; + using ControlBuffer = std::pair, std::optional>; - -using BufferVariant = - std::variant // Add other message types here - >; +using PlanningBuffer = std::pair>; +using BufferVariant = std::variant; enum class MessageType { Unknown = 0, AckermannControlCommand = 1, + Trajectory = 2, }; struct TopicConfig @@ -120,7 +120,8 @@ class ReactionAnalyzerNode : public rclcpp::Node std::vector subscribers_; void controlCommandOutputCallback( const std::string & node_name, const AckermannControlCommand::ConstSharedPtr msg_ptr); - + void planningOutputCallback( + const std::string & node_name, const Trajectory::ConstSharedPtr msg_ptr); std::unordered_map messageBuffers_; std::mutex mutex_; @@ -182,10 +183,6 @@ class ReactionAnalyzerNode : public rclcpp::Node bool all_topics_reacted_{false}; std::optional last_initialize_test_time_; - // measurement for control cmd - // std::optional trajectory_follower_brake_cmd; - // std::optional vehicle_cmd_gate_brake_cmd; - // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/tools/reaction_analyzer/package.xml b/tools/reaction_analyzer/package.xml index 9943ae8fc65fc..64383966a7ba3 100644 --- a/tools/reaction_analyzer/package.xml +++ b/tools/reaction_analyzer/package.xml @@ -20,6 +20,7 @@ autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs + motion_utils eigen libpcl-all-dev pcl_conversions diff --git a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml index ca552281cabba..957fa0faf5b78 100644 --- a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml +++ b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml @@ -1,6 +1,18 @@ /**: ros__parameters: chain: + obstacle_stop_planner: + topic_name: /planning/scenario_planning/lane_driving/trajectory + message_type: autoware_auto_planning_msgs::msg::Trajectory + scenario_selector: + topic_name: /planning/scenario_planning/scenario_selector/trajectory + message_type: autoware_auto_planning_msgs::msg::Trajectory + motion_velocity_smoother: + topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory + message_type: autoware_auto_planning_msgs::msg::Trajectory + planning_validator: + topic_name: /planning/scenario_planning/trajectory + message_type: autoware_auto_planning_msgs::msg::Trajectory trajectory_follower: topic_name: /control/trajectory_follower/control_cmd message_type: autoware_auto_control_msgs::msg::AckermannControlCommand diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp index ec3fd29ac4de2..f12fa9f32d742 100644 --- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -82,6 +82,8 @@ MessageType stringToMessageType(const std::string & input) { if (input == "autoware_auto_control_msgs::msg::AckermannControlCommand") { return MessageType::AckermannControlCommand; + } else if (input == "autoware_auto_planning_msgs::msg::Trajectory") { + return MessageType::Trajectory; } else { return MessageType::Unknown; } @@ -305,6 +307,28 @@ void ReactionAnalyzerNode::onTimer() mutex_.unlock(); } } + } else if (auto * message = std::get_if(&variant)) { + if (!message->second) { + all_topics_reacted_ = false; + const auto nearest_idx = motion_utils::findNearestIndex( + message->first.points, current_odometry_ptr->pose.pose.position); + // + // constexpr double lookahead_distance = 15.0; + // const auto destination_idx = motion_utils::calcLongitudinalOffsetPoint( + // message->first.points, nearest_idx, lookahead_distance); + const auto zero_vel_idx = motion_utils::searchZeroVelocityIndex( + message->first.points, nearest_idx, message->first.points.size() - 1); + + if (zero_vel_idx) { + std::cout << key << " found: nearest_idx: " << nearest_idx + << " zero_vel_idx: " << zero_vel_idx.value() + << " size: " << message->first.points.size() << " stamp: " << message->first.header.stamp.nanosec << std::endl; + mutex_.lock(); + auto * tmp = std::get_if(&messageBuffers_[key]); + if (tmp) tmp->second = message->first; + mutex_.unlock(); + } + } } // Add similar blocks for other types } @@ -317,6 +341,10 @@ void ReactionAnalyzerNode::onTimer() if (message->second) { reaction_times.emplace_back(key, rclcpp::Time(message->second->stamp)); } + } else if (auto * message = std::get_if(&variant)) { + if (message->second) { + reaction_times.emplace_back(key, rclcpp::Time(message->second->header.stamp)); + } } } @@ -450,6 +478,20 @@ void ReactionAnalyzerNode::controlCommandOutputCallback( setControlCommandToBuffer(std::get(variant).first, *msg_ptr); } +void ReactionAnalyzerNode::planningOutputCallback( + const std::string & node_name, const Trajectory::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + auto & variant = messageBuffers_[node_name]; + if (!std::holds_alternative(variant)) { + // If the variant doesn't hold a vector of AckermannControlCommand yet, initialize it + PlanningBuffer buffer(*msg_ptr, std::nullopt); + variant = buffer; + } else { + std::get(variant).first = *msg_ptr; + } +} + void ReactionAnalyzerNode::initObservers(const reaction_analyzer::ChainModules & modules) { for (const auto & module : modules) { @@ -463,6 +505,15 @@ void ReactionAnalyzerNode::initObservers(const reaction_analyzer::ChainModules & subscribers_.push_back(subscriber); break; } + case MessageType::Trajectory: { + auto callback = [this, module](const Trajectory::ConstSharedPtr msg) { + this->planningOutputCallback(module.node_name, msg); + }; + auto subscriber = this->create_subscription( + module.topic_address, 1, callback, createSubscriptionOptions(this)); + subscribers_.push_back(subscriber); + break; + } case MessageType::Unknown: RCLCPP_WARN( this->get_logger(), "Unknown message type for topic: %s", module.topic_address.c_str());