Now that it is possible to construct a fixed-lag smoother without sta…

…rting 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

@@ -62,21 +62,22 @@ 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);
   set_graph_service_ = node_handle_.advertiseService(ros::names::resolve(params_.set_graph_service),
                                                      &GraphIgnition::setGraphServiceCallback, this);
 }
 
-void GraphIgnition::start()
-{
-  started_ = true;
-}
-
 void GraphIgnition::stop()
 {
+  set_graph_service_.shutdown();
+  subscriber_.shutdown();
   started_ = false;
 }
 

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;
 }