From 7a52098d8f2b33693379a11f23b60d3100749de7 Mon Sep 17 00:00:00 2001
From: Stephen Williams <swilliams@locusrobotics.com>
Date: Mon, 29 Jul 2024 20:16:46 -0700
Subject: [PATCH] Don't allow a running instance to be started. Instance must
 be stopped first.

---
 fuse_optimizers/src/fixed_lag_smoother.cpp | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp
index d094be08..6d024897 100644
--- a/fuse_optimizers/src/fixed_lag_smoother.cpp
+++ b/fuse_optimizers/src/fixed_lag_smoother.cpp
@@ -458,6 +458,12 @@ bool FixedLagSmoother::startServiceCallback(std_srvs::Empty::Request&, std_srvs:
 
 void FixedLagSmoother::start()
 {
+  if (started_)
+  {
+    ROS_WARN_STREAM("Requested to start the optimizer while it is already running. Ignoring request.");
+    return;
+  }
+
   ROS_INFO_STREAM("Starting optimizer.");
   // Tell all the plugins to start
   startPlugins();