From 2976f67c28a8d2fff2fca17644612047c2fc4686 Mon Sep 17 00:00:00 2001 From: Brady Moon Date: Tue, 22 Oct 2024 16:24:59 -0400 Subject: [PATCH] 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 --- docker/Dockerfile.airstack-dev | 3 +- docker/docker-compose.yaml | 4 +- docker/ground_control_station/.bashrc | 2 + ros_ws/src/airstack_msgs/msg/SearchPrior.msg | 6 +- .../mission_manager/CMakeLists.txt | 11 +- .../config/mission_manager.rviz | 16 +- .../config/mission_manager.yaml | 4 + .../include/mission_manager/BeliefMap.h | 26 +- .../include/mission_manager/MissionManager.h | 64 +++- .../mission_manager/mission_manager_node.h | 21 +- .../include/mission_manager/visualization.h | 101 ++++++ .../launch/mission_manager_launch.py | 7 + .../mission_manager/package.xml | 1 + .../mission_manager/src/BeliefMap.cpp | 41 ++- .../mission_manager/src/MissionManager.cpp | 332 +++++++++++++++++- .../src/example_search_request.cpp | 12 +- 16 files changed, 603 insertions(+), 48 deletions(-) create mode 100644 ros_ws/src/ground_control_station/mission_manager/config/mission_manager.yaml create mode 100644 ros_ws/src/ground_control_station/mission_manager/include/mission_manager/visualization.h diff --git a/docker/Dockerfile.airstack-dev b/docker/Dockerfile.airstack-dev index c3b2bb12..f38d921b 100644 --- a/docker/Dockerfile.airstack-dev +++ b/docker/Dockerfile.airstack-dev @@ -26,7 +26,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 diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml index 0e346891..e3fe78fb 100644 --- a/docker/docker-compose.yaml +++ b/docker/docker-compose.yaml @@ -100,8 +100,8 @@ services: dockerfile: docker/Dockerfile.airstack-dev tags: - airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:latest - - airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:v0.5.0 - image: airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:v0.5.0 + - airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:v0.5.1 + image: airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:v0.5.1 # container_name: robot-1 entrypoint: bash -c "service ssh restart && bash" # Interactive shell diff --git a/docker/ground_control_station/.bashrc b/docker/ground_control_station/.bashrc index adb1e9e1..cb45f13a 100644 --- a/docker/ground_control_station/.bashrc +++ b/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 \ No newline at end of file diff --git a/ros_ws/src/airstack_msgs/msg/SearchPrior.msg b/ros_ws/src/airstack_msgs/msg/SearchPrior.msg index 746dc982..feddc600 100644 --- a/ros_ws/src/airstack_msgs/msg/SearchPrior.msg +++ b/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 \ No newline at end of file +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 \ No newline at end of file diff --git a/ros_ws/src/ground_control_station/mission_manager/CMakeLists.txt b/ros_ws/src/ground_control_station/mission_manager/CMakeLists.txt index 91b5b78e..461c7ef1 100644 --- a/ros_ws/src/ground_control_station/mission_manager/CMakeLists.txt +++ b/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 $ - $) + $ + ${CGAL_INCLUDE_DIRS}) target_include_directories(example_search_request_node PUBLIC $ $) +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 diff --git a/ros_ws/src/ground_control_station/mission_manager/config/mission_manager.rviz b/ros_ws/src/ground_control_station/mission_manager/config/mission_manager.rviz index c9e240f6..d257ef6f 100644 --- a/ros_ws/src/ground_control_station/mission_manager/config/mission_manager.rviz +++ b/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: Value: Orbit (rviz) - Yaw: 4.693580150604248 + Yaw: 4.71858024597168 Saved: ~ Window Geometry: Displays: diff --git a/ros_ws/src/ground_control_station/mission_manager/config/mission_manager.yaml b/ros_ws/src/ground_control_station/mission_manager/config/mission_manager.yaml new file mode 100644 index 00000000..6fe12c30 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/ros_ws/src/ground_control_station/mission_manager/include/mission_manager/BeliefMap.h b/ros_ws/src/ground_control_station/mission_manager/include/mission_manager/BeliefMap.h index 83095c14..e550085e 100644 --- a/ros_ws/src/ground_control_station/mission_manager/include/mission_manager/BeliefMap.h +++ b/ros_ws/src/ground_control_station/mission_manager/include/mission_manager/BeliefMap.h @@ -30,7 +30,7 @@ class BeliefMap // std::vector> 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_; + }; diff --git a/ros_ws/src/ground_control_station/mission_manager/include/mission_manager/MissionManager.h b/ros_ws/src/ground_control_station/mission_manager/include/mission_manager/MissionManager.h index eac347b3..ee0dbc56 100644 --- a/ros_ws/src/ground_control_station/mission_manager/include/mission_manager/MissionManager.h +++ b/ros_ws/src/ground_control_station/mission_manager/include/mission_manager/MissionManager.h @@ -7,19 +7,71 @@ #include #include #include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" +#include +#include "geometry_msgs/msg/point32.hpp" +#include #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 Vb; +typedef CGAL::Alpha_shape_face_base_2 Fb; +typedef CGAL::Triangulation_data_structure_2 Tds; +typedef CGAL::Delaunay_triangulation_2 Triangulation_2; +typedef CGAL::Alpha_shape_2 Alpha_shape_2; +typedef Alpha_shape_2::Edge Edge; +typedef CGAL::Segment_2 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 assign_tasks(rclcpp::Logger logger) const; + std::vector assign_tasks(rclcpp::Logger logger, + const airstack_msgs::msg::SearchMissionRequest &plan_request, + rclcpp::Publisher::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 get_valid_agents() const { return valid_agents_; } @@ -30,5 +82,15 @@ class MissionManager std::vector time_of_last_call_; std::vector valid_agents_; + //search map allocation + std::vector>> allocate_search_map(rclcpp::Logger logger, + int number_of_search_tasks, + const airstack_msgs::msg::SearchMissionRequest &plan_request, + std::vector> &cluster_centroids, + std::vector> &clusters); + std::vector> calculate_cluster_centroids(rclcpp::Logger logger, int num_agents, const airstack_msgs::msg::SearchMissionRequest &plan_request, std::vector &CGAL_bounds) const; + std::vector> calculate_clusters(rclcpp::Logger logger, int num_agents, std::vector> cluster_centroids, const std::vector &CGAL_bounds) const; + std::vector>> calculate_cluster_bounds(rclcpp::Logger logger, std::vector> clusters) const; + }; #endif /* MISSIONMANAGER_H_INCLUDED */ \ No newline at end of file diff --git a/ros_ws/src/ground_control_station/mission_manager/include/mission_manager/mission_manager_node.h b/ros_ws/src/ground_control_station/mission_manager/include/mission_manager/mission_manager_node.h index 58113713..8a10412d 100644 --- a/ros_ws/src/ground_control_station/mission_manager/include/mission_manager/mission_manager_node.h +++ b/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( "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( "search_map_basestation", rclcpp::QoS(1).transient_local()); + + viz_pub_ = this->create_publisher("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::SharedPtr> plan_request_pubs_; + rclcpp::Publisher::SharedPtr viz_pub_; rclcpp::Publisher::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 mission_manager_; @@ -133,8 +146,8 @@ 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) @@ -142,7 +155,7 @@ class MissionManagerNode : public rclcpp::Node 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_)); } } diff --git a/ros_ws/src/ground_control_station/mission_manager/include/mission_manager/visualization.h b/ros_ws/src/ground_control_station/mission_manager/include/mission_manager/visualization.h new file mode 100644 index 00000000..0e09127c --- /dev/null +++ b/ros_ws/src/ground_control_station/mission_manager/include/mission_manager/visualization.h @@ -0,0 +1,101 @@ +#ifndef VISUALIZATIONS_MM_H_INCLUDED +#define VISUALIZATIONS_MM_H_INCLUDED + +#include +#include + +#include "mission_manager/MissionManager.h" + + +visualization_msgs::msg::MarkerArray visualize_multi_agent_search_request(int num_agents, + std::vector> cluster_centroids, + std::vector> clusters, + std::vector>> convex_hulls) +{ + visualization_msgs::msg::MarkerArray ma; + + visualization_msgs::msg::Marker m; + m.header.frame_id = "map"; + m.id = 0; + m.type = visualization_msgs::msg::Marker::CUBE_LIST; + m.action = visualization_msgs::msg::Marker::ADD; + m.pose.orientation.w = 1.0; + m.scale.x = 10.0; + m.scale.y = 10.0; + m.scale.z = 10.0; + m.color.a = 1.0; + m.color.r = 1.0; + m.color.g = 1.0; + m.color.b = 1.0; + + for(std::vector centroid : cluster_centroids) + { + geometry_msgs::msg::Point new_point; + new_point.x = centroid[0]; + new_point.y = centroid[1]; + new_point.z = 5.0; + m.points.push_back(new_point); + } + ma.markers.push_back(m); + + //publish visualization for different regions + std::vector> agent_colors = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}; + for(int agent_idx = 0; agent_idx < num_agents; ++agent_idx) + { + visualization_msgs::msg::Marker cluster_m; + cluster_m.header.frame_id = "map"; + cluster_m.id = agent_idx + 1; + cluster_m.type = visualization_msgs::msg::Marker::CUBE_LIST; + cluster_m.action = visualization_msgs::msg::Marker::ADD; + cluster_m.pose.orientation.w = 1.0; + cluster_m.scale.x = 1.0; + cluster_m.scale.y = 1.0; + cluster_m.scale.z = 1.0; + cluster_m.color.a = 0.5; + std::vector agent_color = agent_colors[agent_idx]; + cluster_m.color.r = agent_color[0]; + cluster_m.color.g = agent_color[1]; + cluster_m.color.b = agent_color[2]; + + for(ClusterPoint cp : clusters[agent_idx]) + { + geometry_msgs::msg::Point new_point; + new_point.x = cp.x; + new_point.y = cp.y; + new_point.z = 1.0; + cluster_m.points.push_back(new_point); + } + ma.markers.push_back(cluster_m); + } + + //publish visualization for the convex hull of each region + for(auto agent_idx = 0u; agent_idx < convex_hulls.size(); ++agent_idx) + { + visualization_msgs::msg::Marker convex_hull_m; + convex_hull_m.header.frame_id = "map"; + convex_hull_m.id = agent_idx + 100; + convex_hull_m.type = visualization_msgs::msg::Marker::LINE_STRIP; + convex_hull_m.action = visualization_msgs::msg::Marker::ADD; + convex_hull_m.pose.orientation.w = 1.0; + convex_hull_m.scale.x = 2.0; + convex_hull_m.color.a = 1.0; + std::vector agent_color = agent_colors[agent_idx]; + convex_hull_m.color.r = agent_color[0]; + convex_hull_m.color.g = agent_color[1]; + convex_hull_m.color.b = agent_color[2]; + + auto convex_hull = convex_hulls[agent_idx]; + for(auto point : convex_hull) + { + // RCLCPP_INFO_STREAM(logger, point[0] << " " << point[1]); + geometry_msgs::msg::Point new_point; + new_point.x = point[0]; + new_point.y = point[1]; + new_point.z = 1.0; + convex_hull_m.points.push_back(new_point); + } + ma.markers.push_back(convex_hull_m); + } + return ma; +} +#endif /* VISUALIZATIONS_MM_H_INCLUDED */ \ No newline at end of file diff --git a/ros_ws/src/ground_control_station/mission_manager/launch/mission_manager_launch.py b/ros_ws/src/ground_control_station/mission_manager/launch/mission_manager_launch.py index d077ddba..15cadc46 100644 --- a/ros_ws/src/ground_control_station/mission_manager/launch/mission_manager_launch.py +++ b/ros_ws/src/ground_control_station/mission_manager/launch/mission_manager_launch.py @@ -12,6 +12,12 @@ def generate_launch_description(): # Define the path to the configuration file visualization_config_file = os.path.join(package_share_directory, 'config', 'grid_map.yaml') rviz_config_file = os.path.join(package_share_directory, 'config', 'mission_manager.rviz') + + config = os.path.join( + package_share_directory, + 'config', + 'mission_manager.yaml' + ) mission_manager = Node( package='mission_manager', @@ -19,6 +25,7 @@ def generate_launch_description(): executable='mission_manager_node', output="screen", name='mission_manager_node', + parameters=[config] ) # grid_map_visualization_node = Node( diff --git a/ros_ws/src/ground_control_station/mission_manager/package.xml b/ros_ws/src/ground_control_station/mission_manager/package.xml index ca599679..9f7a7373 100644 --- a/ros_ws/src/ground_control_station/mission_manager/package.xml +++ b/ros_ws/src/ground_control_station/mission_manager/package.xml @@ -15,6 +15,7 @@ std_msgs airstack_msgs grid_map + visualization_msgs ament_lint_auto ament_lint_common diff --git a/ros_ws/src/ground_control_station/mission_manager/src/BeliefMap.cpp b/ros_ws/src/ground_control_station/mission_manager/src/BeliefMap.cpp index f9266452..9ee8bbd1 100644 --- a/ros_ws/src/ground_control_station/mission_manager/src/BeliefMap.cpp +++ b/ros_ws/src/ground_control_station/mission_manager/src/BeliefMap.cpp @@ -2,19 +2,19 @@ BeliefMap::BeliefMap() -: map_(std::vector({"probability"})) +: map_(std::vector({"probability", "priority"})) { } -bool BeliefMap::reset_map(rclcpp::Logger logger, airstack_msgs::msg::SearchMissionRequest search_mission_request) +bool BeliefMap::reset_map(rclcpp::Logger logger, airstack_msgs::msg::SearchMissionRequest search_mission_request, double grid_cell_size) { RCLCPP_INFO(logger, "Resetting map"); // Setting up map. // Get the max and min x and y values from the search area - double resolution = 5.0; - double min_x = DBL_MAX; - double max_x = -DBL_MAX; - double min_y = DBL_MAX; - double max_y = -DBL_MAX; + grid_cell_size_ = grid_cell_size; + min_x = DBL_MAX; + max_x = -DBL_MAX; + min_y = DBL_MAX; + max_y = -DBL_MAX; for (const auto& point : search_mission_request.search_bounds.points) { if (point.x < min_x) @@ -37,15 +37,15 @@ bool BeliefMap::reset_map(rclcpp::Logger logger, airstack_msgs::msg::SearchMissi // TODO param for grid resolution // TODO fix coordinate frames. Match airstack to IPP to grid map. grid map is NWU, IPP is ENU, airstack is ... // round to nearest resolution - min_x = std::floor(min_x / resolution) * resolution; - max_x = std::ceil(max_x / resolution) * resolution; - min_y = std::floor(min_y / resolution) * resolution; - max_y = std::ceil(max_y / resolution) * resolution; + min_x = std::floor(min_x / grid_cell_size_) * grid_cell_size_; + max_x = std::ceil(max_x / grid_cell_size_) * grid_cell_size_; + min_y = std::floor(min_y / grid_cell_size_) * grid_cell_size_; + max_y = std::ceil(max_y / grid_cell_size_) * grid_cell_size_; double x_length = std::abs(max_x - min_x); double y_length = std::abs(max_y - min_y); map_.setGeometry(grid_map::Length(x_length, y_length), - resolution, + grid_cell_size_, grid_map::Position(std::round(min_x + x_length / 2.0), std::round(min_y + y_length / 2.0))); map_.setFrameId("map"); // TODO update to correct frame or set TF frame somewhere. There is already a map frame @@ -55,6 +55,11 @@ bool BeliefMap::reset_map(rclcpp::Logger logger, airstack_msgs::msg::SearchMissi { if (search_prior.grid_prior_type == airstack_msgs::msg::SearchPrior::POLYGON_PRIOR) { + if (search_prior.value.size() > 1 || search_prior.priority.size() > 1) + { + RCLCPP_ERROR(logger, "Polygon priors with multiple values or priorities not valid"); + continue; + } grid_map::Polygon polygon; polygon.setFrameId(map_.getFrameId()); for (const auto& point : search_prior.points_list.points) @@ -65,8 +70,10 @@ bool BeliefMap::reset_map(rclcpp::Logger logger, airstack_msgs::msg::SearchMissi !iterator.isPastEnd(); ++iterator) { float& current_value = map_.at("probability", *iterator); - if (std::isnan(current_value) || current_value < search_prior.value) { - current_value = search_prior.value; + float& current_priority = map_.at("priority", *iterator); + if (std::isnan(current_value) || (current_value * current_priority) < (search_prior.value[0]*search_prior.priority[0])) { + current_value = search_prior.value[0]; + current_priority = search_prior.priority[0]; } } } @@ -103,17 +110,17 @@ bool BeliefMap::update_map(rclcpp::Logger logger, const airstack_msgs::msg::Beli grid_map::Index submapBufferSize(new_belief_data->x_end - new_belief_data->x_start, new_belief_data->y_end - new_belief_data->y_start); // check coordinate frame! int cur_index_num = 0; - grid_map::Matrix& data = map_["probability"]; + grid_map::Matrix& probability_data = map_["probability"]; for (grid_map::SubmapIterator iterator(map_, submapStartIndex, submapBufferSize); !iterator.isPastEnd(); ++iterator) { // map_.at("probability", *iterator) = 1.0; const grid_map::Index index(*iterator); // always save with the lower value - if (new_belief_data->map_values[cur_index_num] < data(index(0), index(1))) + if (new_belief_data->map_values[cur_index_num] < probability_data(index(0), index(1))) { // Scale the value back from 0-UIN16_MAX to 0-1 - data(index(0), index(1)) = static_cast(new_belief_data->map_values[cur_index_num]) / static_cast(UINT16_MAX); // TODO ensure maps are encoded correctly to match the iterator + probability_data(index(0), index(1)) = static_cast(new_belief_data->map_values[cur_index_num]) / static_cast(UINT16_MAX); // TODO ensure maps are encoded correctly to match the iterator } cur_index_num++; } diff --git a/ros_ws/src/ground_control_station/mission_manager/src/MissionManager.cpp b/ros_ws/src/ground_control_station/mission_manager/src/MissionManager.cpp index 35997a91..e8d497ba 100644 --- a/ros_ws/src/ground_control_station/mission_manager/src/MissionManager.cpp +++ b/ros_ws/src/ground_control_station/mission_manager/src/MissionManager.cpp @@ -1,4 +1,5 @@ #include "mission_manager/MissionManager.h" +#include "mission_manager/visualization.h" /* Empty Constructor @@ -47,31 +48,344 @@ bool MissionManager::check_target_changes(rclcpp::Logger logger, std::string tar return false; } +std::vector> MissionManager::calculate_cluster_centroids(rclcpp::Logger logger, int num_agents, const airstack_msgs::msg::SearchMissionRequest &plan_request, std::vector &CGAL_bounds) const +{ + std::random_device dev; + std::mt19937 rng(dev()); + for(auto& point : plan_request.search_bounds.points) + { + // add to vector of bounds + CGAL_bounds.push_back(Point(point.x, point.y)); + } + double min_x = this->belief_map_.get_min_x(); + double max_x = this->belief_map_.get_max_x(); + double min_y = this->belief_map_.get_min_y(); + double max_y = this->belief_map_.get_max_y(); + + std::vector> boundary = {{min_x, min_y},{max_x, min_y},{max_x, max_y},{min_x, max_y}}; + + //divide bounds for the search area based on number of agents + double x_step_size = (max_x - min_x) / num_agents; + double y_step_size = (max_y - min_y) / num_agents; + + std::vector> rand_centroids; + if(num_agents == 3) + { + rand_centroids = {{20.0,60.0}, {142.0, 40.0}, {128.0, 119.0}}; + } + + //generate bounds for each subregion + while(static_cast(rand_centroids.size()) < num_agents) // Assumes rand centroids not crazy big + { + int i = rand_centroids.size(); + + //calculate bounds for random sampling + double min_x_sample = min_x + i * x_step_size; + double min_y_sample = min_y + i * y_step_size; + double max_x_sample = min_x + (i+1) * x_step_size; + double max_y_sample = min_y + (i+1) * y_step_size; + + //create uniform distributions to sample from + std::uniform_real_distribution rand_x_dist(min_x_sample, max_x_sample); + std::uniform_real_distribution rand_y_dist(min_y_sample, max_y_sample); + + //generate random samples + double rand_x = rand_x_dist(rng); + double rand_y = rand_y_dist(rng); + + //check if this random sample is within bounds + if (CGAL::bounded_side_2(CGAL_bounds.begin(), + CGAL_bounds.end(), + Point(rand_x, rand_y), K()) == CGAL::ON_BOUNDED_SIDE) + { + rand_centroids.push_back({rand_x, rand_y}); + RCLCPP_DEBUG_STREAM(logger, "Search mission centroid:" << rand_x << " " << rand_y); + } + } + return rand_centroids; +} + +double compute_dist(double x1, double y1, double x2, double y2) { + return std::sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)); +} + +std::vector> MissionManager::calculate_clusters(rclcpp::Logger logger, int num_agents, std::vector> cluster_centroids, const std::vector &CGAL_bounds) const +{ + double min_x = this->belief_map_.get_min_x(); + double max_x = this->belief_map_.get_max_x(); + double min_y = this->belief_map_.get_min_y(); + double max_y = this->belief_map_.get_max_y(); + //calculate points in bounds + std::priority_queue, CompareClusterPoint> cluster_pq; + for(int x_idx = min_x; x_idx < max_x; ++x_idx) + { + for(int y_idx = min_y; y_idx < max_y; ++y_idx) + { + if (CGAL::bounded_side_2(CGAL_bounds.begin(), + CGAL_bounds.end(), + Point(x_idx, y_idx), K()) == CGAL::ON_BOUNDED_SIDE) + { + //find closest and farthest centroids to this point + int closest_centroid_idx = -1; + int farthest_centroid_idx = -1; + double min_dist = std::numeric_limits::infinity(); + double max_dist = -std::numeric_limits::infinity(); + for(int centroid_idx = 0; centroid_idx < static_cast(cluster_centroids.size()); ++centroid_idx) // Assumes rand centroids not crazy big + { + double dist = compute_dist(cluster_centroids[centroid_idx][0], cluster_centroids[centroid_idx][1], x_idx, y_idx); + if(dist < min_dist) + { + min_dist = dist; + closest_centroid_idx = centroid_idx; + } + if(dist > max_dist) + { + max_dist = dist; + farthest_centroid_idx = centroid_idx; + } + } + if (closest_centroid_idx == -1 || farthest_centroid_idx == -1) + { + RCLCPP_ERROR_STREAM(logger, "No closest or farthest centroid found for point " << x_idx << " " << y_idx); + } + cluster_pq.push(ClusterPoint(x_idx, y_idx, closest_centroid_idx, min_dist - max_dist)); + } + } + } + //actual do the clustering now + std::vector cluster_full; + std::vector> clusters; + int cluster_size = cluster_pq.size() / num_agents; + for(int i = 0; i < num_agents; ++i) + { + clusters.push_back({}); + cluster_full.push_back(false); + } + while(true) + { + ClusterPoint point = cluster_pq.top(); + // RCLCPP_INFO_STREAM(logger, point.cluster_dist << " " << point.x << " " << point.y); + cluster_pq.pop(); + + //check the size of preferred cluster + if(static_cast(clusters[point.cluster].size()) < cluster_size) // Assumes clusters not crazy big + { + clusters[point.cluster].push_back(point); + } + else if(!cluster_full[point.cluster]) + { + //pop all remaining elements off priority queue and resort + std::vector remaining_points; + while(!cluster_pq.empty()) + { + ClusterPoint point = cluster_pq.top(); + remaining_points.push_back(point); + cluster_pq.pop(); + } + //iterate through vector and resort + for(ClusterPoint cp : remaining_points) + { + //find closest and farthest centroids to this point + int closest_centroid_idx = -1; + int farthest_centroid_idx = -1; + double min_dist = std::numeric_limits::infinity(); + double max_dist = -std::numeric_limits::infinity(); + for(int centroid_idx = 0; centroid_idx < static_cast(cluster_centroids.size()); ++centroid_idx) // Assumes not crazy number of centroids + { + if(centroid_idx != point.cluster and !cluster_full[centroid_idx]) //check that it's not the existing full centroid OR a previous full centroid + { + double dist = compute_dist(cluster_centroids[centroid_idx][0], cluster_centroids[centroid_idx][1], cp.x, cp.y); + if(dist < min_dist) + { + min_dist = dist; + closest_centroid_idx = centroid_idx; + } + if(dist > max_dist) + { + max_dist = dist; + farthest_centroid_idx = centroid_idx; + } + } + } + if (closest_centroid_idx == -1 || farthest_centroid_idx == -1) + { + RCLCPP_ERROR_STREAM(logger, "V2: No closest or farthest centroid found for point " << cp.x << " " << cp.y); + } + cluster_pq.push(ClusterPoint(cp.x, cp.y, closest_centroid_idx, min_dist - max_dist)); + } + //set cluster as full so we don't redo this procedure till another cluster is full + cluster_full[point.cluster] = true; + RCLCPP_INFO_STREAM(logger, "Cluster " << point.cluster << " full"); + } + // RCLCPP_INFO_STREAM(logger, cluster_pq.size()); + if(cluster_pq.empty()) + { + break; + } + } + return clusters; +} + +std::vector>> MissionManager::calculate_cluster_bounds(rclcpp::Logger logger, std::vector> clusters) const +{ + std::vector>> alpha_regions; + std::unordered_map, Segment, boost::hash>> boundary_edges; + if (clusters.size() == 0) + { + RCLCPP_ERROR(logger, "No clusters found"); + } + for(std::vector cluster : clusters) + { + //convert points inside polygon to cgal points + std::vector cgal_points; + for(ClusterPoint point : cluster) + { + cgal_points.push_back(Point(point.x, point.y)); + } + //calculate alpha shape formed by the polygon points + double alpha = 0.5; + Alpha_shape_2 A(cgal_points.begin(), cgal_points.end(), alpha, Alpha_shape_2::REGULARIZED); + //extract the boundary edges for the alpha shape + for (auto it = A.alpha_shape_edges_begin(); it != A.alpha_shape_edges_end(); ++it) + { + if (A.classify(*it) == Alpha_shape_2::REGULAR) + { + auto segment = A.segment(*it); + // Get the source and target points of the segment + Point source = segment.source(); + // Point target = segment.target(); + //put into our hashmap + boundary_edges[std::make_pair(source.x(), source.y())] = segment; + } + } + //grab the first key in the dictionary and set it's value as our start segment + std::pair start_key; + auto it = boundary_edges.begin(); + if (it != boundary_edges.end()) + { + start_key = it->first; + } + Segment start_seg = boundary_edges[start_key]; + Segment curr_seg = start_seg; + + //iterate through the segments and form the polygon + std::vector> region; + while(true) + { + //we want to find the target of our current segment as the source for a new segment + Point curr_seg_target = curr_seg.target(); + region.push_back({curr_seg_target.x(), curr_seg_target.y()}); + + //check if the current node target equals the start node source (we completed the loop) + if(curr_seg_target == start_seg.source()) + { + break; + } + + std::pair curr_key = std::make_pair(curr_seg_target.x(), curr_seg_target.y()); + if(boundary_edges.find(curr_key) != boundary_edges.end()) + { + Segment next_seg = boundary_edges[curr_key]; -std::vector MissionManager::assign_tasks(rclcpp::Logger logger) const + // RCLCPP_INFO_STREAM(logger, "(" << curr_seg.source().x() << "," << curr_seg.source().y() + // << ") (" << curr_seg.target().x() << "," << curr_seg.target().y() + // << ") -> (" << next_seg.source().x() << "," << next_seg.source().y() + // << ") (" << next_seg.target().x() << "," << next_seg.target().y() << ")"); + + curr_seg = next_seg; + } + } + alpha_regions.push_back(region); + } + return alpha_regions; +} + +std::vector>> MissionManager::allocate_search_map(rclcpp::Logger logger, + int number_of_search_tasks, + const airstack_msgs::msg::SearchMissionRequest &plan_request, + std::vector> &cluster_centroids, + std::vector> &clusters) +{ + std::vector CGAL_bounds; + //calculate random centroids + cluster_centroids = calculate_cluster_centroids(logger, number_of_search_tasks, plan_request, CGAL_bounds); + //calculate the clusters + clusters = calculate_clusters(logger, number_of_search_tasks, cluster_centroids, CGAL_bounds); + //calculate boundaries of clusters formed + return calculate_cluster_bounds(logger, clusters); +} + +std::vector MissionManager::assign_tasks( + rclcpp::Logger logger, + const airstack_msgs::msg::SearchMissionRequest &plan_request, + rclcpp::Publisher::SharedPtr pub, + bool visualize_search_allocation) { RCLCPP_INFO(logger, "Assigning tasks to drones"); // Find how many active robots - int number_of_agents = std::accumulate(valid_agents_.begin(), valid_agents_.end(), 0); + // int num_agents = std::accumulate(valid_agents_.begin(), valid_agents_.end(), 0); + int num_agents = 3; //MAGIC NUMBER. TODO:UPDATE FOR LINE ABOVE WHEN REST OF SYSTEM COMPLETE // Decide how many search vs track tasks to assign int number_of_track_tasks = 0; // TODO - int number_of_search_tasks = std::max(number_of_agents - number_of_track_tasks, 0); + int number_of_search_tasks = std::max(num_agents - number_of_track_tasks, 0); RCLCPP_INFO_STREAM(logger, "Assigning " << number_of_search_tasks << " search tasks and " << number_of_track_tasks << " track tasks"); + if (number_of_search_tasks > 3) + { + RCLCPP_ERROR(logger, "Too many search tasks requested for the current implementation"); + return {}; + } // Send out the request for the search map division - // TODO Nayana - - // Assign the search and track tasks to drones based on distance to the task + std::vector> cluster_centroids; + std::vector> clusters; + std::vector>> cluster_bounds = allocate_search_map(logger, number_of_search_tasks, plan_request, cluster_centroids, clusters); - // return the msg to be published - std::vector task_assignments(this->max_number_agents_); - for (int i = 0; i < this->max_number_agents_; i++) + //convert to task assignment + std::vector task_assignments(num_agents); + // TODO: should allocate both search and track. Right now assumes all search tasks + for(int i = 0; i < num_agents; ++i) { task_assignments[i].assigned_task_type = airstack_msgs::msg::TaskAssignment::SEARCH; task_assignments[i].assigned_task_number = i; + grid_map::Polygon search_prior_polygon; + search_prior_polygon.setFrameId(this->belief_map_.map_.getFrameId()); + for(std::vector coord : cluster_bounds[i]) + { + search_prior_polygon.addVertex(grid_map::Position(coord[0], coord[1])); // TODO coordinate frame + geometry_msgs::msg::Point32 new_point; + new_point.x = coord[0]; + new_point.y = coord[1]; + new_point.z = 0.0; + task_assignments[i].plan_request.search_bounds.points.push_back(new_point); + } + + // Copy over the cell probabilities and priorities into new task + airstack_msgs::msg::SearchPrior task_search_prior; + geometry_msgs::msg::Point32 point_prior; + grid_map::Matrix& probability_data = this->belief_map_.map_["probability"]; + grid_map::Matrix& priority_data = this->belief_map_.map_["priority"]; + for (grid_map::PolygonIterator iterator(this->belief_map_.map_, search_prior_polygon); + !iterator.isPastEnd(); ++iterator) + { + const grid_map::Index index(*iterator); + grid_map::Position position; + this->belief_map_.map_.getPosition(*iterator, position); + point_prior.x = position.x(); + point_prior.y = position.y(); + point_prior.z = 0.0; + task_search_prior.points_list.points.push_back(point_prior); + task_search_prior.value.push_back(probability_data(index(0), index(1))); + task_search_prior.priority.push_back(priority_data(index(0), index(1))); + } + task_assignments[i].plan_request.search_priors.push_back(task_search_prior); } + //publish visualizations for search request + if (visualize_search_allocation) + { + pub->publish(visualize_multi_agent_search_request(num_agents, cluster_centroids, clusters, cluster_bounds)); + } return task_assignments; } \ No newline at end of file diff --git a/ros_ws/src/ground_control_station/mission_manager/src/example_search_request.cpp b/ros_ws/src/ground_control_station/mission_manager/src/example_search_request.cpp index 77226383..4e4d669d 100644 --- a/ros_ws/src/ground_control_station/mission_manager/src/example_search_request.cpp +++ b/ros_ws/src/ground_control_station/mission_manager/src/example_search_request.cpp @@ -61,9 +61,9 @@ class MinimalPublisher : public rclcpp::Node msg.search_bounds = search_bounds; airstack_msgs::msg::SearchPrior search_prior; - search_prior.value = 0.5; - search_prior.priority = 10.0; - search_prior.sensor_model_id = 0; + search_prior.value.push_back(0.5); + search_prior.priority.push_back(10.0); + search_prior.sensor_model_id.push_back(0); search_prior.grid_prior_type = 1; search_prior.header.frame_id = "local_enu"; geometry_msgs::msg::Polygon grid_bounds; @@ -87,9 +87,9 @@ class MinimalPublisher : public rclcpp::Node msg.search_priors.push_back(search_prior); airstack_msgs::msg::SearchPrior search_prior2; - search_prior2.value = 0.2; - search_prior2.priority = 1.0; - search_prior2.sensor_model_id = 0; + search_prior2.value.push_back(0.2); + search_prior2.priority.push_back(1.0); + search_prior2.sensor_model_id.push_back(0); search_prior2.grid_prior_type = 1; search_prior2.header.frame_id = "local_enu"; geometry_msgs::msg::Polygon points_list;