diff --git a/nav2_costmap_2d/src/costmap_2d_cloud.cpp b/nav2_costmap_2d/src/costmap_2d_cloud.cpp index 8c40709be6d..fba76d11d60 100644 --- a/nav2_costmap_2d/src/costmap_2d_cloud.cpp +++ b/nav2_costmap_2d/src/costmap_2d_cloud.cpp @@ -225,6 +225,12 @@ int main(int argc, char ** argv) "voxel_grid", rclcpp::SystemDefaultsQoS(), voxelCallback); rclcpp::spin(g_node->get_node_base_interface()); + + // Force dtors to fire before shutdown(). We see segfaults otherwise. + g_node.reset(); + pub_marked.reset(); + pub_unknown.reset(); + rclcpp::shutdown(); return 0;