Added Canonical Scan Matcher optional dependency

CMakeLists.txt

@@ -110,6 +110,7 @@ IF(YAMLCPP_FOUND)
 ELSEIF(YAMLCPP_FOUND)
     MESSAGE("yaml-cpp Library NOT FOUND!")
 ENDIF(YAMLCPP_FOUND)
+FIND_PACKAGE(csm QUIET)
 
 FIND_PATH(
     Suitesparse_INCLUDE_DIRS
@@ -148,6 +149,7 @@ INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
 INCLUDE_DIRECTORIES(${wolf_INCLUDE_DIR})
 include_directories(BEFORE "include")
 INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${csm_INCLUDE_DIR})
 IF(Ceres_FOUND)
     INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
 ENDIF(Ceres_FOUND)
@@ -183,7 +185,6 @@ SET(HDRS_LANDMARK
 SET(HDRS_PROCESSOR
   include/laser/processor/polyline_2D_utils.h
   include/laser/processor/processor_tracker_feature_polyline_2D.h
-  include/laser/processor/processor_2D_demo.h
   )
 SET(HDRS_SENSOR
   include/laser/sensor/sensor_laser_2D.h
@@ -218,7 +219,6 @@ SET(SRCS_LANDMARK
 SET(SRCS_PROCESSOR
   src/processor/polyline_2D_utils.cpp
   src/processor/processor_tracker_feature_polyline_2D.cpp
-  src/processor/processor_2D_demo.cpp
   )
 SET(SRCS_SENSOR
   src/sensor/sensor_laser_2D.cpp
@@ -231,6 +231,14 @@ SET(SRCS_YAML
   src/yaml/sensor_laser_2D_yaml.cpp
   )
 #OPTIONALS
+if(csm_FOUND)
+  SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+    include/laser/processor/processor_2D_demo.h
+    )
+  SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+    src/processor/processor_2D_demo.cpp
+    )
+endif(csm_FOUND)
 #optional HDRS and SRCS
 # ==================EXAMPLE===============
 # IF (Ceres_FOUND)
@@ -272,7 +280,7 @@ ADD_LIBRARY(${PLUGIN_NAME}
   )
 TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CMAKE_THREAD_LIBS_INIT})
 
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolf_LIBRARY} ${laser_scan_utils_LIBRARY})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolf_LIBRARY} ${laser_scan_utils_LIBRARY} ${csm_LIBRARY})
 #Link the created libraries
 #===============EXAMPLE=========================
 # IF (Ceres_FOUND)

cmake_modules/Findcsm.cmake

+FIND_PATH(
+    csm_INCLUDE_DIR
+    NAMES algos.h
+    PATHS /usr/local/include/csm)
+IF(csm_INCLUDE_DIR)
+  MESSAGE("Found csm include dirs: ${csm_INCLUDE_DIR}")
+ELSE(csm_INCLUDE_DIR)
+  MESSAGE("Couldn't find csm include dirs")
+ENDIF(csm_INCLUDE_DIR)
+
+FIND_LIBRARY(
+    csm_LIBRARY
+    NAMES libcsm.so libcsm.dylib
+    PATHS /usr/local/lib)
+IF(csm_LIBRARY)
+  MESSAGE("Found csm lib: ${csm_LIBRARY}")
+ELSE(csm_LIBRARY)
+  MESSAGE("Couldn't find csm lib")
+ENDIF(csm_LIBRARY)
+
+IF (csm_INCLUDE_DIR AND csm_LIBRARY)
+   SET(csm_FOUND TRUE)
+ ELSE(csm_INCLUDE_DIR AND csm_LIBRARY)
+   set(csm_FOUND FALSE)
+ENDIF (csm_INCLUDE_DIR AND csm_LIBRARY)
+
+IF (csm_FOUND)
+   IF (NOT csm_FIND_QUIETLY)
+      MESSAGE(STATUS "Found csm: ${csm_LIBRARY}")
+   ENDIF (NOT csm_FIND_QUIETLY)
+ELSE (csm_FOUND)
+   IF (csm_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find csm")
+   ENDIF (csm_FIND_REQUIRED)
+ENDIF (csm_FOUND)
+
+
+macro(csm_report_not_found REASON_MSG)
+  set(csm_FOUND FALSE)
+  unset(csm_INCLUDE_DIR)
+  unset(csm_LIBRARIES)
+
+  # Reset the CMake module path to its state when this script was called.
+  set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
+
+  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
+  # FindPackage() use the camelcase library name, not uppercase.
+  if (csm_FIND_QUIETLY)
+    message(STATUS "Failed to find csm- " ${REASON_MSG} ${ARGN})
+  else (csm_FIND_REQUIRED)
+    message(FATAL_ERROR "Failed to find csm - " ${REASON_MSG} ${ARGN})
+  else()
+    # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
+    # that prevents generation, but continues configuration.
+    message(SEND_ERROR "Failed to find csm - " ${REASON_MSG} ${ARGN})
+  endif ()
+  return()
+endmacro(csm_report_not_found)
+
+if(NOT csm_FOUND)
+  csm_report_not_found("Something went wrong while setting up csm.")
+endif(NOT csm_FOUND)
+# Set the include directories for csm (itself).
+set(csm_FOUND TRUE)
\ No newline at end of file