From 5893d1a3e906254ea17893119eb68f9871c38938 Mon Sep 17 00:00:00 2001
From: Jeremie Deray <jeremie.deray@pal-robotics.com>
Date: Tue, 16 May 2017 12:56:24 +0200
Subject: [PATCH] moving headers & setLaserPose with CPose3D arg

---
 .../rf2o_laser_odometry/CLaserOdometry2D.h    |  5 +-
 src/CLaserOdometry2D.cpp                      | 15 +----
 src/CLaserOdometry2DNode.cpp                  | 59 ++++++++++++++-----
 3 files changed, 47 insertions(+), 32 deletions(-)

diff --git a/include/rf2o_laser_odometry/CLaserOdometry2D.h b/include/rf2o_laser_odometry/CLaserOdometry2D.h
index a2b2436..4f70b5b 100644
--- a/include/rf2o_laser_odometry/CLaserOdometry2D.h
+++ b/include/rf2o_laser_odometry/CLaserOdometry2D.h
@@ -17,11 +17,8 @@
 #define CLaserOdometry2D_H
 
 #include <ros/ros.h>
-#include <tf/transform_broadcaster.h>
-#include <tf/transform_listener.h>
 #include <nav_msgs/Odometry.h>
 #include <sensor_msgs/LaserScan.h>
-#include <geometry_msgs/Twist.h>
 
 // MRPT related headers
 #include <mrpt/version.h>
@@ -77,7 +74,7 @@ protected:
     void odometryCalculation(const sensor_msgs::LaserScan& scan);
 >>>>>>> wip separating node from computation
 
-    void setLaserPose(const tf::Transform& laser_pose);
+    void setLaserPose(const mrpt::poses::CPose3D& laser_pose);
 
     mrpt::poses::CPose3D& getIncrement();
     const mrpt::poses::CPose3D& getIncrement() const;
diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp
index 472fd98..5d3d0a0 100644
--- a/src/CLaserOdometry2D.cpp
+++ b/src/CLaserOdometry2D.cpp
@@ -32,22 +32,11 @@ CLaserOdometry2D::~CLaserOdometry2D()
   //
 }
 
-void CLaserOdometry2D::setLaserPose(const tf::Transform& laser_pose)
+void CLaserOdometry2D::setLaserPose(const mrpt::poses::CPose3D& 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 = laser_pose;
   laser_pose_on_robot_inv = laser_pose_on_robot;
   laser_pose_on_robot_inv.inverse();
 }
diff --git a/src/CLaserOdometry2DNode.cpp b/src/CLaserOdometry2DNode.cpp
index f1be914..f3b5ccb 100644
--- a/src/CLaserOdometry2DNode.cpp
+++ b/src/CLaserOdometry2DNode.cpp
@@ -15,6 +15,9 @@
 
 #include "rf2o_laser_odometry/CLaserOdometry2D.h"
 
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+
 class CLaserOdometry2DNode : CLaserOdometry2D
 {
 public:
@@ -25,6 +28,8 @@ public:
   void process();
   void publish();
 
+  bool setLaserPoseFromTf();
+
 public:
 
   bool verbose,publish_tf,new_scan_available;
@@ -95,21 +100,7 @@ CLaserOdometry2DNode::CLaserOdometry2DNode() :
         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);
+    setLaserPoseFromTf();
 
     //Init variables
     module_initialized = false;
@@ -121,6 +112,44 @@ CLaserOdometry2DNode::~CLaserOdometry2DNode()
   //
 }
 
+bool CLaserOdometry2DNode::setLaserPoseFromTf()
+{
+  bool retrieved = false;
+
+  // 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);
+      retrieved = true;
+  }
+  catch (tf::TransformException &ex)
+  {
+      ROS_ERROR("%s",ex.what());
+      ros::Duration(1.0).sleep();
+      retrieved = false;
+  }
+
+  //TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge)
+  mrpt::poses::CPose3D laser_tf;
+  const tf::Vector3 &t = transform.getOrigin();
+  laser_tf.x() = t[0];
+  laser_tf.y() = t[1];
+  laser_tf.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];
+  laser_tf.setRotationMatrix(R);
+
+  setLaserPose(laser_tf);
+
+  return retrieved;
+}
+
 bool CLaserOdometry2DNode::scan_available()
 {
     return new_scan_available;