From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Tue, 31 May 2022 16:31:15 +0000 (+0200)
Subject: Implement rrts planner as service
X-Git-Tag: v0.1.0~2
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/wrrr.git/commitdiff_plain/9f6fb0098ec64e10736b1e64940794cc2445d032

Implement rrts planner as service
---

diff --git a/src/rrts_wr.cc b/src/rrts_wr.cc
index b13fb9d..ceebe33 100644
--- 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