diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt
index f978782..6e9e1ce 100644
--- a/cartographer_ros/CMakeLists.txt
+++ b/cartographer_ros/CMakeLists.txt
@@ -28,7 +28,23 @@ set(PACKAGE_DEPENDENCIES
   tf2_eigen
 )
 
-set(CMAKE_CXX_FLAGS "-std=c++11 -Wreorder")
+set(CMAKE_CXX_FLAGS "-pthread -std=c++11 -Wreorder")
+
+if(NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "")
+  set(CMAKE_BUILD_TYPE Release)
+endif()
+
+if(CMAKE_BUILD_TYPE STREQUAL "Release")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -DNDEBUG")
+elseif(CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -g -DNDEBUG")
+elseif(CMAKE_BUILD_TYPE STREQUAL "Debug")
+  message(FATAL_ERROR "Cartographer is too slow to be useful in debug mode.")
+else()
+  message(FATAL_ERROR "Unknown CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}")
+endif()
+
+message(STATUS "Build type: ${CMAKE_BUILD_TYPE}")
 
 find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
 include_directories(${catkin_INCLUDE_DIRS})
@@ -77,15 +93,15 @@ link_directories(${Boost_LIBRARY_DIRS})
 
 add_executable(cartographer_node
   src/cartographer_node_main.cc
-  src/node_constants.h
-  src/msg_conversion.h
   src/msg_conversion.cc
-  src/time_conversion.h
+  src/msg_conversion.h
+  src/node_constants.h
   src/sensor_data.cc
   src/sensor_data.h
   src/sensor_data_producer.cc
   src/sensor_data_producer.h
   src/time_conversion.cc
+  src/time_conversion.h
 )
 target_link_libraries(cartographer_node
   ${CARTOGRAPHER_LIBRARIES}
diff --git a/cartographer_ros/launch/backpack_2d.launch b/cartographer_ros/launch/backpack_2d.launch
index ab332cb..41d8674 100644
--- a/cartographer_ros/launch/backpack_2d.launch
+++ b/cartographer_ros/launch/backpack_2d.launch
@@ -25,7 +25,7 @@
       type="cartographer_node" args="
           -configuration_directory $(find cartographer_ros)/configuration_files
           -configuration_basename backpack_2d.lua"
-      output="screen" >
+      output="screen">
     <remap from="echoes" to="horizontal_laser_2d" />
   </node>
 
diff --git a/cartographer_ros/launch/backpack_3d.launch b/cartographer_ros/launch/backpack_3d.launch
index d1c2fcc..87fb752 100644
--- a/cartographer_ros/launch/backpack_3d.launch
+++ b/cartographer_ros/launch/backpack_3d.launch
@@ -25,7 +25,7 @@
       type="cartographer_node" args="
           -configuration_directory $(find cartographer_ros)/configuration_files
           -configuration_basename backpack_3d.lua"
-      output="screen" >
+      output="screen">
     <remap from="points2_1" to="horizontal_laser_3d" />
     <remap from="points2_2" to="vertical_laser_3d" />
   </node>
diff --git a/cartographer_ros/launch/turtlebot.launch b/cartographer_ros/launch/turtlebot.launch
index 0174c7e..194fe35 100644
--- a/cartographer_ros/launch/turtlebot.launch
+++ b/cartographer_ros/launch/turtlebot.launch
@@ -25,7 +25,7 @@
       type="cartographer_node" args="
           -configuration_directory $(find cartographer_ros)/configuration_files
           -configuration_basename turtlebot.lua"
-      output="screen" >
+      output="screen">
     <remap from="scan" to="horizontal_laser_2d" />
   </node>
 
diff --git a/cartographer_ros_msgs/CMakeLists.txt b/cartographer_ros_msgs/CMakeLists.txt
index 251228d..1c458a3 100644
--- a/cartographer_ros_msgs/CMakeLists.txt
+++ b/cartographer_ros_msgs/CMakeLists.txt
@@ -13,8 +13,27 @@
 # limitations under the License.
 
 cmake_minimum_required(VERSION 2.8.3)
+
 project(cartographer_ros_msgs)
-set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}")
+
+set(CMAKE_CXX_FLAGS "-pthread -std=c++11 ${CMAKE_CXX_FLAGS}")
+
+if(NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "")
+  set(CMAKE_BUILD_TYPE Release)
+endif()
+
+if(CMAKE_BUILD_TYPE STREQUAL "Release")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -DNDEBUG")
+elseif(CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -g -DNDEBUG")
+elseif(CMAKE_BUILD_TYPE STREQUAL "Debug")
+  message(FATAL_ERROR "Cartographer is too slow to be useful in debug mode.")
+else()
+  message(FATAL_ERROR "Unknown CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}")
+endif()
+
+message(STATUS "Build type: ${CMAKE_BUILD_TYPE}")
+
 find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation)
 
 add_message_files(