update output msg

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Author: danielsanchezaran

evaluator/control_evaluator/include/control_evaluator/control_evaluator_node.hpp

@@ -42,7 +42,7 @@ class controlEvaluatorNode : public rclcpp::Node
   /**
    * @brief publish the given metric statistic
    */
-  DiagnosticStatus generateDiagnosticStatus() const;
+  DiagnosticStatus generateDiagnosticStatus(const bool is_emergency_brake) const;
   void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg);
 
 private:

evaluator/control_evaluator/src/control_evaluator_node.cpp

@@ -38,49 +38,40 @@ controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_opti
   metrics_pub_ = create_publisher<DiagnosticArray>("~/metrics", 1);
 }
 
-DiagnosticStatus controlEvaluatorNode::generateDiagnosticStatus() const
+DiagnosticStatus controlEvaluatorNode::generateDiagnosticStatus(const bool is_emergency_brake) const
 {
   DiagnosticStatus status;
   status.level = status.OK;
-  status.name = "AEB";
+  status.name = "autonomous_emergency_braking";
   diagnostic_msgs::msg::KeyValue key_value;
-  key_value.key = "min";
-  key_value.value = boost::lexical_cast<decltype(key_value.value)>(0.0);
-  status.values.push_back(key_value);
-  key_value.key = "max";
-  key_value.value = boost::lexical_cast<decltype(key_value.value)>(0.5);
-  status.values.push_back(key_value);
-  key_value.key = "mean";
-  key_value.value = boost::lexical_cast<decltype(key_value.value)>(1.0);
+  key_value.key = "decision";
+  key_value.value = (is_emergency_brake) ? "stop" : "none";
   status.values.push_back(key_value);
   return status;
 }
 
 void controlEvaluatorNode::onDiagnostics(
   [[maybe_unused]] const DiagnosticArray::ConstSharedPtr diag_msg)
-
 {
   const auto start = now();
-
-  const bool aeb_activated =
-    std::any_of(diag_msg->status.begin(), diag_msg->status.end(), [](const auto & status) {
-      return status.name.find("autonomous_emergency_braking") != std::string::npos;
+  const auto aeb_status =
+    std::find_if(diag_msg->status.begin(), diag_msg->status.end(), [](const auto & status) {
+      const bool aeb_found = status.name.find("autonomous_emergency_braking") != std::string::npos;
+      return aeb_found;
     });
 
-  if (!aeb_activated) {
-    return;
-  }
+  if (aeb_status == diag_msg->status.end()) return;
+
+  const bool is_emergency_brake = (aeb_status->level == DiagnosticStatus::ERROR);
   DiagnosticArray metrics_msg;
   metrics_msg.header.stamp = now();
-
-  metrics_msg.status.push_back(generateDiagnosticStatus());
+  metrics_msg.status.emplace_back(generateDiagnosticStatus(is_emergency_brake));
 
   if (!metrics_msg.status.empty()) {
     metrics_pub_->publish(metrics_msg);
   }
   const auto runtime = (now() - start).seconds();
   RCLCPP_DEBUG(get_logger(), "control evaluation calculation time: %2.2f ms", runtime * 1e3);
-  std::cerr << "control evaluation calculation time: " << runtime << "\n";
 }
 
 }  // namespace control_diagnostics