diff --git a/ros_wrapping/lidar_slam/CMakeLists.txt b/ros_wrapping/lidar_slam/CMakeLists.txt
index 0ec4afa3316f5ccda9f37306b01f66fe71e93dc3..d0bb9441657eba45ce4d5ce22a782c9b6a7e69d3 100644
--- a/ros_wrapping/lidar_slam/CMakeLists.txt
+++ b/ros_wrapping/lidar_slam/CMakeLists.txt
@@ -145,7 +145,10 @@ include_directories(
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-add_executable(lidar_slam_node src/LidarSlamNode.cxx)
+add_executable(lidar_slam_node 
+src/LidarSlamNode.cxx 
+src/LidarSlamNode_main.cxx
+)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
diff --git a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
index 645d01c112263f2e96b53f684231c379d97c6253..307ff86f0c216a3cec24118078b6baef56ab2da7 100644
--- a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
+++ b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
@@ -684,23 +684,4 @@ void LidarSlamNode::SetSlamParameters(ros::NodeHandle& priv_nh)
   SetKeypointsExtractorParam(double, "slam/ke/edge_depth_gap_threshold", EdgeDepthGapThreshold)
   SetKeypointsExtractorParam(double, "slam/ke/edge_saliency_threshold", EdgeSaliencyThreshold)
   SetKeypointsExtractorParam(double, "slam/ke/edge_intensity_gap_threshold", EdgeIntensityGapThreshold)
-}
-
-//------------------------------------------------------------------------------
-/*!
- * @brief Main node entry point.
- */
-int main(int argc, char **argv)
-{
-  ros::init(argc, argv, "lidar_slam");
-  ros::NodeHandle nh;
-  ros::NodeHandle priv_nh("~");
-
-  // create lidar slam node, which subscribes to pointclouds
-  LidarSlamNode slam(nh, priv_nh);
-
-  // handle callbacks until shut down
-  ros::spin();
-
-  return 0;
 }
\ No newline at end of file
diff --git a/ros_wrapping/lidar_slam/src/LidarSlamNode.h b/ros_wrapping/lidar_slam/src/LidarSlamNode.h
index 0656057ea77cf0b70fab93fa2b9ca38ba3587643..928a0f9c3cc3a3cbf7741f421630e17d6079a3be 100644
--- a/ros_wrapping/lidar_slam/src/LidarSlamNode.h
+++ b/ros_wrapping/lidar_slam/src/LidarSlamNode.h
@@ -54,7 +54,7 @@ public:
    * @brief     New lidar frame callback, running SLAM and publishing TF.
    * @param[in] cloud New Lidar Frame, published by velodyne_pointcloud/cloud_node.
    */
-  void ScanCallback(const CloudV& cloud);
+  virtual void ScanCallback(const CloudV& cloud);
 
   //----------------------------------------------------------------------------
   /*!
@@ -71,7 +71,7 @@ public:
    */
   void SlamCommandCallback(const lidar_slam::SlamCommand& msg);
 
-private:
+protected:
 
   //----------------------------------------------------------------------------
   /*!
diff --git a/ros_wrapping/lidar_slam/src/LidarSlamNode_main.cxx b/ros_wrapping/lidar_slam/src/LidarSlamNode_main.cxx
new file mode 100644
index 0000000000000000000000000000000000000000..b942f51758e5411a4a45bf04df9b1dec5dab0a46
--- /dev/null
+++ b/ros_wrapping/lidar_slam/src/LidarSlamNode_main.cxx
@@ -0,0 +1,20 @@
+#include "LidarSlamNode.h"
+
+//------------------------------------------------------------------------------
+/*!
+ * @brief Main node entry point.
+ */
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "lidar_slam");
+  ros::NodeHandle nh;
+  ros::NodeHandle priv_nh("~");
+
+  // Create lidar slam node, which subscribes to pointclouds coming from conversion node
+  LidarSlamNode slam(nh, priv_nh);
+
+  // handle callbacks until shut down
+  ros::spin();
+
+  return 0;
+}
\ No newline at end of file