diff --git a/CMakeLists.txt b/CMakeLists.txt
index 79190a0ef8244b5dc0064a8433c0f48fb00824c9..5220bb8b4d89f7eb35f6e6ee65b7d3ad3a0ae955 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,11 +2,11 @@ cmake_minimum_required(VERSION 2.8.3)
 project(iri_lidar_lite_driver)
 
 ## Find catkin macros and libraries
-find_package(catkin REQUIRED)
+#find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_driver roscpp sensor_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_driver roscpp roslib sensor_msgs)
 
 ## System dependencies are found with CMake's conventions
 find_package(Boost REQUIRED COMPONENTS thread)
@@ -14,8 +14,8 @@ find_package(Boost REQUIRED COMPONENTS thread)
 # ******************************************************************** 
 #           Add system and labrobotica dependencies here
 # ******************************************************************** 
-find_package(usb_i2c_adapter REQUIRED)
 find_package(lidar_lite REQUIRED)
+find_package(usb_i2c_adapter REQUIRED)
 
 # ******************************************************************** 
 #           Add topic, service and action definition here
@@ -61,22 +61,37 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_driver roscpp sensor_msgs
+ CATKIN_DEPENDS iri_base_driver roscpp roslib sensor_msgs
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
- DEPENDS 
+ DEPENDS Boost usb_i2c_adapter lidar_lite
 )
 
 ###########
 ## Build ##
 ###########
 
+#Set compiler according C++11 support
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+if(COMPILER_SUPPORTS_CXX11)
+		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+elseif(COMPILER_SUPPORTS_CXX0X)
+		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+else()
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+endif()
+
 # ******************************************************************** 
 #                   Add the include directories 
 # ******************************************************************** 
 include_directories(include)
 include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${Boost_INCLUDE_DIRS})
 include_directories(${lidar_lite_INCLUDE_DIR})
 include_directories(${usb_i2c_adapter_INCLUDE_DIR})
 # include_directories(${<dependency>_INCLUDE_DIR})
@@ -91,6 +106,7 @@ add_executable(${PROJECT_NAME} src/lidar_lite_driver.cpp src/lidar_lite_driver_n
 #                   Add the libraries
 # ******************************************************************** 
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${Boost_SYSTEM_LIBRARY})
 target_link_libraries(${PROJECT_NAME} ${lidar_lite_LIBRARY})
 target_link_libraries(${PROJECT_NAME} ${usb_i2c_adapter_LIBRARY})
 # target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
diff --git a/cfg/LidarLiteDriver.cfg b/cfg/LidarLiteDriver.cfg
index 5ad6986698f82542b9d2249f25b86f985e15b6e8..a82728a635031435b1b0d1cb9acecdd87306c37b 100755
--- a/cfg/LidarLiteDriver.cfg
+++ b/cfg/LidarLiteDriver.cfg
@@ -42,4 +42,4 @@ gen = ParameterGenerator()
 #gen.add("velocity_scale_factor",  double_t,  SensorLevels.RECONFIGURE_STOP,   "Maximum velocity scale factor",  0.5,      0.0,  1.0)
 gen.add("Config_mode", int_t, 0, "LidarLite configuration mode", 0, 1, 5)
 
-exit(gen.generate(PACKAGE, "LidarLiteDriver", "LidarLite"))
+exit(gen.generate(PACKAGE, "LidarLiteDriver", "LidarLiteDriver"))
diff --git a/package.xml b/package.xml
index 66990d94343a0927289a3ae4788214c93b3b05eb..317f38497da4fa6199b98bc571d38fede0e6cca5 100644
--- a/package.xml
+++ b/package.xml
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package>
+<package format="1">
   <name>iri_lidar_lite_driver</name>
   <version>0.0.0</version>
   <description>The iri_lidar_lite_driver package</description>
@@ -40,14 +40,17 @@
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>roslib</build_depend>
   <build_depend>iri_base_driver</build_depend>
   <build_depend>sensor_msgs</build_depend>
-  <build_depend>roscpp</build_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>roslib</run_depend>
   <run_depend>iri_base_driver</run_depend>
   <run_depend>sensor_msgs</run_depend>
-  <run_depend>roscpp</run_depend>
-
-
+  
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->