add user configurable diag mapping support of termination types. (#410)

needphpsql · svwilliams · web-flow · commit 095de2441ea2 · 2025-07-01T15:07:15.000-04:00
* add user configurable diag mapping support of termination types.

* missing return.

* replace the entire block for diag.

* add unit test.

* Update fuse_optimizers/src/fixed_lag_smoother.cpp

Co-authored-by: Stephen Williams &lt;swilliams@locusrobotics.com&gt;

---------

Co-authored-by: Stephen Williams &lt;swilliams@locusrobotics.com&gt;
diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt
@@ -199,6 +199,34 @@ if(CATKIN_ENABLE_TESTING)
 
   add_rostest(test/optimizer.test ARGS config:=list DEPENDENCIES test_optimizer)
 
+  # Fixed lag smoother Tests
+  add_rostest_gtest(test_fixed_lag_smoother
+    test/fixed_lag_smoother.test
+    test/test_fixed_lag_smoother.cpp
+  )
+  add_dependencies(test_fixed_lag_smoother
+    ${catkin_EXPORTED_TARGETS}
+  )
+  target_include_directories(test_fixed_lag_smoother
+    PRIVATE
+      include
+  )
+  target_include_directories(test_fixed_lag_smoother
+    SYSTEM PRIVATE
+      ${catkin_INCLUDE_DIRS}
+      ${CMAKE_CURRENT_SOURCE_DIR}
+  )
+  target_link_libraries(test_fixed_lag_smoother
+    ${PROJECT_NAME}
+    ${catkin_LIBRARIES}
+  )
+  set_target_properties(test_fixed_lag_smoother
+    PROPERTIES
+      CXX_STANDARD 17
+      CXX_STANDARD_REQUIRED YES
+  )
+
+  add_rostest(test/fixed_lag_smoother.test ARGS config:=list DEPENDENCIES test_fixed_lag_smoother)
   # Fixed-lag Ignition test
   add_rostest_gtest(test_fixed_lag_ignition
     test/fixed_lag_ignition.test
diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h
@@ -334,6 +334,23 @@ class FixedLagSmoother : public Optimizer
     const std::string& sensor_name,
     fuse_core::Transaction::SharedPtr transaction) override;
 
+/**
+ * @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
+ * @param[in] diag_warnings The diagnostic warnings
+ * @param[in] diag_errors The diagnostic errors
+ * @return The diagnostic status with the level and message corresponding to the optimization termination type
+ */
+  diagnostic_msgs::DiagnosticStatus terminationTypeToDiagnosticStatus(const ceres::TerminationType termination_type,
+                                                                      const std::vector<std::string>& diag_warnings,
+                                                                      const std::vector<std::string>& diag_errors);
   /**
    * @brief Update and publish diagnotics
    * @param[in] status The diagnostic status
diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h
@@ -55,6 +55,16 @@ namespace fuse_optimizers
 struct FixedLagSmootherParams
 {
 public:
+  /**
+   * @brief Map the ceres::optimizer log levels to diagnostic levels
+   */
+  std::vector<std::string> diagnostic_warning_status { "NO_CONVERGENCE" };
+
+  /**
+   * @brief Map the ceres::optimizer log levels to diagnostic levels
+   */
+  std::vector<std::string> diagnostic_error_status {"FAILURE", "USER_FAILURE"};
+
   /**
    * @brief If true, the state estimator will not start until the start or reset service is called
    */
@@ -115,6 +125,10 @@ struct FixedLagSmootherParams
   void loadFromROS(const ros::NodeHandle& nh)
   {
     // Read settings from the parameter server
+    nh.param("diagnostic_warning_status", diagnostic_warning_status, diagnostic_warning_status);
+
+    nh.param("diagnostic_error_status", diagnostic_error_status, diagnostic_error_status);
+
     nh.getParam("disabled_at_startup", disabled_at_startup);
 
     fuse_core::getPositiveParam(nh, "lag_duration", lag_duration);
diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp
@@ -624,28 +624,58 @@ diagnostic_msgs::DiagnosticStatus makeDiagnosticStatus(const int8_t level, const
 }
 
 /**
- * @brief Helper function to generate the diagnostic status for each optimization termination type
+ * @brief Helper function to map termination type to string
  *
- * 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
+ * @param[in] termination_type The termination type
+ * @return the string format of the termination type
  */
-diagnostic_msgs::DiagnosticStatus terminationTypeToDiagnosticStatus(const ceres::TerminationType termination_type)
+std::string mapCeresLogToDiagLog(const ceres::TerminationType & termination_type)
 {
   switch (termination_type)
   {
     case ceres::TerminationType::CONVERGENCE:
+      return "CONVERGENCE";
     case ceres::TerminationType::USER_SUCCESS:
-      return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::OK, "Optimization converged");
+      return "USER_SUCCESS";
     case ceres::TerminationType::NO_CONVERGENCE:
-      return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::WARN, "Optimization didn't converge");
+      return "NO_CONVERGENCE";
+    case ceres::TerminationType::FAILURE:
+      return "FAILURE";
+    case ceres::TerminationType::USER_FAILURE:
+      return "USER_FAILURE";
     default:
-      return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::ERROR, "Optimization failed");
+      return "FAILURE";
+  }
+}
+
+/**
+ * @brief Helper function to check if a string is contained in the list
+ *
+ * @param[in] vec The list of strings
+ * @param[in] str The str to search
+ * @return true if contains, false otherwise
+ */
+inline bool contains(const std::vector<std::string>& vec, const std::string& str)
+{
+  return std::find(vec.begin(), vec.end(), str) != vec.end();
+}
+
+diagnostic_msgs::DiagnosticStatus FixedLagSmoother::terminationTypeToDiagnosticStatus(
+  const ceres::TerminationType termination_type, const std::vector<std::string>& diag_warnings,
+  const std::vector<std::string>& diag_errors)
+{
+  std::string diag_level = mapCeresLogToDiagLog(termination_type);
+  if (contains(diag_errors, diag_level))
+  {
+    return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::ERROR, "Optimization failed");
+  }
+  else if (contains(diag_warnings, diag_level))
+  {
+    return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::WARN, "Optimization didn't converge");
+  }
+  else
+  {
+    return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::OK, "Optimization converged");
   }
 }
 
@@ -686,7 +716,8 @@ void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrappe
       status.add("Initial Cost", summary.initial_cost);
       status.add("Final Cost", summary.final_cost);
 
-      status.mergeSummary(terminationTypeToDiagnosticStatus(summary.termination_type));
+      status.mergeSummary(terminationTypeToDiagnosticStatus(summary.termination_type, params_.diagnostic_warning_status,
+                                                            params_.diagnostic_error_status));
     }
 
     // Add time since the last optimization request time. This is useful to detect if no transactions are received for
diff --git a/fuse_optimizers/test/fixed_lag_smoother.test b/fuse_optimizers/test/fixed_lag_smoother.test
@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="config" default="struct" doc="Config format: {list, struct}"/>
+
+  <test test-name="FixedLagSmoother" pkg="fuse_optimizers" type="test_fixed_lag_smoother">
+  </test>
+</launch>
diff --git a/fuse_optimizers/test/test_fixed_lag_smoother.cpp b/fuse_optimizers/test/test_fixed_lag_smoother.cpp
@@ -0,0 +1,135 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2025, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <fuse_graphs/hash_graph.h>
+#include <fuse_graphs/hash_graph_params.h>
+#include <fuse_optimizers/fixed_lag_smoother.h>
+
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+
+class FixedLagSmootherForTest : public fuse_optimizers::FixedLagSmoother
+{
+public:
+  FixedLagSmootherForTest(fuse_core::Graph::UniquePtr graph,
+                          const ros::NodeHandle& nh,
+                          const ros::NodeHandle& pnh)
+    : fuse_optimizers::FixedLagSmoother(std::move(graph), nh, pnh) {}
+
+  using fuse_optimizers::FixedLagSmoother::terminationTypeToDiagnosticStatus;
+};
+
+class TestFixedLagSmoother : public ::testing::Test
+{
+protected:
+  ros::NodeHandle nh_;
+  ros::NodeHandle pnh_;
+  std::unique_ptr<FixedLagSmootherForTest> smoother_;
+
+  void SetUp() override
+  {
+    fuse_graphs::HashGraphParams graph_params;
+    graph_params.loadFromROS(pnh_);
+    smoother_ = std::make_unique<FixedLagSmootherForTest>(
+        fuse_graphs::HashGraph::make_unique(graph_params),
+        nh_, pnh_);
+  }
+
+  void TearDown() override
+  {
+    smoother_.reset();
+  }
+};
+
+TEST_F(TestFixedLagSmoother, terminationTypeToDiagnosticStatus)
+{
+  ceres::TerminationType termination_type;
+  std::vector<std::string> diagnostic_warning_status { "NO_CONVERGENCE" };
+  std::vector<std::string> diagnostic_error_status {"FAILURE", "USER_FAILURE"};
+  diagnostic_msgs::DiagnosticStatus diag_msg;
+
+  // NO_CONVERGENCE as WARNING
+  termination_type = ceres::TerminationType::NO_CONVERGENCE;
+  diag_msg = smoother_->terminationTypeToDiagnosticStatus(termination_type, diagnostic_warning_status,
+                                                          diagnostic_error_status);
+  EXPECT_EQ(diag_msg.level, diagnostic_msgs::DiagnosticStatus::WARN);
+
+  // NO_CONVERGENCE as OK
+  diagnostic_warning_status = {};
+  diag_msg = smoother_->terminationTypeToDiagnosticStatus(termination_type, diagnostic_warning_status,
+                                                          diagnostic_error_status);
+  EXPECT_EQ(diag_msg.level, diagnostic_msgs::DiagnosticStatus::OK);
+
+  // CONVERGENCE
+  termination_type = ceres::TerminationType::CONVERGENCE;
+  diag_msg = smoother_->terminationTypeToDiagnosticStatus(termination_type, diagnostic_warning_status,
+                                                          diagnostic_error_status);
+  EXPECT_EQ(diag_msg.level, diagnostic_msgs::DiagnosticStatus::OK);
+
+  // USER_SUCCESS
+  termination_type = ceres::TerminationType::USER_SUCCESS;
+  diag_msg = smoother_->terminationTypeToDiagnosticStatus(termination_type, diagnostic_warning_status,
+                                                          diagnostic_error_status);
+  EXPECT_EQ(diag_msg.level, diagnostic_msgs::DiagnosticStatus::OK);
+
+  // FAILURE
+  termination_type = ceres::TerminationType::FAILURE;
+  diag_msg = smoother_->terminationTypeToDiagnosticStatus(termination_type, diagnostic_warning_status,
+                                                          diagnostic_error_status);
+  EXPECT_EQ(diag_msg.level, diagnostic_msgs::DiagnosticStatus::ERROR);
+
+  // USER_FAILURE
+  termination_type = ceres::TerminationType::USER_FAILURE;
+  diag_msg = smoother_->terminationTypeToDiagnosticStatus(termination_type, diagnostic_warning_status,
+                                                          diagnostic_error_status);
+  EXPECT_EQ(diag_msg.level, diagnostic_msgs::DiagnosticStatus::ERROR);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "fixed_lag_smoother_node");
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}
