Lidar can now be in any orientation, including updside down

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})

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"

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>

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_;
   }