From c82e6d1a8090b1f7f4587126963a1b3d3beed982 Mon Sep 17 00:00:00 2001
From: milesial <milesial@users.noreply.github.com>
Date: Sat, 21 Apr 2018 22:40:40 +0200
Subject: [PATCH] Lidar can now be in any orientation, including updside down

---
 CMakeLists.txt                                |  4 +--
 .../obstacle_extractor.h                      |  4 +++
 package.xml                                   |  2 ++
 src/obstacle_extractor.cpp                    | 29 +++++++++++++++----
 4 files changed, 31 insertions(+), 8 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index de42702..b86a922 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,7 +3,7 @@ project(processing_lidar_objects)
 
 set(CMAKE_CXX_FLAGS "-std=c++11 -fpermissive ${CMAKE_CXX_FLAGS} -Wfatal-errors\ ")
 
-find_package(catkin REQUIRED COMPONENTS roscpp roslaunch nodelet rviz std_msgs std_srvs geometry_msgs sensor_msgs tf laser_geometry message_generation)
+find_package(catkin REQUIRED COMPONENTS roscpp roslaunch nodelet rviz std_msgs std_srvs geometry_msgs sensor_msgs tf tf2 tf2_geometry_msgs laser_geometry message_generation)
 find_package(Armadillo REQUIRED)
 find_package(Boost 1.54.0 REQUIRED system)
 
@@ -13,7 +13,7 @@ generate_messages(DEPENDENCIES std_msgs geometry_msgs)
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES scans_merger obstacle_extractor obstacle_tracker obstacle_publisher ${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gui
-  CATKIN_DEPENDS roscpp nodelet rviz std_msgs std_srvs geometry_msgs sensor_msgs tf laser_geometry message_runtime
+  CATKIN_DEPENDS roscpp nodelet rviz std_msgs std_srvs geometry_msgs sensor_msgs tf tf2 tf2_geometry_msgs laser_geometry message_runtime
 )
 
 include_directories(include ${catkin_INCLUDE_DIRS} ${ARMADILLO_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
diff --git a/include/processing_lidar_objects/obstacle_extractor.h b/include/processing_lidar_objects/obstacle_extractor.h
index 85233cd..e16750c 100644
--- a/include/processing_lidar_objects/obstacle_extractor.h
+++ b/include/processing_lidar_objects/obstacle_extractor.h
@@ -41,6 +41,10 @@
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/PointCloud.h>
 #include <processing_lidar_objects/Obstacles.h>
+#include <geometry_msgs/Point.h>
+#include <tf2/convert.h>
+#include <tf/transform_datatypes.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 #include "processing_lidar_objects/utilities/point.h"
 #include "processing_lidar_objects/utilities/segment.h"
diff --git a/package.xml b/package.xml
index 48e4c1a..66497d5 100644
--- a/package.xml
+++ b/package.xml
@@ -12,6 +12,8 @@
   <exec_depend>message_runtime</exec_depend>
 
   <depend>tf</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>rviz</depend>
   <depend>roscpp</depend>
   <depend>roslaunch</depend>
diff --git a/src/obstacle_extractor.cpp b/src/obstacle_extractor.cpp
index 5b20b95..b7d689a 100644
--- a/src/obstacle_extractor.cpp
+++ b/src/obstacle_extractor.cpp
@@ -37,6 +37,7 @@
 #include "processing_lidar_objects/utilities/figure_fitting.h"
 #include "processing_lidar_objects/utilities/math_utilities.h"
 
+
 using namespace std;
 using namespace processing_lidar_objects;
 
@@ -408,26 +409,42 @@ void ObstacleExtractor::publishObstacles() {
 
   if (p_transform_coordinates_) {
     tf::StampedTransform transform;
+    geometry_msgs::TransformStamped geom_transform;
 
     try {
       tf_listener_.waitForTransform(p_frame_id_, base_frame_id_, stamp_, ros::Duration(0.1));
       tf_listener_.lookupTransform(p_frame_id_, base_frame_id_, stamp_, transform);
+      tf::transformStampedTFToMsg(transform, geom_transform);
     }
     catch (tf::TransformException& ex) {
       ROS_INFO_STREAM(ex.what());
       return;
     }
 
-    tf::Vector3 origin = transform.getOrigin();
-    double theta = tf::getYaw(transform.getRotation());
+    geometry_msgs::PointStamped geom_point;
+    geom_point.header = obstacles_msg->header;
 
     for (Segment& s : segments_) {
-      s.first_point = transformPoint(s.first_point, origin.x(), origin.y(), theta);
-      s.last_point = transformPoint(s.last_point, origin.x(), origin.y(), theta);
+      geom_point.point.x = s.first_point.x;
+      geom_point.point.y = s.first_point.y;
+      tf2::doTransform(geom_point, geom_point, geom_transform);
+      s.first_point.x = geom_point.point.x;
+      s.first_point.y = geom_point.point.y;
+
+      geom_point.point.x = s.last_point.x;
+      geom_point.point.y = s.last_point.y;
+      tf2::doTransform(geom_point, geom_point, geom_transform);
+      s.last_point.x = geom_point.point.x;
+      s.last_point.y = geom_point.point.y;
     }
 
-    for (Circle& c : circles_)
-      c.center = transformPoint(c.center, origin.x(), origin.y(), theta);
+    for (Circle& c : circles_) {
+      geom_point.point.x = c.center.x;
+      geom_point.point.y = c.center.y;
+      tf2::doTransform(geom_point, geom_point, geom_transform);
+      c.center.x = geom_point.point.x;
+      c.center.y = geom_point.point.y;
+    }
 
     obstacles_msg->header.frame_id = p_frame_id_;
   }