diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1ee62a315f23441953f0e52fed6ec0394a79312d..50b8e9af2cda347713e0b8763067d40feefac75a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -22,6 +22,9 @@ find_package(catkin REQUIRED COMPONENTS
 find_package(wolfcore REQUIRED)
 find_package(wolflaser REQUIRED)
 find_package(falkolib QUIET)
+find_package(PkgConfig)
+pkg_check_modules(csm QUIET csm)
+link_directories(${csm_LIBRARY_DIRS})
 
 # produces the XXXConfig.cmake file among other things
 catkin_package(
@@ -35,11 +38,13 @@ catkin_package(
 ## Your package locations should be listed before other locations
 include_directories(
   include
-  ${wolflaser_INCLUDE_DIRS}
-  # ${laser_scan_utils_INCLUDE_DIRS}
   ${catkin_INCLUDE_DIRS}
 )
 
+# Bad -> should be removed eventually
+INCLUDE_DIRECTORIES("${falkolib_INCLUDE_DIRS}")
+INCLUDE_DIRECTORIES("${csm_INCLUDE_DIRS}")
+
 add_library(publisher_${PROJECT_NAME}
   ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_laser_map.cpp
 )
@@ -47,19 +52,16 @@ add_library(subscriber_${PROJECT_NAME}
   ${CMAKE_CURRENT_SOURCE_DIR}/src/subscriber_laser2d.cpp
 )
 
-# target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_laser_map.cpp)
-# target_sources(subscriber_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/subscriber_laser2d.cpp)
-
-# if (falkolib_FOUND)
-# 	message("Found Falkolib. Compiling publisher_falko.")
-# 	target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_falko.cpp)
-# endif()
+if (falkolib_FOUND)
+	message("Found Falkolib. Compiling publisher_falko.")
+	target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_falko.cpp)
+endif()
 
-# find_file(ICP wolflaser_INCLUDE_DIRS laser/processor/processor_odom_icp.h)
-# if (NOT ICP_NOTFOUND)
-# 	message("Found 'processor_odom_icp.h'. Compiling publisher_odom_icp.")
-# 	target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_odom_icp.cpp)
-# endif ()
+find_file(ICP wolflaser_INCLUDE_DIRS laser/processor/processor_odom_icp.h)
+if (NOT ICP_NOTFOUND)
+	message("Found 'processor_odom_icp.h'. Compiling publisher_odom_icp.")
+	target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_odom_icp.cpp)
+endif ()
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(subscriber_${PROJECT_NAME}