Add params for mission manager

Author: bradygm

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/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_;
 
@@ -62,6 +62,7 @@ class BeliefMap
   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

@@ -68,7 +68,10 @@ class MissionManager
   public:
     MissionManager(int max_number_agents);
 
-    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);
+    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_; }

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

@@ -42,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));
 
@@ -102,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_;
@@ -138,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(), latest_search_mission_request_, viz_pub_));
+      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(), latest_search_mission_request_, viz_pub_));
+        this->publish_tasks(this->mission_manager_->assign_tasks(this->get_logger(), latest_search_mission_request_, viz_pub_, visualize_search_allocation_));
       }
     }
 
@@ -159,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(), latest_search_mission_request_, viz_pub_));
+        this->publish_tasks(this->mission_manager_->assign_tasks(this->get_logger(), latest_search_mission_request_, viz_pub_, visualize_search_allocation_));
       }
     }
 

ros_ws/src/ground_control_station/mission_manager/launch/mission_manager_launch.py

@@ -12,13 +12,20 @@ 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',
         # namespace='mission_manager',
         executable='mission_manager_node',
         output="screen",
         name='mission_manager_node',
+        parameters=[config]
     )
 
     # grid_map_visualization_node = Node(

ros_ws/src/ground_control_station/mission_manager/src/BeliefMap.cpp

@@ -5,12 +5,12 @@ BeliefMap::BeliefMap()
 : map_(std::vector<std::string>({"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;
+  grid_cell_size_ = grid_cell_size;
   min_x = DBL_MAX;
   max_x = -DBL_MAX;
   min_y = DBL_MAX;
@@ -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

ros_ws/src/ground_control_station/mission_manager/src/MissionManager.cpp

@@ -318,7 +318,8 @@ std::vector<std::vector<std::vector<double>>> MissionManager::allocate_search_ma
 std::vector<airstack_msgs::msg::TaskAssignment> MissionManager::assign_tasks(
   rclcpp::Logger logger,
   const airstack_msgs::msg::SearchMissionRequest &plan_request,
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub)
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub,
+  bool visualize_search_allocation)
 {
   RCLCPP_INFO(logger, "Assigning tasks to drones");
 
@@ -382,7 +383,9 @@ std::vector<airstack_msgs::msg::TaskAssignment> MissionManager::assign_tasks(
   }
 
   //publish visualizations for search request
-  pub->publish(visualize_multi_agent_search_request(num_agents, cluster_centroids, clusters, cluster_bounds));
-
+  if (visualize_search_allocation)
+  {
+    pub->publish(visualize_multi_agent_search_request(num_agents, cluster_centroids, clusters, cluster_bounds));
+  }
   return task_assignments;
 }