Add solver summary info to diagnostics

This also changes the diagnostic updater name, making things cleaner in
the children classes.

fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h

@@ -175,6 +175,7 @@ class FixedLagSmoother : public Optimizer
   ros::Time lag_expiration_;  //!< The oldest stamp that is inside the fixed-lag smoother window
   fuse_core::Transaction marginal_transaction_;  //!< The marginals to add during the next optimization cycle
   VariableStampIndex timestamp_tracking_;  //!< Object that tracks the timestamp associated with each variable
+  ceres::Solver::Summary summary_;  //!< Optimization summary, written by optimizationLoop and read by setDiagnostics
 
   // Guarded by optimization_requested_mutex_
   std::mutex optimization_requested_mutex_;  //!< Required condition variable mutex

fuse_optimizers/src/fixed_lag_smoother.cpp

@@ -202,7 +202,8 @@ void FixedLagSmoother::optimizationLoop()
       // Update the graph
       graph_->update(*new_transaction);
       // Optimize the entire graph
-      graph_->optimize(params_.solver_options);
+      summary_ = graph_->optimize(params_.solver_options);
+
       // Optimization is complete. Notify all the things about the graph changes.
       notify(std::move(new_transaction), graph_->clone());
       // Compute a transaction that marginalizes out those variables.
@@ -495,20 +496,112 @@ void FixedLagSmoother::transactionCallback(
   }
 }
 
-void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status)
+/**
+ * @brief Make a diagnostic_msgs::DiagnosticStatus message filling in the level and message
+ *
+ * @param[in] level   The diagnostic status level
+ * @param[in] message The diagnostic status message
+ */
+diagnostic_msgs::DiagnosticStatus makeDiagnosticStatus(const int8_t level, const std::string& message)
 {
-  Optimizer::setDiagnostics(status);
+  diagnostic_msgs::DiagnosticStatus status;
 
-  if (status.level == diagnostic_msgs::DiagnosticStatus::OK)
+  status.level = level;
+  status.message = message;
+
+  return status;
+}
+
+/**
+ * @brief Helper function to generate the diagnostic status for each optimization termination type
+ *
+ * The termination type -> diagnostic status mapping is as follows:
+ *
+ * - CONVERGENCE, USER_SUCCESS -> OK
+ * - NO_CONVERGENCE            -> WARN
+ * - FAILURE, USER_FAILURE     -> ERROR (default)
+ *
+ * @param[in] termination_type The optimization termination type
+ * @return The diagnostic status with the level and message corresponding to the optimization termination type
+ */
+diagnostic_msgs::DiagnosticStatus terminationTypeToDiagnosticStatus(const ceres::TerminationType termination_type)
+{
+  switch (termination_type)
   {
-    status.message = "FixedLagSmoother " + status.message;
+    case ceres::TerminationType::CONVERGENCE:
+    case ceres::TerminationType::USER_SUCCESS:
+      return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::OK, "Optimization converged");
+    case ceres::TerminationType::NO_CONVERGENCE:
+      return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::WARN, "Optimization didn't converged");
+    default:
+      return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::ERROR, "Optimization failed");
   }
+}
 
-  status.add("Started", started_);
+void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status)
+{
+  Optimizer::setDiagnostics(status);
+
+  // Load std::atomic<bool> flag that indicates whether the optimizer has started or not
+  const bool started = started_;
+
+  status.add("Started", started);
   {
     std::lock_guard<std::mutex> lock(pending_transactions_mutex_);
     status.add("Pending Transactions", pending_transactions_.size());
   }
+
+  if (started)
+  {
+    // Add some optimization summary report fields to the diagnostics status if the optimizer has started
+    auto summary = decltype(summary_)();
+    {
+      const std::unique_lock<std::mutex> lock(optimization_mutex_, std::try_to_lock);
+      if (lock)
+      {
+        summary = summary_;
+      }
+      else
+      {
+        status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Optimization running");
+      }
+    }
+
+    if (summary.total_time_in_seconds >= 0.0)  // This is -1 for the default-constructed summary object
+    {
+      status.add("Optimization Termination Type", ceres::TerminationTypeToString(summary.termination_type));
+      status.add("Optimization Total Time [s]", summary.total_time_in_seconds);
+      status.add("Optimization Iterations", summary.iterations.size());
+      status.add("Initial Cost", summary.initial_cost);
+      status.add("Final Cost", summary.final_cost);
+
+      status.mergeSummary(terminationTypeToDiagnosticStatus(summary.termination_type));
+    }
+
+    // Add time since the last optimization request time. This is useful to detect if no transactions are received for
+    // too long
+    auto optimization_deadline = decltype(optimization_deadline_)();
+    {
+      const std::unique_lock<std::mutex> lock(optimization_requested_mutex_, std::try_to_lock);
+      if (lock)
+      {
+        optimization_deadline = optimization_deadline_;
+      }
+    }
+
+    if (!optimization_deadline.isZero())  // This is zero for the default-constructed optimization_deadline object
+    {
+      const auto optimization_deadline = optimization_deadline_;
+      optimization_requested_mutex_.unlock();
+
+      if (!optimization_deadline.isZero())
+      {
+        const auto optimization_request_time = optimization_deadline - params_.optimization_period;
+        const auto time_since_last_optimization_request = ros::Time::now() - optimization_request_time;
+        status.add("Time Since Last Optimization Request [s]", time_since_last_optimization_request.toSec());
+      }
+    }
+  }
 }
 
 }  // namespace fuse_optimizers

fuse_optimizers/src/optimizer.cpp

@@ -99,7 +99,7 @@ Optimizer::Optimizer(
       private_node_handle_.createTimer(ros::Duration(diagnostic_updater_timer_period_),
                                        boost::bind(&diagnostic_updater::Updater::update, &diagnostic_updater_));
 
-  diagnostic_updater_.add("Optimizer", this, &Optimizer::setDiagnostics);
+  diagnostic_updater_.add(private_node_handle_.getNamespace(), this, &Optimizer::setDiagnostics);
   diagnostic_updater_.setHardwareID("fuse");
 
   // Wait for a valid time before loading any of the plugins
@@ -482,7 +482,7 @@ void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat
     return;
   }
 
-  status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Optimizer");
+  status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Optimization converged");
 
   auto print_key = [](const std::string& result, const auto& entry) { return result + entry.first + ' '; };