diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h index 0e489f9d3..d74ea0e1a 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h +++ b/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 diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 11a3d8ed1..a29ecf00f 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/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 flag that indicates whether the optimizer has started or not + const bool started = started_; + + status.add("Started", started); { std::lock_guard 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 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 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 diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index d3d6da68e..203ee1769 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/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 + ' '; };