Search requests processed by mission_manager, belief map initialized,…

… visualized in rviz, and task publisher added. (#69)

* Add stump function for checking for target changes

* Created framework and logic for the assign_tasks member function. Returns array of task msgs

* Add function to publish tasks for active agents

* Add publisher for the search map

* Fix issues in task publisher

* Add mission_manager launch file

* Add reset_map for belief map

* Add example search mission request and node. belief map visualized.

* Add ros-humble-grid-map to dockerfile

* Add grid_map to package.xml of mission_manager

docker/airstack-dev.dockerfile

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

ros_ws/src/ground_control_station/mission_manager/CMakeLists.txt

@@ -10,21 +10,43 @@ 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)
 
 
 add_executable(mission_manager_node
   src/mission_manager_node.cpp
   src/MissionManager.cpp
   src/BeliefMap.cpp)
-ament_target_dependencies(mission_manager_node rclcpp std_msgs airstack_msgs)
+
+add_executable(example_search_request_node
+  src/example_search_request.cpp)
+
+ament_target_dependencies(mission_manager_node rclcpp std_msgs airstack_msgs grid_map_ros)
+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>)
+
+target_include_directories(example_search_request_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
 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
 
 install(TARGETS mission_manager_node
   DESTINATION lib/${PROJECT_NAME})
 
+install(TARGETS example_search_request_node
+  DESTINATION lib/${PROJECT_NAME})
+
+install(
+  DIRECTORY launch config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights

ros_ws/src/ground_control_station/mission_manager/README.md

@@ -3,7 +3,16 @@
 This package handles the allocation of tasks for the multiple agents.
 It assigned agents to search or track. For search it divides the search space.
 
+```
+colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+```
+
 Debugging this node
 ```
 ros2 run --prefix 'gdb -ex run --args'  mission_manager mission_manager_node
+```
+
+```
+ros2 launch mission_manager mission_manager_launch.py
+ros2 run rviz2 rviz2
 ```

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

@@ -0,0 +1,160 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /GridMap1
+      Splitter Ratio: 0.5
+    Tree Height: 872
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: false
+      Class: grid_map_rviz_plugin/GridMap
+      Color: 200; 200; 200
+      Color Layer: probability
+      Color Transformer: IntensityLayer
+      Enabled: true
+      Height Layer: probability
+      Height Transformer: GridMapLayer
+      History Length: 1
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 2
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: GridMap
+      Show Grid Lines: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /search_map_basestation
+      Use Rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 660.692138671875
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 92.63066101074219
+        Y: 60.50679016113281
+        Z: -87.42770385742188
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5047963857650757
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.693580150604248
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1163
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001bf000003f1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003f1000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003f1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003f1000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004a6000003f100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 1920
+  Y: 0

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

@@ -8,25 +8,36 @@
 #include <eigen3/Eigen/Dense>
 #include <float.h>
 
+#include <grid_map_ros/grid_map_ros.hpp>
+#include <geometry_msgs/msg/polygon.hpp>
+
+#include "airstack_msgs/msg/search_mission_request.hpp"
+#include "airstack_msgs/msg/search_prior.hpp"
+#include "airstack_msgs/msg/keep_out_zone.hpp"
 
 class BeliefMap
 {
-private:
-
 public:
 
   std::vector<std::vector<double>> polygon_bounds;
 
   // std::vector<planner_map_interfaces::GridPrior> prior_list;  // list of priors
 
-  // map use grid map toolbox
-
   // map of structs to keep track of various traits at each grid point
   // std::vector<std::vector<double>> priority;
   // std::vector<std::vector<int>> sensor_model_id;
 
   BeliefMap();
+  bool reset_map(rclcpp::Logger logger, airstack_msgs::msg::SearchMissionRequest search_mission_request);
+  grid_map::GridMap map_;
+
+  bool is_initialized() const
+  {
+    return !map_.getSize()(0) == 0;
+  }
 
+private:
+
 };
 
 #endif // BELIEFMAP_H

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

@@ -19,11 +19,13 @@ class MissionManager
   public:
     MissionManager(int max_number_agents);
 
-    void assign_tasks(rclcpp::Logger logger) const;
-    bool check_agent_changes(rclcpp::Logger, uint8_t robot_id, rclcpp::Time current_time);
+    std::vector<airstack_msgs::msg::TaskAssignment> assign_tasks(rclcpp::Logger logger) const;
+    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_; }
+    BeliefMap belief_map_; // TODO make private
 
   private: 
-    BeliefMap belief_map_;
     int max_number_agents_;
     std::vector<rclcpp::Time> time_of_last_call_;
     std::vector<bool> valid_agents_;

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

@@ -70,13 +70,20 @@ class MissionManagerNode : public rclcpp::Node
         "search_map_updates", 1, std::bind(&MissionManagerNode::search_map_callback, this, std::placeholders::_1));
 
       mission_manager_ = std::make_shared<MissionManager>(this->max_number_agents_);
+
+      // 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());
+      timer_ = this->create_wall_timer(
+        std::chrono::seconds(1), std::bind(&MissionManagerNode::search_map_publisher, this));
     }
 
   private:
     /* --- ROS SPECIFIC --- */
 
     // Publisher
     std::vector<rclcpp::Publisher<airstack_msgs::msg::TaskAssignment>::SharedPtr> plan_request_pubs_;
+    rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr search_map_publisher_;
 
     // Subscribers
     rclcpp::Subscription<airstack_msgs::msg::SearchMissionRequest>::SharedPtr mission_subscriber_;
@@ -88,41 +95,68 @@ class MissionManagerNode : public rclcpp::Node
     /* --- MEMBER ATTRIBUTES --- */
 
     size_t count_;
+    rclcpp::TimerBase::SharedPtr timer_;
     int max_number_agents_ = 5; // TODO: get from param server
     airstack_msgs::msg::SearchMissionRequest latest_search_mission_request_;
     std::shared_ptr<MissionManager> mission_manager_;
 
+    void publish_tasks(std::vector<airstack_msgs::msg::TaskAssignment> tasks) const
+    {
+      std::vector<bool> valid_agents = this->mission_manager_->get_valid_agents();
+      for (uint8_t i = 0; i < this->max_number_agents_; i++)
+      {
+        if (valid_agents[i])
+        {
+          plan_request_pubs_[i]->publish(tasks[i]);
+        }
+      }
+    }
+
+    void search_map_publisher()
+    {
+      if (this->mission_manager_->belief_map_.is_initialized())
+      {
+        this->mission_manager_->belief_map_.map_.setTimestamp(this->now().nanoseconds());
+        std::unique_ptr<grid_map_msgs::msg::GridMap> message;
+        message = grid_map::GridMapRosConverter::toMessage(this->mission_manager_->belief_map_.map_);
+        RCLCPP_DEBUG(
+          this->get_logger(), "Publishing grid map (timestamp %f).",
+          rclcpp::Time(message->header.stamp).nanoseconds() * 1e-9);
+        search_map_publisher_->publish(std::move(message));
+      }
+    }
+
     void search_mission_request_callback(const airstack_msgs::msg::SearchMissionRequest::SharedPtr msg)
     {
       RCLCPP_INFO(this->get_logger(), "Received new search mission request");
       latest_search_mission_request_ = *msg;
 
       // TODO: clear the map knowledge? Only if the search area has changed?
 
-      this->mission_manager_->assign_tasks(this->get_logger());
-      // TODO: publish the task assignment to the drones
+      // 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()));
     }
 
     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());
-      rclcpp::Time current_time = this->now();
-      if (this->mission_manager_->check_agent_changes(this->get_logger(), robot_id, current_time))
+      if (this->mission_manager_->check_agent_changes(this->get_logger(), robot_id, this->now()))
       {
-        this->mission_manager_->assign_tasks(this->get_logger());
+        this->publish_tasks(this->mission_manager_->assign_tasks(this->get_logger()));
       }
-      // TODO: publish the task assignment to the drones
     }
 
-    void tracked_targets_callback(const std_msgs::msg::String::SharedPtr msg) const
+    void tracked_targets_callback(const std_msgs::msg::String::SharedPtr msg)
     {
       RCLCPP_INFO(this->get_logger(), "Received target track list '%s'", msg->data.c_str());
       // TODO: save the list of tracked target
 
-      // TODO: check if change in the number of targets or id numbers
-      // if so, reassign tasks
-      // this->assign_tasks();
-      // TODO: publish the task assignment to the drones
+      // 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()));
+      }
     }
 
     void search_map_callback(const std_msgs::msg::String::SharedPtr msg) const