diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp index 308088ad68637..ec3fd29ac4de2 100644 --- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -91,7 +91,6 @@ void ReactionAnalyzerNode::loadChainModules() { const auto param_key = std::string("chain"); const auto module_names = this->list_parameters({param_key}, 3).prefixes; - // std::cout << module_names.size() << std::endl; for (const auto & module_name : module_names) { const auto splitted_name = split(module_name, '.'); TopicConfig tmp; @@ -300,7 +299,6 @@ void ReactionAnalyzerNode::onTimer() all_topics_reacted_ = false; const auto brake_start_idx = findFirstBrakeIdx(message->first); if (brake_start_idx) { -// std::cout << "found brake idx" <(&messageBuffers_[key]); if (tmp) tmp->second = message->first.at(brake_start_idx.value()); @@ -444,14 +442,11 @@ void ReactionAnalyzerNode::controlCommandOutputCallback( { std::lock_guard lock(mutex_); auto & variant = messageBuffers_[node_name]; - if (std::holds_alternative(variant)) { - std::get(variant).first.push_back(*msg_ptr); - } else { + if (!std::holds_alternative(variant)) { // If the variant doesn't hold a vector of AckermannControlCommand yet, initialize it ControlBuffer buffer(std::vector{*msg_ptr}, std::nullopt); variant = buffer; } -// std::cout << node_name << std::endl; setControlCommandToBuffer(std::get(variant).first, *msg_ptr); } @@ -520,18 +515,12 @@ void ReactionAnalyzerNode::setControlCommandToBuffer( itr++; } } -// std::cout << "buffer size: " << buffer.size() << std::endl; buffer.emplace_back(cmd); } std::optional ReactionAnalyzerNode::findFirstBrakeIdx( const std::vector & cmd_array) { -// for (size_t k = 0; k < cmd_array.size(); ++k) { -// std::cout << cmd_array.at(k).longitudinal.acceleration << " "; -// } -// std::cout << std::endl; - if ( cmd_array.size() < static_cast(node_params_.min_number_descending_order_control_cmd) || !spawn_cmd_time) @@ -543,8 +532,7 @@ std::optional ReactionAnalyzerNode::findFirstBrakeIdx( cmd_array.at(cmd_array.size() - node_params_.min_number_descending_order_control_cmd).stamp) < spawn_cmd_time) return {}; - std::cout << "searching " << std::endl; - // find the place that acc cmd decreases continuously + for (size_t i = 0; i < cmd_array.size() - node_params_.min_number_descending_order_control_cmd + 1; ++i) { size_t decreased_cmd_counter = 1; // because # of the decreased cmd = iteration + 1