From 6375bd914d81f446b1d333a60235122029ab4b1c Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Tue, 16 Aug 2022 11:37:43 +0200
Subject: [PATCH] Add occupancy grid subscriber

---
 src/rrts_wr.cc | 25 +++++++++++++++++++++++--
 1 file changed, 23 insertions(+), 2 deletions(-)

diff --git a/src/rrts_wr.cc b/src/rrts_wr.cc
index a3ba753..9b64ab6 100644
--- a/src/rrts_wr.cc
+++ b/src/rrts_wr.cc
@@ -1,13 +1,16 @@
 #include "nav_msgs/srv/get_plan.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "rrtsp.hh"
 
+rrts::P40 p;
+auto pts = std::make_shared<std::vector<rrts::Point>>();
+
 void
-plan_path(const std::shared_ptr<nav_msgs::srv::GetPlan::Request> req,
+plan_path(std::shared_ptr<nav_msgs::srv::GetPlan::Request> const req,
 		std::shared_ptr<nav_msgs::srv::GetPlan::Response> res)
 {
-	rrts::P40 p;
 	double sx = req->start.pose.position.x;
 	double sy = req->start.pose.position.y;
 	tf2::Quaternion qh;
@@ -25,6 +28,7 @@ plan_path(const std::shared_ptr<nav_msgs::srv::GetPlan::Request> req,
 	// TODO make 1000 settable over ROS (max iters before reset)
 	p.set_imax_reset(1000);
 	p.reset();
+	p.set_points_to_check(pts.get());
 	unsigned icnt = 0;
 	unsigned ncnt = 0;
 	double best_cost = 0.0;
@@ -66,6 +70,21 @@ plan_path(const std::shared_ptr<nav_msgs::srv::GetPlan::Request> req,
 	}
 }
 
+void
+update_points_to_check(nav_msgs::msg::OccupancyGrid const m)
+{
+	auto points_from_map = std::make_shared<std::vector<rrts::Point>>();
+	for (unsigned int row = 0; row < m.info.height; row++) {
+		for (unsigned int col = 0; col < m.info.width; col++) {
+			if (m.data[row * m.info.width + col] > 65) {
+				points_from_map.get()->push_back(
+						rrts::Point(col, row));
+			}
+		}
+	}
+	pts = points_from_map;
+}
+
 int
 main(int argc, char **argv)
 {
@@ -73,6 +92,8 @@ main(int argc, char **argv)
 	auto node = rclcpp::Node::make_shared("rrts");
 	auto srv = node->create_service<nav_msgs::srv::GetPlan>("rrts/planner",
 			&plan_path);
+	auto sub = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
+			"map", 10, &update_points_to_check);
 	RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to plan paths.");
 	rclcpp::spin(node);
 	rclcpp::shutdown();
-- 
2.39.2