bug fix

Author: brkay54

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" <<std::endl;
             mutex_.lock();
             auto * tmp = std::get_if<ControlBuffer>(&messageBuffers_[key]);
             if (tmp) tmp->second = message->first.at(brake_start_idx.value());
@@ -444,14 +442,11 @@ void ReactionAnalyzerNode::controlCommandOutputCallback(
 {
   std::lock_guard<std::mutex> lock(mutex_);
   auto & variant = messageBuffers_[node_name];
-  if (std::holds_alternative<ControlBuffer>(variant)) {
-    std::get<ControlBuffer>(variant).first.push_back(*msg_ptr);
-  } else {
+  if (!std::holds_alternative<ControlBuffer>(variant)) {
     // If the variant doesn't hold a vector of AckermannControlCommand yet, initialize it
     ControlBuffer buffer(std::vector<AckermannControlCommand>{*msg_ptr}, std::nullopt);
     variant = buffer;
   }
-//  std::cout << node_name << std::endl;
   setControlCommandToBuffer(std::get<ControlBuffer>(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<size_t> ReactionAnalyzerNode::findFirstBrakeIdx(
   const std::vector<AckermannControlCommand> & 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<size_t>(node_params_.min_number_descending_order_control_cmd) ||
     !spawn_cmd_time)
@@ -543,8 +532,7 @@ std::optional<size_t> 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