From dc769d328af99e1b6f3d629451aaf4f6cc1a3fae Mon Sep 17 00:00:00 2001 From: milesial Date: Sat, 28 Apr 2018 20:55:00 +0200 Subject: [PATCH 1/2] Extractor: obstacles are now correctly transformed --- include/obstacle_detector/utilities/math_utilities.h | 7 +++++++ src/obstacle_extractor.cpp | 10 ++++------ 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/include/obstacle_detector/utilities/math_utilities.h b/include/obstacle_detector/utilities/math_utilities.h index ce61970..54d4819 100644 --- 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 index a36436e..2a86df3 100644 --- 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_; } From 5ff6266c81c60230a59d5a49dc45d08c8c6f68f4 Mon Sep 17 00:00:00 2001 From: milesial Date: Sat, 28 Apr 2018 20:57:23 +0200 Subject: [PATCH 2/2] Removed unused variable --- src/obstacle_extractor.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/obstacle_extractor.cpp b/src/obstacle_extractor.cpp index 2a86df3..15937a8 100644 --- a/src/obstacle_extractor.cpp +++ b/src/obstacle_extractor.cpp @@ -418,7 +418,6 @@ void ObstacleExtractor::publishObstacles() { return; } - tf::Vector3 vec; for (Segment& s : segments_) { s.first_point = transformPoint(s.first_point, transform); s.last_point = transformPoint(s.last_point, transform);