From 7b61f8824aeba7735f501d52f1ae2ed547d2e01d Mon Sep 17 00:00:00 2001 From: Brady Moon Date: Fri, 25 Oct 2024 09:14:32 -0400 Subject: [PATCH] Brady/ipp global plan request (#112) * Faster method for iterating the grid map when reseting * mission manager: don't overwrite the grid_map values from drones search maps that are equal to 0 * Update method to check if agents are active. Checks min altitude. * Parameterize more variables * Save agent poses to be used later in the plan request * Add all params for the plan request. Fixed issue of assigning search tasks to drones not active. --- .../config/mission_manager.yaml | 9 ++- .../include/mission_manager/MissionManager.h | 15 +++- .../mission_manager/mission_manager_node.h | 62 ++++++++++++---- .../mission_manager/src/BeliefMap.cpp | 15 +++- .../mission_manager/src/MissionManager.cpp | 74 ++++++++++++++----- 5 files changed, 133 insertions(+), 42 deletions(-) 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 index 6fe12c30..79d2d397 100644 --- 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 @@ -1,4 +1,11 @@ mission_manager_node: ros__parameters: grid_cell_size: 5.0 - visualize_search_allocation: true \ No newline at end of file + visualize_search_allocation: true + max_number_agents: 3 + active_agent_check_n_seconds: 5.0 + min_agent_altitude_to_be_active: 3.0 + time_till_agent_not_valid: 10.0 + max_planning_time: 5.0 + budget: 100.0 + desired_speed: 5.0 \ No newline at end of file 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 ee0dbc56..b961951d 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 @@ -66,21 +66,30 @@ struct ClusterPoint class MissionManager { public: - MissionManager(int max_number_agents); + MissionManager(int max_number_agents, double active_agent_check_n_seconds, double min_agent_altitude_to_be_active, double time_till_agent_not_valid); 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 visualize_search_allocation, + double max_planning_time, + double budget, + double desired_speed); + bool check_agent_changes(rclcpp::Logger logger, uint8_t robot_id, geometry_msgs::msg::Pose robot_pose, 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_; } BeliefMap belief_map_; // TODO make private private: int max_number_agents_; + rclcpp::Duration active_agent_check_n_seconds_; + double min_agent_altitude_to_be_active_; + rclcpp::Duration time_till_agent_not_valid_; std::vector time_of_last_call_; + rclcpp::Time time_of_last_check_; + std::vector agent_poses_; std::vector valid_agents_; + int scenario_coutner_ = 0; //search map allocation std::vector>> allocate_search_map(rclcpp::Logger logger, 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 8a10412d..ecc329d0 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 @@ -9,7 +9,9 @@ #include #include "rclcpp/rclcpp.hpp" +// #include "airstack_common/ros2_helper.hpp" #include "std_msgs/msg/string.hpp" +#include "nav_msgs/msg/odometry.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include "airstack_msgs/msg/search_mission_request.hpp" #include "airstack_msgs/msg/task_assignment.hpp" @@ -40,13 +42,31 @@ class MissionManagerNode : public rclcpp::Node { public: MissionManagerNode() - : Node("mission_manager"), count_(0) + : Node("mission_manager") { + double min_agent_altitude_to_be_active; + double active_agent_check_n_seconds; + double time_till_agent_not_valid; + + // grid_cell_size_ = airstack::get_param(this, "grid_cell_size", 10.0); this->declare_parameter("grid_cell_size", 10.0); this->declare_parameter("visualize_search_allocation", false); + this->declare_parameter("max_number_agents", 5); + this->declare_parameter("active_agent_check_n_seconds", 5.0); + this->declare_parameter("min_agent_altitude_to_be_active", 2.0); + this->declare_parameter("time_till_agent_not_valid", 10.0); + this->declare_parameter("max_planning_time", 5.0); + this->declare_parameter("budget", 100.0); + this->declare_parameter("desired_speed", 4.0); this->get_parameter("grid_cell_size", grid_cell_size_); this->get_parameter("visualize_search_allocation", visualize_search_allocation_); - + this->get_parameter("max_number_agents", max_number_agents_); + this->get_parameter("active_agent_check_n_seconds", active_agent_check_n_seconds); + this->get_parameter("min_agent_altitude_to_be_active", min_agent_altitude_to_be_active); + this->get_parameter("time_till_agent_not_valid", time_till_agent_not_valid); + this->get_parameter("max_planning_time", max_planning_time_); + this->get_parameter("budget", budget_); + this->get_parameter("desired_speed", desired_speed_); mission_subscriber_ = this->create_subscription( "search_mission_request", 1, std::bind(&MissionManagerNode::search_mission_request_callback, this, std::placeholders::_1)); @@ -56,16 +76,14 @@ class MissionManagerNode : public rclcpp::Node { std::string topic_name = "agent_" + std::to_string(i) + "/odom"; agent_odoms_subs_.push_back( - this->create_subscription( + this->create_subscription( topic_name, 1, - [this, i](const std_msgs::msg::String::SharedPtr msg) { + [this, i](const nav_msgs::msg::Odometry::SharedPtr msg) { this->agent_odom_callback(msg, i); } ) ); - - std::string agent_topic = "agent_" + std::to_string(i) + "/plan_request"; plan_request_pubs_.push_back( this->create_publisher(agent_topic, 10)); @@ -77,7 +95,7 @@ class MissionManagerNode : public rclcpp::Node belief_map_sub_ = this->create_subscription( "belief_map_updates", 1, std::bind(&MissionManagerNode::belief_map_callback, this, std::placeholders::_1)); - mission_manager_ = std::make_shared(this->max_number_agents_); + mission_manager_ = std::make_shared(this->max_number_agents_, active_agent_check_n_seconds, min_agent_altitude_to_be_active, time_till_agent_not_valid); // TODO: set param for rate, make sure not communicated over network search_map_publisher_ = this->create_publisher( @@ -100,19 +118,21 @@ class MissionManagerNode : public rclcpp::Node // Subscribers rclcpp::Subscription::SharedPtr mission_subscriber_; rclcpp::Subscription::SharedPtr tracked_targets_sub_; - std::vector::SharedPtr> agent_odoms_subs_; + std::vector::SharedPtr> agent_odoms_subs_; rclcpp::Subscription::SharedPtr belief_map_sub_; /* --- MEMBER ATTRIBUTES --- */ - 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 + int max_number_agents_; // TODO: get from param server airstack_msgs::msg::SearchMissionRequest latest_search_mission_request_; std::shared_ptr mission_manager_; + double max_planning_time_; + double budget_; + double desired_speed_; void publish_tasks(std::vector tasks) const { @@ -122,6 +142,7 @@ class MissionManagerNode : public rclcpp::Node if (valid_agents[i]) { plan_request_pubs_[i]->publish(tasks[i]); + RCLCPP_INFO(this->get_logger(), "Published task assignment for agent %d", i); } } } @@ -147,15 +168,21 @@ class MissionManagerNode : public rclcpp::Node // TODO: visualize the seach mission request 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_)); + this->publish_tasks(this->mission_manager_->assign_tasks( + this->get_logger(), latest_search_mission_request_, + viz_pub_, visualize_search_allocation_, + max_planning_time_, budget_, desired_speed_)); } - void agent_odom_callback(const std_msgs::msg::String::SharedPtr msg, const uint8_t &robot_id) + void agent_odom_callback(const nav_msgs::msg::Odometry::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())) + RCLCPP_INFO(this->get_logger(), "Received agent odom for robot %d", robot_id); + if (this->mission_manager_->check_agent_changes(this->get_logger(), robot_id, msg->pose.pose, this->now())) { - this->publish_tasks(this->mission_manager_->assign_tasks(this->get_logger(), latest_search_mission_request_, viz_pub_, visualize_search_allocation_)); + this->publish_tasks(this->mission_manager_->assign_tasks( + this->get_logger(), latest_search_mission_request_, + viz_pub_, visualize_search_allocation_, + max_planning_time_, budget_, desired_speed_)); } } @@ -167,7 +194,10 @@ 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(), latest_search_mission_request_, viz_pub_, visualize_search_allocation_)); + this->publish_tasks(this->mission_manager_->assign_tasks( + this->get_logger(), latest_search_mission_request_, + viz_pub_, visualize_search_allocation_, + max_planning_time_, budget_, desired_speed_)); } } 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 9ee8bbd1..889868e2 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 @@ -66,11 +66,16 @@ bool BeliefMap::reset_map(rclcpp::Logger logger, airstack_msgs::msg::SearchMissi { polygon.addVertex(grid_map::Position(point.x, point.y)); // TODO coordinate frame } + grid_map::Matrix& probability_data = map_["probability"]; + grid_map::Matrix& priority_data = map_["priority"]; for (grid_map::PolygonIterator iterator(map_, polygon); !iterator.isPastEnd(); ++iterator) { - float& current_value = map_.at("probability", *iterator); - float& current_priority = map_.at("priority", *iterator); + // TODO: Check if the cell is in the search boundary (CGAL)? Not necessary I think. + const grid_map::Index index(*iterator); + float& current_value = probability_data(index(0), index(1)); + float& current_priority = priority_data(index(0), index(1)); + 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]; @@ -117,10 +122,12 @@ bool BeliefMap::update_map(rclcpp::Logger logger, const airstack_msgs::msg::Beli // 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] < probability_data(index(0), index(1))) + // TODO: Check not overwriting the values that are outside of the drone search area + if (new_belief_data->map_values[cur_index_num] != 0 && + 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 - 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 + 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 e8d497ba..0e99e798 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 @@ -4,20 +4,32 @@ /* Empty Constructor */ -MissionManager::MissionManager(int max_number_agents) : max_number_agents_(max_number_agents) +MissionManager::MissionManager(int max_number_agents, double active_agent_check_n_seconds, double min_agent_altitude_to_be_active, double time_till_agent_not_valid) : + max_number_agents_(max_number_agents), + active_agent_check_n_seconds_(rclcpp::Duration::from_seconds(active_agent_check_n_seconds)), + min_agent_altitude_to_be_active_(min_agent_altitude_to_be_active), + time_till_agent_not_valid_(rclcpp::Duration::from_seconds(time_till_agent_not_valid)) { rclcpp::Time default_time(0, 0, RCL_ROS_TIME); time_of_last_call_.resize(max_number_agents_, default_time); + time_of_last_check_ = default_time; + agent_poses_.resize(max_number_agents_); valid_agents_.resize(max_number_agents_, false); } -bool MissionManager::check_agent_changes(rclcpp::Logger logger, uint8_t robot_id, rclcpp::Time current_time) +bool MissionManager::check_agent_changes(rclcpp::Logger logger, uint8_t robot_id, geometry_msgs::msg::Pose robot_pose, rclcpp::Time current_time) { RCLCPP_INFO(logger, "Checking agent changes"); time_of_last_call_[robot_id] = current_time; + agent_poses_[robot_id] = robot_pose; - // TODO this logic does not need to happen every time an odom is received + // Only check at the specified number of loops + if (time_of_last_check_ - current_time < active_agent_check_n_seconds_) + { + return false; + } + time_of_last_check_ = current_time; // Check how many agents have reported in the last x seconds // If change in the agents reporting, reassign tasks @@ -25,7 +37,8 @@ bool MissionManager::check_agent_changes(rclcpp::Logger logger, uint8_t robot_id rclcpp::Duration time_till_agent_not_valid = rclcpp::Duration::from_seconds(10.0); for (uint8_t i = 0; i < max_number_agents_; i++) { - if (current_time - time_of_last_call_[i] < time_till_agent_not_valid) + if (current_time - time_of_last_call_[i] < time_till_agent_not_valid && + agent_poses_[i].position.z > min_agent_altitude_to_be_active_) { curr_valid_agents[i] = true; } @@ -48,11 +61,11 @@ 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::vector> MissionManager::calculate_cluster_centroids(rclcpp::Logger logger, int num_agents, const airstack_msgs::msg::SearchMissionRequest &search_mission_request, std::vector &CGAL_bounds) const { std::random_device dev; std::mt19937 rng(dev()); - for(auto& point : plan_request.search_bounds.points) + for(auto& point : search_mission_request.search_bounds.points) { // add to vector of bounds CGAL_bounds.push_back(Point(point.x, point.y)); @@ -215,7 +228,7 @@ std::vector> MissionManager::calculate_clusters(rclcpp } //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 " << point.cluster << " full"); } // RCLCPP_INFO_STREAM(logger, cluster_pq.size()); if(cluster_pq.empty()) @@ -302,13 +315,13 @@ std::vector>> MissionManager::calculate_cluster_ std::vector>> MissionManager::allocate_search_map(rclcpp::Logger logger, int number_of_search_tasks, - const airstack_msgs::msg::SearchMissionRequest &plan_request, + const airstack_msgs::msg::SearchMissionRequest &search_mission_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); + cluster_centroids = calculate_cluster_centroids(logger, number_of_search_tasks, search_mission_request, CGAL_bounds); //calculate the clusters clusters = calculate_clusters(logger, number_of_search_tasks, cluster_centroids, CGAL_bounds); //calculate boundaries of clusters formed @@ -317,38 +330,61 @@ std::vector>> MissionManager::allocate_search_ma std::vector MissionManager::assign_tasks( rclcpp::Logger logger, - const airstack_msgs::msg::SearchMissionRequest &plan_request, + const airstack_msgs::msg::SearchMissionRequest &search_mission_request, rclcpp::Publisher::SharedPtr pub, - bool visualize_search_allocation) + bool visualize_search_allocation, + double max_planning_time, + double budget, + double desired_speed) { RCLCPP_INFO(logger, "Assigning tasks to drones"); // Find how many active robots - // 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 + // int num_active_agents = std::accumulate(valid_agents_.begin(), valid_agents_.end(), 0); + int num_active_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(num_agents - number_of_track_tasks, 0); + int number_of_search_tasks = std::max(num_active_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 {}; } + if (number_of_search_tasks == 0) + { + RCLCPP_WARN(logger, "No activate agents available for search tasks"); + return {}; + } // Send out the request for the search map division std::vector> cluster_centroids; std::vector> clusters; - std::vector>> cluster_bounds = allocate_search_map(logger, number_of_search_tasks, plan_request, cluster_centroids, clusters); + std::vector>> cluster_bounds = allocate_search_map(logger, number_of_search_tasks, search_mission_request, cluster_centroids, clusters); //convert to task assignment - std::vector task_assignments(num_agents); + std::vector task_assignments(max_number_agents_); // TODO: should allocate both search and track. Right now assumes all search tasks - for(int i = 0; i < num_agents; ++i) + for(int i = 0; i < max_number_agents_; ++i) { + if (!valid_agents_[i]) + { + continue; + } task_assignments[i].assigned_task_type = airstack_msgs::msg::TaskAssignment::SEARCH; task_assignments[i].assigned_task_number = i; + task_assignments[i].plan_request.start_pose = agent_poses_[i]; + task_assignments[i].plan_request.max_planning_time = max_planning_time; + task_assignments[i].plan_request.maximum_range = budget; + + task_assignments[i].plan_request.desired_speed = desired_speed; + task_assignments[i].plan_request.search_bounds = search_mission_request.search_bounds; + + task_assignments[i].plan_request.clear_tree = true; + task_assignments[i].plan_request.scenario = scenario_coutner_; + + grid_map::Polygon search_prior_polygon; search_prior_polygon.setFrameId(this->belief_map_.map_.getFrameId()); for(std::vector coord : cluster_bounds[i]) @@ -385,7 +421,9 @@ std::vector MissionManager::assign_tasks( //publish visualizations for search request if (visualize_search_allocation) { - pub->publish(visualize_multi_agent_search_request(num_agents, cluster_centroids, clusters, cluster_bounds)); + pub->publish(visualize_multi_agent_search_request(num_active_agents, cluster_centroids, clusters, cluster_bounds)); } + + scenario_coutner_++; return task_assignments; } \ No newline at end of file