add option for sse4 and avs512 (#3853)

nav2_mppi_controller/CMakeLists.txt

@@ -36,9 +36,19 @@ nav2_package()
 
 include(CheckCXXCompilerFlag)
 
+check_cxx_compiler_flag("-mno-avx512f" COMPILER_SUPPORTS_AVX512)
+check_cxx_compiler_flag("-msse4.2" COMPILER_SUPPORTS_SSE4)
 check_cxx_compiler_flag("-mavx2" COMPILER_SUPPORTS_AVX2)
 check_cxx_compiler_flag("-mfma" COMPILER_SUPPORTS_FMA)
 
+if(COMPILER_SUPPORTS_AVX512)
+  add_compile_options(-mno-avx512f)
+endif()
+
+if(COMPILER_SUPPORTS_SSE4)
+  add_compile_options(-msse4.2)
+endif()
+
 if(COMPILER_SUPPORTS_AVX2)
   add_compile_options(-mavx2)
 endif()
@@ -48,7 +58,7 @@ if(COMPILER_SUPPORTS_FMA)
 endif()
 
 # If building one the same hardware to be deployed on, try `-march=native`!
-add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic -mno-avx512f)
+add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic)
 
 add_library(mppi_controller SHARED
   src/controller.cpp