Add occupancy grid subscriber

diff --git a/src/rrts_wr.cc b/src/rrts_wr.cc
index a3ba753e94f64d5f8c3d007018c7a681cb85220f..9b64ab6f0142ffce6acfb2f0ab199354017ae315 100644 (file)
--- 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();