wip separating node from computation

CMakeLists.txt

@@ -52,8 +52,15 @@ include_directories(
   ${EIGEN_INCLUDE_DIRS}
 )
 
+## Declare a cpp library
+add_library(${PROJECT_NAME}
+    src/CLaserOdometry2D.cpp
+)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
 ## Declare a cpp executable
-add_executable(rf2o_laser_odometry_node src/CLaserOdometry2D.cpp)
+add_executable(rf2o_laser_odometry_node src/CLaserOdometry2DNode.cpp)
+target_link_libraries(rf2o_laser_odometry_node ${PROJECT_NAME})
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(rf2o_laser_odometry_node

include/rf2o_laser_odometry/CLaserOdometry2D.h

@@ -59,19 +59,13 @@ class CLaserOdometry2D
 public:
 	CLaserOdometry2D();
     ~CLaserOdometry2D();
+
+    void Init(const sensor_msgs::LaserScan& scan,
+              const geometry_msgs::Pose& initial_robot_pose);
+
     bool is_initialized();
-    bool scan_available();
-    void Init();
-    void odometryCalculation();
-
-    std::string         laser_scan_topic;
-    std::string         odom_topic;
-    bool                publish_tf;
-    std::string         base_frame_id;
-    std::string         odom_frame_id;
-    std::string         init_pose_from_topic;
-    double              freq;
 
+<<<<<<< HEAD
 protected:
     ros::NodeHandle             n;
     sensor_msgs::LaserScan      last_scan;
@@ -79,14 +73,15 @@ protected:
     tf::TransformListener       tf_listener;          //Do not put inside the callback
     tf::TransformBroadcaster    odom_broadcaster;
     nav_msgs::Odometry          initial_robot_pose;
+=======
+    void odometryCalculation(const sensor_msgs::LaserScan& scan);
+>>>>>>> wip separating node from computation
 
-    //Subscriptions & Publishers
+    void setLaserPose(const tf::Transform& laser_pose);
-    ros::Subscriber laser_sub, initPose_sub;
-    ros::Publisher odom_pub;
 
-    //CallBacks
+protected:
-    void LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan);
+
-    void initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose);
+  bool module_initialized,first_laser_scan;
 
     // Internal Data
 	std::vector<Eigen::MatrixXf> range;
@@ -130,17 +125,22 @@ protected:
     unsigned int iter_irls;
 	float g_mask[5];
 
+  double lin_speed, ang_speed;
+
     //mrpt::gui::CDisplayWindowPlots window;
 	mrpt::utils::CTicTac		m_clock;
 	float		m_runtime;
-    ros::Time last_odom_time;
+    ros::Time last_odom_time, current_scan_time;
 
 	mrpt::math::CMatrixFloat31 kai_abs;
 	mrpt::math::CMatrixFloat31 kai_loc;
 	mrpt::math::CMatrixFloat31 kai_loc_old;
 	mrpt::math::CMatrixFloat31 kai_loc_level;
 
-	mrpt::poses::CPose3D laser_pose;
+  mrpt::poses::CPose3D laser_pose_on_robot;
+  mrpt::poses::CPose3D laser_pose_on_robot_inv;
+
+  mrpt::poses::CPose3D laser_pose;
 	mrpt::poses::CPose3D laser_oldpose;
     mrpt::poses::CPose3D robot_pose;
     mrpt::poses::CPose3D robot_oldpose;

src/CLaserOdometry2D.cpp

@@ -28,105 +28,56 @@ using namespace Eigen;
 
 CLaserOdometry2D::CLaserOdometry2D()
 {
-    ROS_INFO("Initializing RF2O node...");
+  //
-
-    //Read Parameters
-    //----------------
-    ros::NodeHandle pn("~");
-    pn.param<std::string>("laser_scan_topic",laser_scan_topic,"/laser_scan");
-    pn.param<std::string>("odom_topic", odom_topic, "/odom_rf2o");
-    pn.param<std::string>("base_frame_id", base_frame_id, "/base_link");
-    pn.param<std::string>("odom_frame_id", odom_frame_id, "/odom");
-    pn.param<bool>("publish_tf", publish_tf, true);
-    pn.param<std::string>("init_pose_from_topic", init_pose_from_topic, "/base_pose_ground_truth");
-    pn.param<double>("freq",freq,10.0);
-    pn.param<bool>("verbose", verbose, true);
-
-    //Publishers and Subscribers
-    //--------------------------    
-    odom_pub = pn.advertise<nav_msgs::Odometry>(odom_topic, 5);
-    laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_scan_topic,1,&CLaserOdometry2D::LaserCallBack,this);
-
-    //init pose??
-    if (init_pose_from_topic != "")
-    {
-        initPose_sub = n.subscribe<nav_msgs::Odometry>(init_pose_from_topic,1,&CLaserOdometry2D::initPoseCallBack,this);
-        GT_pose_initialized  = false;
-    }
-    else
-    {
-        GT_pose_initialized = true;
-        initial_robot_pose.pose.pose.position.x = 0;
-        initial_robot_pose.pose.pose.position.y = 0;
-        initial_robot_pose.pose.pose.position.z = 0;
-        initial_robot_pose.pose.pose.orientation.w = 0;
-        initial_robot_pose.pose.pose.orientation.x = 0;
-        initial_robot_pose.pose.pose.orientation.y = 0;
-        initial_robot_pose.pose.pose.orientation.z = 0;
-    }
-
-    //Init variables
-    module_initialized = false;
-    first_laser_scan = true;
 }
 
 
 CLaserOdometry2D::~CLaserOdometry2D()
 {
+  //
 }
 
+void CLaserOdometry2D::setLaserPose(const tf::Transform& laser_pose)
+{
+  //Set laser pose on the robot
+
+  //TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge)
+  const tf::Vector3 &t = laser_pose.getOrigin();
+  laser_pose_on_robot.x() = t[0];
+  laser_pose_on_robot.y() = t[1];
+  laser_pose_on_robot.z() = t[2];
+  const tf::Matrix3x3 &basis = laser_pose.getBasis();
+  mrpt::math::CMatrixDouble33 R;
+  for(int r = 0; r < 3; r++)
+      for(int c = 0; c < 3; c++)
+          R(r,c) = basis[r][c];
+  laser_pose_on_robot.setRotationMatrix(R);
+
+  laser_pose_on_robot_inv = laser_pose_on_robot;
+  laser_pose_on_robot_inv.inverse();
+}
 
 bool CLaserOdometry2D::is_initialized()
 {
     return module_initialized;
 }
 
-bool CLaserOdometry2D::scan_available()
+void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan,
-{
+                            const geometry_msgs::Pose& initial_robot_pose)
-    return new_scan_available;
-}
-
-void CLaserOdometry2D::Init()
 {
     //Got an initial scan laser, obtain its parametes
     if (verbose)
         ROS_INFO("[rf2o] Got first Laser Scan .... Configuring node");
     width = last_scan.ranges.size();    // Num of samples (size) of the scan laser
+
     cols = width;						// Max reolution. Should be similar to the width parameter
-    fovh = fabs(last_scan.angle_max - last_scan.angle_min); // Horizontal Laser's FOV
+    fovh = fabs(scan.angle_max - scan.angle_min); // Horizontal Laser's FOV
     ctf_levels = 5;                     // Coarse-to-Fine levels
     iter_irls = 5;                      //Num iterations to solve iterative reweighted least squares
 
-    //Set laser pose on the robot (through tF)
-    // This allow estimation of the odometry with respect to the robot base reference system.
-    mrpt::poses::CPose3D LaserPoseOnTheRobot;
-    tf::StampedTransform transform;
-    try
-    {
-        tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform);
-    }
-    catch (tf::TransformException &ex)
-    {
-        ROS_ERROR("%s",ex.what());
-        ros::Duration(1.0).sleep();
-    }
-
-    //TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge)
-    const tf::Vector3 &t = transform.getOrigin();
-    LaserPoseOnTheRobot.x() = t[0];
-    LaserPoseOnTheRobot.y() = t[1];
-    LaserPoseOnTheRobot.z() = t[2];
-    const tf::Matrix3x3 &basis = transform.getBasis();
-    mrpt::math::CMatrixDouble33 R;
-    for(int r = 0; r < 3; r++)
-        for(int c = 0; c < 3; c++)
-            R(r,c) = basis[r][c];
-    LaserPoseOnTheRobot.setRotationMatrix(R);
-
-
     //Robot initial pose (see MQTT:bridge)
     mrpt::poses::CPose3D robotInitialPose;
-    geometry_msgs::Pose _src = initial_robot_pose.pose.pose;
+    geometry_msgs::Pose _src = initial_robot_pose;
 
     robotInitialPose.x(_src.position.x);
     robotInitialPose.y(_src.position.y);
@@ -141,13 +92,10 @@ void CLaserOdometry2D::Init()
     robotInitialPose.setYawPitchRoll(yaw,pitch,roll);
     //robotInitialPose.phi(yaw);
 
-
-
     //Set the initial pose
     laser_pose = robotInitialPose + LaserPoseOnTheRobot;
     laser_oldpose = robotInitialPose + LaserPoseOnTheRobot;
 
-
     // Init module (internal)
     //------------------------
     range_wf.setSize(1, width);
@@ -227,12 +175,16 @@ void CLaserOdometry2D::Init()
 }
 
 
-void CLaserOdometry2D::odometryCalculation()
+void CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
 {
     //==================================================================================
     //						DIFERENTIAL  ODOMETRY  MULTILEVEL
     //==================================================================================
 
+    //copy laser scan to internal variable
+    for (unsigned int i = 0; i<width; i++)
+        range_wf(i) = scan.ranges[i];
+
     m_clock.Tic();
     createImagePyramid();
 
@@ -289,7 +241,6 @@ void CLaserOdometry2D::odometryCalculation()
 
     //Update poses
     PoseUpdate();
-    new_scan_available = false;     //avoids the possibility to run twice on the same laser scan
 }
 
 
@@ -958,52 +909,25 @@ void CLaserOdometry2D::PoseUpdate()
         ROS_INFO("[rf2o] LASERodom = [%f %f %f]",laser_pose.x(),laser_pose.y(),laser_pose.yaw());
 
 
-    // GET ROBOT POSE from LASER POSE
-    //------------------------------  
-    mrpt::poses::CPose3D LaserPoseOnTheRobot_inv;
-    tf::StampedTransform transform;
-    try
-    {
-        tf_listener.lookupTransform(last_scan.header.frame_id, base_frame_id, ros::Time(0), transform);
-    }
-    catch (tf::TransformException &ex)
-    {
-        ROS_ERROR("%s",ex.what());
-        ros::Duration(1.0).sleep();
-    }
-
-    //TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge)
-    const tf::Vector3 &t = transform.getOrigin();
-    LaserPoseOnTheRobot_inv.x() = t[0];
-    LaserPoseOnTheRobot_inv.y() = t[1];
-    LaserPoseOnTheRobot_inv.z() = t[2];
-    const tf::Matrix3x3 &basis = transform.getBasis();
-    mrpt::math::CMatrixDouble33 R;
-    for(int r = 0; r < 3; r++)
-        for(int c = 0; c < 3; c++)
-            R(r,c) = basis[r][c];
-    LaserPoseOnTheRobot_inv.setRotationMatrix(R);
-
     //Compose Transformations
     robot_pose = laser_pose + LaserPoseOnTheRobot_inv;
     if (verbose)
         ROS_INFO("BASEodom = [%f %f %f]",robot_pose.x(),robot_pose.y(),robot_pose.yaw());
 
-
     // Estimate linear/angular speeds (mandatory for base_local_planner)
     // last_scan -> the last scan received
     // last_odom_time -> The time of the previous scan lasser used to estimate the pose
     //-------------------------------------------------------------------------------------
-    double time_inc_sec = (last_scan.header.stamp - last_odom_time).toSec();
+    double time_inc_sec = (current_scan_time - last_odom_time).toSec();
-    last_odom_time = last_scan.header.stamp;
+    last_odom_time = current_scan_time;
-    double lin_speed = acu_trans(0,2) / time_inc_sec;
+    lin_speed = acu_trans(0,2) / time_inc_sec;
     //double lin_speed = sqrt( square(robot_oldpose.x()-robot_pose.x()) + square(robot_oldpose.y()-robot_pose.y()) )/time_inc_sec;
     double ang_inc = robot_pose.yaw() - robot_oldpose.yaw();
     if (ang_inc > 3.14159)
         ang_inc -= 2*3.14159;
     if (ang_inc < -3.14159)
         ang_inc += 2*3.14159;
-    double ang_speed = ang_inc/time_inc_sec;
+    ang_speed = ang_inc/time_inc_sec;
     robot_oldpose = robot_pose;
 
     //filter speeds
@@ -1020,112 +944,4 @@ void CLaserOdometry2D::PoseUpdate()
     double sum2 = std::accumulate(last_m_ang_speeds.begin(), last_m_ang_speeds.end(), 0.0);
     ang_speed = sum2 / last_m_ang_speeds.size();
     */
-
-    //first, we'll publish the odometry over tf
-    //---------------------------------------
-    if (publish_tf)
-    {
-        //ROS_INFO("[rf2o] Publishing TF: [base_link] to [odom]");
-        geometry_msgs::TransformStamped odom_trans;
-        odom_trans.header.stamp = ros::Time::now();
-        odom_trans.header.frame_id = odom_frame_id;
-        odom_trans.child_frame_id = base_frame_id;
-        odom_trans.transform.translation.x = robot_pose.x();
-        odom_trans.transform.translation.y = robot_pose.y();
-        odom_trans.transform.translation.z = 0.0;
-        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(robot_pose.yaw());
-        //send the transform
-        odom_broadcaster.sendTransform(odom_trans);
-    }
-
-    //next, we'll publish the odometry message over ROS
-    //-------------------------------------------------
-    //ROS_INFO("[rf2o] Publishing Odom Topic");
-    nav_msgs::Odometry odom;
-    odom.header.stamp = ros::Time::now();
-    odom.header.frame_id = odom_frame_id;
-    //set the position
-    odom.pose.pose.position.x = robot_pose.x();
-    odom.pose.pose.position.y = robot_pose.y();
-    odom.pose.pose.position.z = 0.0;
-    odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(robot_pose.yaw());
-    //set the velocity
-    odom.child_frame_id = base_frame_id;
-    odom.twist.twist.linear.x = lin_speed;    //linear speed
-    odom.twist.twist.linear.y = 0.0;
-    odom.twist.twist.angular.z = ang_speed;   //angular speed
-    //publish the message
-    odom_pub.publish(odom);
-}
-
-
-
-//-----------------------------------------------------------------------------------
-//                                   CALLBACKS
-//-----------------------------------------------------------------------------------
-
-void CLaserOdometry2D::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan)
-{
-    if (GT_pose_initialized)
-    {
-        //Keep in memory the last received laser_scan
-        last_scan = *new_scan;
-
-        //Initialize module on first scan
-        if (first_laser_scan)
-        {
-            Init();
-            first_laser_scan = false;
-        }
-        else
-        {
-            //copy laser scan to internal variable
-            for (unsigned int i = 0; i<width; i++)
-                range_wf(i) = new_scan->ranges[i];
-            new_scan_available = true;
-        }
-    }
-}
-
-
-void CLaserOdometry2D::initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose)
-{
-    //Initialize module on first GT pose. Else do Nothing!
-    if (!GT_pose_initialized)
-    {
-        initial_robot_pose = *new_initPose;
-        GT_pose_initialized = true;
-    }
-}
-
-//-----------------------------------------------------------------------------------
-//                                   MAIN
-//-----------------------------------------------------------------------------------
-int main(int argc, char** argv)
-{
-    ros::init(argc, argv, "RF2O_LaserOdom");
-
-    CLaserOdometry2D myLaserOdom;
-
-    //Main Loop
-    //----------
-    ROS_INFO("[rf2o] initialization complete...Looping");
-    ros::Rate loop_rate(myLaserOdom.freq);
-    while (ros::ok())
-    {
-        ros::spinOnce();        //Check for new laser scans
-
-        if( myLaserOdom.is_initialized() && myLaserOdom.scan_available() )
-        {            
-            //Process odometry estimation
-            myLaserOdom.odometryCalculation();
-        }
-        else
-        {
-            ROS_WARN("[rf2o] Waiting for laser_scans....") ;
-        }
-
-        loop_rate.sleep();
-    }
-    return(0);
 }

src/CLaserOdometry2DNode.cpp

@@ -0,0 +1,261 @@
+/** ****************************************************************************************
+*  This node presents a fast and precise method to estimate the planar motion of a lidar
+*  from consecutive range scans. It is very useful for the estimation of the robot odometry from
+*  2D laser range measurements.
+*  This module is developed for mobile robots with innacurate or inexistent built-in odometry.
+*  It allows the estimation of a precise odometry with low computational cost.
+*  For more information, please refer to:
+*
+*  Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA'16.
+*  Available at: http://mapir.isa.uma.es/mapirwebsite/index.php/mapir-downloads/papers/217
+*
+* Maintainer: Javier G. Monroy
+* MAPIR group: http://mapir.isa.uma.es/
+******************************************************************************************** */
+
+#include "rf2o_laser_odometry/CLaserOdometry2D.h"
+using namespace mrpt;
+using namespace mrpt::math;
+using namespace mrpt::utils;
+using namespace mrpt::poses;
+using namespace std;
+using namespace Eigen;
+
+class CLaserOdometry2DNode : CLaserOdometry2D
+{
+public:
+
+  CLaserOdometry2DNode();
+  ~CLaserOdometry2DNode();
+
+  void process();
+  void publish();
+
+public:
+
+  bool verbose,publish_tf,new_scan_available;
+
+  double freq;
+
+  std::string         laser_scan_topic;
+  std::string         odom_topic;
+  std::string         base_frame_id;
+  std::string         odom_frame_id;
+  std::string         init_pose_from_topic;
+
+  ros::NodeHandle             n;
+  sensor_msgs::LaserScan      last_scan;
+  bool                        GT_pose_initialized;
+  tf::TransformListener       tf_listener;          //Do not put inside the callback
+  tf::TransformBroadcaster    odom_broadcaster;
+  nav_msgs::Odometry          initial_robot_pose;
+
+  //Subscriptions & Publishers
+  ros::Subscriber laser_sub, initPose_sub;
+  ros::Publisher odom_pub;
+
+  bool scan_available();
+
+  //CallBacks
+  void LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan);
+  void initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose);
+};
+
+CLaserOdometry2DNode::CLaserOdometry2DNode() :
+  CLaserOdometry2D()
+{
+    ROS_INFO("Initializing RF2O node...");
+
+    //Read Parameters
+    //----------------
+    ros::NodeHandle pn("~");
+    pn.param<std::string>("laser_scan_topic",laser_scan_topic,"/laser_scan");
+    pn.param<std::string>("odom_topic", odom_topic, "/odom_rf2o");
+    pn.param<std::string>("base_frame_id", base_frame_id, "/base_link");
+    pn.param<std::string>("odom_frame_id", odom_frame_id, "/odom");
+    pn.param<bool>("publish_tf", publish_tf, true);
+    pn.param<std::string>("init_pose_from_topic", init_pose_from_topic, "/base_pose_ground_truth");
+    pn.param<double>("freq",freq,10.0);
+    pn.param<bool>("verbose", verbose, true);
+
+    //Publishers and Subscribers
+    //--------------------------
+    odom_pub = pn.advertise<nav_msgs::Odometry>(odom_topic, 5);
+    laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_scan_topic,1,&CLaserOdometry2DNode::LaserCallBack,this);
+
+    //init pose??
+    if (init_pose_from_topic != "")
+    {
+        initPose_sub = n.subscribe<nav_msgs::Odometry>(init_pose_from_topic,1,&CLaserOdometry2DNode::initPoseCallBack,this);
+        GT_pose_initialized  = false;
+    }
+    else
+    {
+        GT_pose_initialized = true;
+        initial_robot_pose.pose.pose.position.x = 0;
+        initial_robot_pose.pose.pose.position.y = 0;
+        initial_robot_pose.pose.pose.position.z = 0;
+        initial_robot_pose.pose.pose.orientation.w = 0;
+        initial_robot_pose.pose.pose.orientation.x = 0;
+        initial_robot_pose.pose.pose.orientation.y = 0;
+        initial_robot_pose.pose.pose.orientation.z = 0;
+    }
+
+    // Set laser pose on the robot (through tF)
+    // This allow estimation of the odometry with respect to the robot base reference system.
+    tf::StampedTransform transform;
+    transform.setIdentity();
+    try
+    {
+        tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform);
+    }
+    catch (tf::TransformException &ex)
+    {
+        ROS_ERROR("%s",ex.what());
+        ros::Duration(1.0).sleep();
+    }
+
+    setLaserPose(transform);
+
+    //Init variables
+    module_initialized = false;
+    first_laser_scan = true;
+}
+
+CLaserOdometry2DNode::~CLaserOdometry2DNode()
+{
+  //
+}
+
+bool CLaserOdometry2DNode::scan_available()
+{
+    return new_scan_available;
+}
+
+void CLaserOdometry2DNode::process()
+{
+  if( is_initialized() && scan_available() )
+  {
+      //Process odometry estimation
+      odometryCalculation(last_scan);
+      publish();
+      new_scan_available = false; //avoids the possibility to run twice on the same laser scan
+  }
+  else
+  {
+      ROS_WARN("Waiting for laser_scans....") ;
+  }
+}
+
+//-----------------------------------------------------------------------------------
+//                                   CALLBACKS
+//-----------------------------------------------------------------------------------
+
+void CLaserOdometry2DNode::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan)
+{
+    if (GT_pose_initialized)
+    {
+        //Keep in memory the last received laser_scan
+        last_scan = *new_scan;
+        current_scan_time = last_scan.header.stamp;
+
+        //Initialize module on first scan
+        if (first_laser_scan)
+        {
+            Init(last_scan, initial_robot_pose.pose.pose);
+            first_laser_scan = false;
+        }
+        else
+        {
+            //copy laser scan to internal variable
+            for (unsigned int i = 0; i<width; i++)
+                range_wf(i) = new_scan->ranges[i];
+            new_scan_available = true;
+        }
+    }
+}
+
+void CLaserOdometry2DNode::initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose)
+{
+    //Initialize module on first GT pose. Else do Nothing!
+    if (!GT_pose_initialized)
+    {
+        initial_robot_pose = *new_initPose;
+        GT_pose_initialized = true;
+    }
+}
+
+void CLaserOdometry2DNode::publish()
+{
+  //first, we'll publish the odometry over tf
+  //---------------------------------------
+  if (publish_tf)
+  {
+      //ROS_INFO("[rf2o] Publishing TF: [base_link] to [odom]");
+      geometry_msgs::TransformStamped odom_trans;
+      odom_trans.header.stamp = ros::Time::now();
+      odom_trans.header.frame_id = odom_frame_id;
+      odom_trans.child_frame_id = base_frame_id;
+      odom_trans.transform.translation.x = robot_pose.x();
+      odom_trans.transform.translation.y = robot_pose.y();
+      odom_trans.transform.translation.z = 0.0;
+      odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(robot_pose.yaw());
+      //send the transform
+      odom_broadcaster.sendTransform(odom_trans);
+  }
+
+  //next, we'll publish the odometry message over ROS
+  //-------------------------------------------------
+  //ROS_INFO("[rf2o] Publishing Odom Topic");
+  nav_msgs::Odometry odom;
+  odom.header.stamp = ros::Time::now();
+  odom.header.frame_id = odom_frame_id;
+  //set the position
+  odom.pose.pose.position.x = robot_pose.x();
+  odom.pose.pose.position.y = robot_pose.y();
+  odom.pose.pose.position.z = 0.0;
+  odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(robot_pose.yaw());
+  //set the velocity
+  odom.child_frame_id = base_frame_id;
+  odom.twist.twist.linear.x = lin_speed;    //linear speed
+  odom.twist.twist.linear.y = 0.0;
+  odom.twist.twist.angular.z = ang_speed;   //angular speed
+  //publish the message
+  odom_pub.publish(odom);
+}
+
+//void CLaserOdometry2D::Reset(CPose3D ini_pose, CObservation2DRangeScan scan)
+//{
+//	//Set the initial pose
+//	laser_pose = ini_pose;
+//	laser_oldpose = ini_pose;
+
+//    //readLaser(scan);
+//	createImagePyramid();
+//    //readLaser(scan);
+//	createImagePyramid();
+//}
+
+//-----------------------------------------------------------------------------------
+//                                   MAIN
+//-----------------------------------------------------------------------------------
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "RF2O_LaserOdom");
+
+    CLaserOdometry2DNode myLaserOdomNode;
+
+    //Main Loop
+    //----------
+    ROS_INFO("initialization complete...Looping");
+    ros::Rate loop_rate(myLaserOdomNode.freq);
+    while (ros::ok())
+    {
+        ros::spinOnce();        //Check for new laser scans
+
+        myLaserOdomNode.process();
+
+        loop_rate.sleep();
+    }
+    return(0);
+}