Implement rrts planner as service

diff --git a/src/rrts_wr.cc b/src/rrts_wr.cc
index b13fb9dc2d47681aff6a4beae0aa1d02c9d1b9fa..ceebe337ee4c10502cc40e9497685c0287fe3c04 100644 (file)
--- a/src/rrts_wr.cc
+++ b/src/rrts_wr.cc
@@ -1,12 +1,65 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/srv/get_plan.hpp"
+#include "rrtsp.hh"
 
 void
 plan_path(const std::shared_ptr<nav_msgs::srv::GetPlan::Request> req,
                std::shared_ptr<nav_msgs::srv::GetPlan::Response> res)
 {
+       rrts::P39 p;
+       double sx = req->start.pose.position.x;
+       double sy = req->start.pose.position.y;
+       double sh = 0.0;  // FIXME
+       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
+       p.set_goal(gx, gy, gb, ge);
+       // TODO make 1000 settable over ROS (max iters before reset)
+       p.set_imax_reset(1000);
+       p.reset();
+       unsigned icnt = 0;
+       unsigned ncnt = 0;
+       double best_cost = 0.0;
+       auto best_path = p.get_path();
+       // TODO make 1000 settable over ROS (max iters total)
+       while (icnt < 1000) {
+               p.icnt(icnt);
+               while (p.next()) {}
+               icnt = p.icnt();
+               if (best_cost == 0.0 || p.get_path_cost() < best_cost) {
+                       // TODO make 0.75 settable over ROS
+                       if (p.get_path_cost() < 0.75 * best_cost) {
+                               ncnt = 0;
+                       }
+                       best_path = p.get_path();
+                       best_cost = p.get_path_cost();
+               } else {
+                       ncnt++;
+               }
+               // TODO make 5 settable over ROS
+               if (ncnt > 5) {
+                       break;
+               }
+               p.reset();
+       }
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"),
-                       "received req, needs to process it.");
+                       "path planned in %lf s, path cost %lf m",
+                       p.scnt(), best_cost);
+       res->plan.poses.clear();
+       for (auto pp: best_path) {
+               geometry_msgs::msg::PoseStamped p;
+               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;
+               res->plan.poses.push_back(p);
+       }
 }
 
 int