diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2cb3520..7745c20 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -6,15 +6,22 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
 
-find_package(std_msgs REQUIRED)
+# Build with release optimizations and debug symbols by default
+if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE RelWithDebug)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(tf2_eigen REQUIRED)
-find_package(eigen3_cmake_module REQUIRED)
-find_package(Eigen3 REQUIRED)
-find_package(ament_cmake REQUIRED)
 find_package(visualization_msgs REQUIRED)
 
+find_package(Eigen3 REQUIRED)
+
 set(INCLUDE_DIRS include)
 include_directories(${INCLUDE_DIRS})
 
@@ -72,6 +79,7 @@ ament_target_dependencies(
   ${PROJECT_NAME}
   SYSTEM
   Eigen3
+  backward_ros
   geometry_msgs
   rclcpp
   std_msgs
diff --git a/package.xml b/package.xml
index ec24dba..d33a6f4 100644
--- a/package.xml
+++ b/package.xml
@@ -17,11 +17,12 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>backward_ros</depend>
+  <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
-  <depend>geometry_msgs</depend>
-  <depend>visualization_msgs</depend>
   <depend>tf2_eigen</depend>
+  <depend>visualization_msgs</depend>
 
   <export>
     <bitbots_documentation>