From bbac22e81c9635000abafa22ad2c63e0bd9e1505 Mon Sep 17 00:00:00 2001
From: Stephen Williams <swilliams@locusrobotics.com>
Date: Fri, 2 Aug 2024 11:31:56 -0700
Subject: [PATCH] Now that it is possible to construct a fixed-lag smoother
 without starting it, there may be a significant difference between when
 onInit() is called and when start() is called. Update all of the sensor
 models delay subscribing to topics until started. This prevents trying to
 process sensor data while the optimizer is stopped.

---
 fuse_models/src/graph_ignition.cpp       | 11 ++++++-----
 fuse_models/src/unicycle_2d_ignition.cpp | 16 +++++++++-------
 2 files changed, 15 insertions(+), 12 deletions(-)

diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp
index 80d5b56f1..f39e599c0 100644
--- a/fuse_models/src/graph_ignition.cpp
+++ b/fuse_models/src/graph_ignition.cpp
@@ -62,7 +62,11 @@ void GraphIgnition::onInit()
   {
     reset_client_ = node_handle_.serviceClient<std_srvs::Empty>(ros::names::resolve(params_.reset_service));
   }
+}
 
+void GraphIgnition::start()
+{
+  started_ = true;
   // Advertise
   subscriber_ = node_handle_.subscribe(ros::names::resolve(params_.topic), params_.queue_size,
                                        &GraphIgnition::subscriberCallback, this);
@@ -70,13 +74,10 @@ void GraphIgnition::onInit()
                                                      &GraphIgnition::setGraphServiceCallback, this);
 }
 
-void GraphIgnition::start()
-{
-  started_ = true;
-}
-
 void GraphIgnition::stop()
 {
+  set_graph_service_.shutdown();
+  subscriber_.shutdown();
   started_ = false;
 }
 
diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp
index 5f162ab40..736a4bba3 100644
--- a/fuse_models/src/unicycle_2d_ignition.cpp
+++ b/fuse_models/src/unicycle_2d_ignition.cpp
@@ -88,6 +88,11 @@ void Unicycle2DIgnition::onInit()
   {
     reset_client_ = node_handle_.serviceClient<std_srvs::Empty>(ros::names::resolve(params_.reset_service));
   }
+}
+
+void Unicycle2DIgnition::start()
+{
+  started_ = true;
 
   // Advertise
   subscriber_ = node_handle_.subscribe(
@@ -103,14 +108,7 @@ void Unicycle2DIgnition::onInit()
     ros::names::resolve(params_.set_pose_deprecated_service),
     &Unicycle2DIgnition::setPoseDeprecatedServiceCallback,
     this);
-}
-
-void Unicycle2DIgnition::start()
-{
-  started_ = true;
 
-  // TODO(swilliams) Should this be executed every time optimizer.reset() is called, or only once ever?
-  //                 I feel like it should be "only once ever".
   // Send an initial state transaction immediately, if requested
   if (params_.publish_on_startup && !initial_transaction_sent_)
   {
@@ -129,6 +127,10 @@ void Unicycle2DIgnition::start()
 
 void Unicycle2DIgnition::stop()
 {
+  initial_transaction_sent_ = false;
+  set_pose_deprecated_service_.shutdown();
+  set_pose_service_.shutdown();
+  subscriber_.shutdown();
   started_ = false;
 }