Ensure costmap_2d_cloud does not seg fault on SIGINT.

One of several clean shutdown issues being address via Issue ros-navigation#3271.

Signed-off-by: mbryan <matthew.bryan@dell.com>

nav2_costmap_2d/src/costmap_2d_cloud.cpp

@@ -225,6 +225,11 @@ int main(int argc, char ** argv)
     "voxel_grid", rclcpp::SystemDefaultsQoS(), voxelCallback);
 
   rclcpp::spin(g_node->get_node_base_interface());
+
+  g_node.reset();
+  pub_marked.reset();
+  pub_unknown.reset();
+
   rclcpp::shutdown();
 
   return 0;

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -299,19 +299,22 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 
   dyn_params_handler.reset();
 
+  stop();
+
+  // Map thread stuff
+  map_update_thread_shutdown_ = true;
+
+  if (map_update_thread_->joinable()) {
+    map_update_thread_->join();
+  }
+
   footprint_pub_->on_deactivate();
   costmap_publisher_->on_deactivate();
 
   for (auto & layer_pub : layer_publishers_) {
     layer_pub->on_deactivate();
   }
 
-  stop();
-
-  // Map thread stuff
-  map_update_thread_shutdown_ = true;
-  map_update_thread_->join();
-
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -575,18 +578,23 @@ void
 Costmap2DROS::stop()
 {
   stop_updates_ = true;
-  std::vector<std::shared_ptr<Layer>> * plugins = layered_costmap_->getPlugins();
-  std::vector<std::shared_ptr<Layer>> * filters = layered_costmap_->getFilters();
-  // unsubscribe from topics
-  for (std::vector<std::shared_ptr<Layer>>::iterator plugin = plugins->begin();
-    plugin != plugins->end(); ++plugin)
-  {
-    (*plugin)->deactivate();
-  }
-  for (std::vector<std::shared_ptr<Layer>>::iterator filter = filters->begin();
-    filter != filters->end(); ++filter)
-  {
-    (*filter)->deactivate();
+
+  // layered_costmap_ is set only if on_configure has been called
+  if (layered_costmap_) {
+    std::vector<std::shared_ptr<Layer>> * plugins = layered_costmap_->getPlugins();
+    std::vector<std::shared_ptr<Layer>> * filters = layered_costmap_->getFilters();
+
+    // unsubscribe from topics
+    for (std::vector<std::shared_ptr<Layer>>::iterator plugin = plugins->begin();
+      plugin != plugins->end(); ++plugin)
+    {
+      (*plugin)->deactivate();
+    }
+    for (std::vector<std::shared_ptr<Layer>>::iterator filter = filters->begin();
+      filter != filters->end(); ++filter)
+    {
+      (*filter)->deactivate();
+    }
   }
   initialized_ = false;
   stopped_ = true;