Search map allocation and splitting (#109)

* New BeliefMapData msg for communicating updates to the search maps

* Add ability for belief maps updates.

* Search map splitting (#108)

* base code for splitting search bounds into individual regions for agents to search

* Small changes and organization. Fixed some build warnings.

* Add CGAL to dev dockerfile

* Change SearchPrior.msg to have vectors of value and priority to reduce size of overal search mission request

* Add priority to grid_map

* Set the search priors for each task assignment from the allocations of the overall search space

* Move some member variables out of MissionManager.cpp and fixed build warnings

* Remove unneccesary member variables in MissionManager. Restructure so search mission allocation resets each call.

* Add params for mission manager

* Bump docker version

---------

Co-authored-by: Nayana Suvarna <[email protected]>

* Increment docker image version

---------

Co-authored-by: Nayana Suvarna <[email protected]>

Author: bradygm

docker/Dockerfile.airstack-dev

@@ -27,7 +27,8 @@ RUN apt update -y && apt install -y \
     ros-humble-stereo-image-proc \
     ros-humble-image-view \
     ros-humble-topic-tools \
-    ros-humble-grid-map
+    ros-humble-grid-map \
+    libcgal-dev 
 
 RUN /opt/ros/humble/lib/mavros/install_geographiclib_datasets.sh
 

docker/docker-compose.yaml

@@ -59,7 +59,7 @@ services:
 
   # ==============================================================================
   robot:
-    image: &airstack-dev-image airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:v0.5.1
+    image: &airstack-dev-image airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:v0.5.2
     build:
       context: ../
       dockerfile: docker/Dockerfile.airstack-dev

docker/ground_control_station/.bashrc

@@ -157,3 +157,5 @@ function cws(){
 
 source /opt/ros/humble/setup.bash
 sws # source the ROS2 workspace by default
+
+export RCUTILS_COLORIZED_OUTPUT=1

ros_ws/src/airstack_msgs/msg/SearchPrior.msg

@@ -6,8 +6,8 @@ uint8 POINT_PRIOR = 3
 uint8 grid_prior_type
 
 geometry_msgs/Polygon points_list # polygon, line seg, or list of point
-float32 value # initial value for the polygon, line seg, or point
+float32[] value # initial value for the polygon, line seg, or point
 
 # Optional values
-float64 priority # Higher values mean higher priority. If empty then the priority is set to 1.0
-uint8 sensor_model_id # the type of sensor model for that region type. Default is 0
+float32[] priority # Higher values mean higher priority. If empty then the priority is set to 1.0
+uint8[] sensor_model_id # the type of sensor model for that region type. Default is 0

ros_ws/src/ground_control_station/mission_manager/CMakeLists.txt

@@ -5,13 +5,15 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+set(CGAL_DATA_DIR ".")
+
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(airstack_msgs REQUIRED)
 find_package(grid_map_ros REQUIRED)
-
+find_package(CGAL REQUIRED)
 
 add_executable(mission_manager_node
   src/mission_manager_node.cpp
@@ -26,12 +28,17 @@ ament_target_dependencies(example_search_request_node rclcpp airstack_msgs)
 
 target_include_directories(mission_manager_node PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
+  $<INSTALL_INTERFACE:include>
+  ${CGAL_INCLUDE_DIRS})
 
 target_include_directories(example_search_request_node PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
 
+target_link_libraries(mission_manager_node
+  ${CGAL_LIBRARIES}  # Link CGAL libraries
+)
+
 target_compile_features(mission_manager_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
 target_compile_features(example_search_request_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
 

ros_ws/src/ground_control_station/mission_manager/config/mission_manager.rviz

@@ -74,6 +74,18 @@ Visualization Manager:
         Value: /search_map_basestation
       Use Rainbow: true
       Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: Search Space Allocation Debugger
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /task_allocation_viz
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -135,10 +147,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.5047963857650757
+      Pitch: 1.5697963237762451
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 4.693580150604248
+      Yaw: 4.71858024597168
     Saved: ~
 Window Geometry:
   Displays:

ros_ws/src/ground_control_station/mission_manager/config/mission_manager.yaml

@@ -0,0 +1,4 @@
+mission_manager_node:
+  ros__parameters:
+    grid_cell_size: 5.0
+    visualize_search_allocation: true

ros_ws/src/ground_control_station/mission_manager/include/mission_manager/BeliefMap.h

@@ -30,7 +30,7 @@ class BeliefMap
   // std::vector<std::vector<int>> sensor_model_id;
 
   BeliefMap();
-  bool reset_map(rclcpp::Logger logger, airstack_msgs::msg::SearchMissionRequest search_mission_request);
+  bool reset_map(rclcpp::Logger logger, airstack_msgs::msg::SearchMissionRequest search_mission_request, double grid_cell_size);
   bool update_map(rclcpp::Logger logger, const airstack_msgs::msg::BeliefMapData::SharedPtr new_belief_data);
   grid_map::GridMap map_;
 
@@ -39,7 +39,31 @@ class BeliefMap
     return !map_.getSize()(0) == 0;
   }
 
+  double get_min_x() const
+  {
+    return min_x;
+  }
+  double get_max_x() const
+  {
+    return max_x;
+  }
+  double get_min_y() const
+  {
+    return min_y;
+  }
+  double get_max_y() const
+  {
+    return max_y;
+  }
+
 private:
+  //search map allocation
+  double min_x = DBL_MAX;
+  double max_x = -DBL_MAX;
+  double min_y = DBL_MAX;
+  double max_y = -DBL_MAX;
+  double grid_cell_size_;
+  
 
 };
 

ros_ws/src/ground_control_station/mission_manager/include/mission_manager/MissionManager.h

@@ -7,19 +7,71 @@
 #include <memory>
 #include <cstdint>
 #include <string>
+#include <limits>
+#include <random>
+#include <queue>
+#include <boost/functional/hash.hpp>
+
+#include <CGAL/Polygon_2.h>
+#include <CGAL/Alpha_shape_2.h>
+#include <CGAL/Simple_cartesian.h>
+#include <CGAL/Polygon_2_algorithms.h>
+#include <CGAL/Delaunay_triangulation_2.h>
+#include <CGAL/Triangulation_data_structure_2.h>
+#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
 
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
+#include <geometry_msgs/msg/point.hpp>
+#include "geometry_msgs/msg/point32.hpp"
+#include <visualization_msgs/msg/marker_array.hpp>
 #include "airstack_msgs/msg/search_mission_request.hpp"
 #include "airstack_msgs/msg/task_assignment.hpp"
 #include "mission_manager/BeliefMap.h"
 
+typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
+typedef K::Point_2 Point;
+typedef CGAL::Alpha_shape_vertex_base_2<K> Vb;
+typedef CGAL::Alpha_shape_face_base_2<K> Fb;
+typedef CGAL::Triangulation_data_structure_2<Vb,Fb> Tds;
+typedef CGAL::Delaunay_triangulation_2<K, Tds> Triangulation_2;
+typedef CGAL::Alpha_shape_2<Triangulation_2> Alpha_shape_2;
+typedef Alpha_shape_2::Edge Edge;
+typedef CGAL::Segment_2<K> Segment;
+
+struct ClusterPoint
+{
+    double x, y;
+    int cluster;
+    double cluster_dist;
+
+    ClusterPoint(double _x, double _y, int _cluster_idx, double _cluster_dist)
+    {
+      x = _x;
+      y = _y;
+      cluster = _cluster_idx;
+      cluster_dist = _cluster_dist;
+    }
+};
+
+  struct CompareClusterPoint 
+{
+  bool operator()(const ClusterPoint& a, const ClusterPoint& b) const 
+  {
+    return a.cluster_dist > b.cluster_dist; // Min-heap based on cluster_dist
+  }
+};
+
+
 class MissionManager
 {
   public:
     MissionManager(int max_number_agents);
 
-    std::vector<airstack_msgs::msg::TaskAssignment> assign_tasks(rclcpp::Logger logger) const;
+    std::vector<airstack_msgs::msg::TaskAssignment> assign_tasks(rclcpp::Logger logger,
+                                                                const airstack_msgs::msg::SearchMissionRequest &plan_request,
+                                                                rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub,
+                                                                bool visualize_search_allocation);
     bool check_agent_changes(rclcpp::Logger logger, uint8_t robot_id, rclcpp::Time current_time);
     bool check_target_changes(rclcpp::Logger logger, std::string target_list, rclcpp::Time current_time);
     std::vector<bool> get_valid_agents() const { return valid_agents_; }
@@ -30,5 +82,15 @@ class MissionManager
     std::vector<rclcpp::Time> time_of_last_call_;
     std::vector<bool> valid_agents_;
 
+    //search map allocation
+    std::vector<std::vector<std::vector<double>>> allocate_search_map(rclcpp::Logger logger,
+                                                                      int number_of_search_tasks,
+                                                                      const airstack_msgs::msg::SearchMissionRequest &plan_request,
+                                                                      std::vector<std::vector<double>> &cluster_centroids,
+                                                                      std::vector<std::vector<ClusterPoint>> &clusters);
+    std::vector<std::vector<double>> calculate_cluster_centroids(rclcpp::Logger logger, int num_agents, const airstack_msgs::msg::SearchMissionRequest &plan_request, std::vector<Point> &CGAL_bounds) const;
+    std::vector<std::vector<ClusterPoint>> calculate_clusters(rclcpp::Logger logger, int num_agents, std::vector<std::vector<double>> cluster_centroids, const std::vector<Point> &CGAL_bounds) const;
+    std::vector<std::vector<std::vector<double>>> calculate_cluster_bounds(rclcpp::Logger logger, std::vector<std::vector<ClusterPoint>> clusters) const;
+
   };
   #endif /* MISSIONMANAGER_H_INCLUDED */

ros_ws/src/ground_control_station/mission_manager/include/mission_manager/mission_manager_node.h

@@ -10,6 +10,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
 #include "airstack_msgs/msg/search_mission_request.hpp"
 #include "airstack_msgs/msg/task_assignment.hpp"
 #include "airstack_msgs/msg/belief_map_data.hpp"
@@ -41,6 +42,12 @@ class MissionManagerNode : public rclcpp::Node
     MissionManagerNode()
     : Node("mission_manager"), count_(0)
     {
+      this->declare_parameter("grid_cell_size", 10.0);
+      this->declare_parameter("visualize_search_allocation", false);
+      this->get_parameter("grid_cell_size", grid_cell_size_);
+      this->get_parameter("visualize_search_allocation", visualize_search_allocation_);
+      
+      
       mission_subscriber_ = this->create_subscription<airstack_msgs::msg::SearchMissionRequest>(
         "search_mission_request", 1, std::bind(&MissionManagerNode::search_mission_request_callback, this, std::placeholders::_1));
 
@@ -75,6 +82,9 @@ class MissionManagerNode : public rclcpp::Node
       // TODO: set param for rate, make sure not communicated over network
       search_map_publisher_ = this->create_publisher<grid_map_msgs::msg::GridMap>(
         "search_map_basestation", rclcpp::QoS(1).transient_local());
+
+      viz_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("task_allocation_viz", 10.0);
+
       timer_ = this->create_wall_timer(
         std::chrono::seconds(1), std::bind(&MissionManagerNode::search_map_publisher, this));
     }
@@ -84,6 +94,7 @@ class MissionManagerNode : public rclcpp::Node
 
     // Publisher
     std::vector<rclcpp::Publisher<airstack_msgs::msg::TaskAssignment>::SharedPtr> plan_request_pubs_;
+    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr viz_pub_;
     rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr search_map_publisher_;
 
     // Subscribers
@@ -97,6 +108,8 @@ class MissionManagerNode : public rclcpp::Node
 
     size_t count_;
     rclcpp::TimerBase::SharedPtr timer_;
+    double grid_cell_size_;
+    bool visualize_search_allocation_;
     int max_number_agents_ = 5; // TODO: get from param server
     airstack_msgs::msg::SearchMissionRequest latest_search_mission_request_;
     std::shared_ptr<MissionManager> mission_manager_;
@@ -133,16 +146,16 @@ class MissionManagerNode : public rclcpp::Node
       latest_search_mission_request_ = *msg;
 
       // TODO: visualize the seach mission request
-      this->mission_manager_->belief_map_.reset_map(this->get_logger(), *msg);
-      this->publish_tasks(this->mission_manager_->assign_tasks(this->get_logger()));
+      this->mission_manager_->belief_map_.reset_map(this->get_logger(), *msg, grid_cell_size_);
+      this->publish_tasks(this->mission_manager_->assign_tasks(this->get_logger(), latest_search_mission_request_, viz_pub_, visualize_search_allocation_));
     }
 
     void agent_odom_callback(const std_msgs::msg::String::SharedPtr msg, const uint8_t &robot_id)
     {
       RCLCPP_INFO(this->get_logger(), "Received agent odom '%s'", msg->data.c_str());
       if (this->mission_manager_->check_agent_changes(this->get_logger(), robot_id, this->now()))
       {
-        this->publish_tasks(this->mission_manager_->assign_tasks(this->get_logger()));
+        this->publish_tasks(this->mission_manager_->assign_tasks(this->get_logger(), latest_search_mission_request_, viz_pub_, visualize_search_allocation_));
       }
     }
 
@@ -154,7 +167,7 @@ class MissionManagerNode : public rclcpp::Node
       // Check if change in the number of targets or id numbers
       if (this->mission_manager_->check_target_changes(this->get_logger(), msg->data, this->now()))
       {
-        this->publish_tasks(this->mission_manager_->assign_tasks(this->get_logger()));
+        this->publish_tasks(this->mission_manager_->assign_tasks(this->get_logger(), latest_search_mission_request_, viz_pub_, visualize_search_allocation_));
       }
     }