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