Skip to content

Commit

Permalink
Brady/ipp global plan request (#112)
Browse files Browse the repository at this point in the history 
* 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.
  • Loading branch information
@bradygm
bradygm authored Oct 25, 2024
1 parent 86a19c1 commit 7b61f88
Show file tree
Hide file tree
Showing 5 changed files with 133 additions and 42 deletions.
9 changes: 8 additions & 1 deletion ros_ws/src/ground_control_station/mission_manager/config/mission_manager.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,11 @@
mission_manager_node:
ros__parameters:
grid_cell_size: 5.0
visualize_search_allocation: true
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
15 changes: 12 additions & 3 deletions ros_ws/src/ground_control_station/mission_manager/include/mission_manager/MissionManager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<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 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<bool> 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<rclcpp::Time> time_of_last_call_;
rclcpp::Time time_of_last_check_;
std::vector<geometry_msgs::msg::Pose> agent_poses_;
std::vector<bool> valid_agents_;
int scenario_coutner_ = 0;

//search map allocation
std::vector<std::vector<std::vector<double>>> allocate_search_map(rclcpp::Logger logger,
Expand Down
62 changes: 46 additions & 16 deletions ...src/ground_control_station/mission_manager/include/mission_manager/mission_manager_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,9 @@
#include <string>

#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"
Expand Down Expand Up @@ -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<airstack_msgs::msg::SearchMissionRequest>(
"search_mission_request", 1, std::bind(&MissionManagerNode::search_mission_request_callback, this, std::placeholders::_1));
Expand All @@ -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<std_msgs::msg::String>(
this->create_subscription<nav_msgs::msg::Odometry>(
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<airstack_msgs::msg::TaskAssignment>(agent_topic, 10));
Expand All @@ -77,7 +95,7 @@ class MissionManagerNode : public rclcpp::Node
belief_map_sub_ = this->create_subscription<airstack_msgs::msg::BeliefMapData>(
"belief_map_updates", 1, std::bind(&MissionManagerNode::belief_map_callback, this, std::placeholders::_1));

mission_manager_ = std::make_shared<MissionManager>(this->max_number_agents_);
mission_manager_ = std::make_shared<MissionManager>(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<grid_map_msgs::msg::GridMap>(
Expand All @@ -100,19 +118,21 @@ class MissionManagerNode : public rclcpp::Node
// Subscribers
rclcpp::Subscription<airstack_msgs::msg::SearchMissionRequest>::SharedPtr mission_subscriber_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr tracked_targets_sub_;
std::vector<rclcpp::Subscription<std_msgs::msg::String>::SharedPtr> agent_odoms_subs_;
std::vector<rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr> agent_odoms_subs_;
rclcpp::Subscription<airstack_msgs::msg::BeliefMapData>::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<MissionManager> mission_manager_;
double max_planning_time_;
double budget_;
double desired_speed_;

void publish_tasks(std::vector<airstack_msgs::msg::TaskAssignment> tasks) const
{
Expand All @@ -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);
}
}
}
Expand All @@ -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_));
}
}

Expand All @@ -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_));
}
}

Expand Down
15 changes: 11 additions & 4 deletions ros_ws/src/ground_control_station/mission_manager/src/BeliefMap.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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];
Expand Down Expand Up @@ -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<float>(new_belief_data->map_values[cur_index_num]) / static_cast<float>(UINT16_MAX); // TODO ensure maps are encoded correctly to match the iterator
probability_data(index(0), index(1)) = static_cast<float>(new_belief_data->map_values[cur_index_num]) / static_cast<float>(UINT16_MAX); // TODO: ensure maps are encoded correctly to match the iterator
}
cur_index_num++;
}
Expand Down
Loading

0 comments on commit 7b61f88

Please sign in to comment.