Add include_directories(${catkin_INCLUDE_DIRS}) to fix compilation error