Use tf2 to consider start/goal headings

[hubacji1/wrrr.git] / src / rrts_wr.cc

diff --git a/src/rrts_wr.cc b/src/rrts_wr.cc
index ceebe337ee4c10502cc40e9497685c0287fe3c04..58cfd823fd236978643a31c288a3cf45354c0266 100644 (file)
--- a/src/rrts_wr.cc
+++ b/src/rrts_wr.cc
@@ -1,5 +1,6 @@
-#include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/srv/get_plan.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "rrtsp.hh"
 
 void
@@ -9,12 +10,17 @@ plan_path(const std::shared_ptr<nav_msgs::srv::GetPlan::Request> req,
        rrts::P39 p;
        double sx = req->start.pose.position.x;
        double sy = req->start.pose.position.y;
-       double sh = 0.0;  // FIXME
+       tf2::Quaternion qh;
+       tf2::fromMsg(req->start.pose.orientation, qh);
+       qh.normalize();
+       double sh = qh.getAngle();
        p.set_start(sx, sy, sh);
        double gx = req->goal.pose.position.x;
        double gy = req->goal.pose.position.y;
-       double gb = 0.0;  // FIXME
-       double ge = 0.0;  // FIXME
+       tf2::fromMsg(req->goal.pose.orientation, qh);
+       qh.normalize();
+       double gb = qh.getAngle();
+       double ge = gb + std::abs(req->tolerance);
        p.set_goal(gx, gy, gb, ge);
        // TODO make 1000 settable over ROS (max iters before reset)
        p.set_imax_reset(1000);
@@ -53,11 +59,9 @@ plan_path(const std::shared_ptr<nav_msgs::srv::GetPlan::Request> req,
                p.pose.position.x = pp.x();
                p.pose.position.y = pp.y();
                p.pose.position.z = 0.0;
-               // TODO use pp.h() to compute orientation
-               p.pose.orientation.x = 0.0;
-               p.pose.orientation.y = 0.0;
-               p.pose.orientation.z = 0.0;
-               p.pose.orientation.w = 0.0;
+               tf2::Quaternion qh;
+               qh.setRPY(0, 0, pp.h());
+               p.pose.orientation = tf2::toMsg(qh);
                res->plan.poses.push_back(p);
        }
 }