Extractor: obstacles are now correctly transformed

tysik · Apr 28, 2018 · dc769d3 · dc769d3
1 parent 7c2cec9
commit dc769d3
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 6 deletions.
diff --git a/include/obstacle_detector/utilities/math_utilities.h b/include/obstacle_detector/utilities/math_utilities.h
@@ -91,6 +91,13 @@ inline Point transformPoint(const Point point, double x, double y, double theta)
   return p;
 }
 
+inline Point transformPoint(const Point& point, const tf::StampedTransform& transform) {
+  tf::Vector3 v(point.x, point.y, 0);
+  v = transform * v;
+
+  return {v.x(), v.y()};
+}
+
 inline bool checkPointInLimits(const geometry_msgs::Point32& p, double x_min, double x_max, double y_min, double y_max) {
   if ((p.x > x_max) || (p.x < x_min) || (p.y > y_max) || (p.y < y_min))
     return false;

diff --git a/src/obstacle_extractor.cpp b/src/obstacle_extractor.cpp
@@ -418,16 +418,14 @@ void ObstacleExtractor::publishObstacles() {
       return;
     }
 
-    tf::Vector3 origin = transform.getOrigin();
-    double theta = tf::getYaw(transform.getRotation());
-
+    tf::Vector3 vec;
     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);
+      s.first_point = transformPoint(s.first_point, transform);
+      s.last_point = transformPoint(s.last_point, transform);
     }
 
     for (Circle& c : circles_)
-      c.center = transformPoint(c.center, origin.x(), origin.y(), theta);
+      c.center = transformPoint(c.center, transform);
 
     obstacles_msg->header.frame_id = p_frame_id_;
   }