Return path in response

[hubacji1/iamcar.git] / base / ros.cc

diff --git a/base/ros.cc b/base/ros.cc
index ac247648c3637e052d3280e45740a7b310137fb5..297e848156e37e9db227e516f59955633e06ca29 100644 (file)
--- a/base/ros.cc
+++ b/base/ros.cc
@@ -20,6 +20,7 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include "ros/ros.h"
 #include "iamcar/Park.h"
 #include "obstacle_msgs/Obstacles.h"
+#include "geometry_msgs/Pose.h"
 
 #include "compile.h"
 #include "obstacle.h"
@@ -138,8 +139,19 @@ bool park(
                                 p.tlog(p.findt());
                 }
         }
-        // TODO res.path
-        return true;
+        for (auto n: p.tlog().back()) {
+                geometry_msgs::Pose p;
+                p.position.x = n->x();
+                p.position.y = n->y();
+                p.position.z = 0;
+                struct quaternion q = htoq(n->h());
+                p.orientation.x = q.x;
+                p.orientation.y = q.y;
+                p.orientation.z = q.z;
+                p.orientation.w = q.w;
+                res.path.push_back(p);
+        }
+        return p.goal_found();
 }
 
 int main(int argc, char **argv)