From 14c47197b8f8af185f091544aadaee06ba412671 Mon Sep 17 00:00:00 2001 From: Brady Moon Date: Wed, 2 Oct 2024 16:32:07 -0400 Subject: [PATCH] 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 | 3 +- .../mission_manager/CMakeLists.txt | 24 +- .../mission_manager/README.md | 9 + .../config/mission_manager.rviz | 160 +++++++++++++ .../include/mission_manager/BeliefMap.h | 19 +- .../include/mission_manager/MissionManager.h | 8 +- .../mission_manager/mission_manager_node.h | 56 ++++- .../launch/mission_manager_launch.py | 46 ++++ .../mission_manager/package.xml | 3 + .../mission_manager/src/BeliefMap.cpp | 93 +++++++- .../mission_manager/src/MissionManager.cpp | 33 ++- .../src/example_search_request.cpp | 224 ++++++++++++++++++ 12 files changed, 653 insertions(+), 25 deletions(-) create mode 100644 ros_ws/src/ground_control_station/mission_manager/config/mission_manager.rviz create mode 100644 ros_ws/src/ground_control_station/mission_manager/launch/mission_manager_launch.py create mode 100644 ros_ws/src/ground_control_station/mission_manager/src/example_search_request.cpp diff --git a/docker/airstack-dev.dockerfile b/docker/airstack-dev.dockerfile index f56d824a..f3e6eff0 100644 --- a/docker/airstack-dev.dockerfile +++ b/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 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 ffda41d3..91b5b78e 100644 --- a/ros_ws/src/ground_control_station/mission_manager/CMakeLists.txt +++ b/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 $ $) + +target_include_directories(example_search_request_node PUBLIC + $ + $) + 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 diff --git a/ros_ws/src/ground_control_station/mission_manager/README.md b/ros_ws/src/ground_control_station/mission_manager/README.md index 48b2d133..c940d451 100644 --- a/ros_ws/src/ground_control_station/mission_manager/README.md +++ b/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 ``` \ No newline at end of file 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 new file mode 100644 index 00000000..c9e240f6 --- /dev/null +++ b/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: + 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: + 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 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 c71944c5..60c5bbae 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 @@ -8,25 +8,36 @@ #include #include +#include +#include + +#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> polygon_bounds; // std::vector 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> priority; // std::vector> 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 \ 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 9fd22def..eac347b3 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 @@ -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 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 get_valid_agents() const { return valid_agents_; } + BeliefMap belief_map_; // TODO make private private: - BeliefMap belief_map_; int max_number_agents_; std::vector time_of_last_call_; std::vector valid_agents_; 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 28bbf223..42525947 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 @@ -70,6 +70,12 @@ class MissionManagerNode : public rclcpp::Node "search_map_updates", 1, std::bind(&MissionManagerNode::search_map_callback, this, std::placeholders::_1)); mission_manager_ = std::make_shared(this->max_number_agents_); + + // 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()); + timer_ = this->create_wall_timer( + std::chrono::seconds(1), std::bind(&MissionManagerNode::search_map_publisher, this)); } private: @@ -77,6 +83,7 @@ class MissionManagerNode : public rclcpp::Node // Publisher std::vector::SharedPtr> plan_request_pubs_; + rclcpp::Publisher::SharedPtr search_map_publisher_; // Subscribers rclcpp::Subscription::SharedPtr mission_subscriber_; @@ -88,10 +95,37 @@ 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 mission_manager_; + void publish_tasks(std::vector tasks) const + { + std::vector 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 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"); @@ -99,30 +133,30 @@ class MissionManagerNode : public rclcpp::Node // 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 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 new file mode 100644 index 00000000..d077ddba --- /dev/null +++ b/ros_ws/src/ground_control_station/mission_manager/launch/mission_manager_launch.py @@ -0,0 +1,46 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + + # Get the package share directory + package_share_directory = get_package_share_directory('mission_manager') + print(package_share_directory) + + # 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') + + mission_manager = Node( + package='mission_manager', + # namespace='mission_manager', + executable='mission_manager_node', + output="screen", + name='mission_manager_node', + ) + + # grid_map_visualization_node = Node( + # package='grid_map_visualization', + # executable='grid_map_visualization', + # name='grid_map_visualization', + # output='screen', + # parameters=[visualization_config_file] + # ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + ld = LaunchDescription() + + ld.add_action(rviz2_node) + # ld.add_action(grid_map_visualization_node) + ld.add_action(mission_manager) + + return ld \ No newline at end of file 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 90fc5fa9..ca599679 100644 --- a/ros_ws/src/ground_control_station/mission_manager/package.xml +++ b/ros_ws/src/ground_control_station/mission_manager/package.xml @@ -14,10 +14,13 @@ rclcpp std_msgs airstack_msgs + grid_map ament_lint_auto ament_lint_common + ros2launch + ament_cmake 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 83c071d4..33b5ba07 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 @@ -1,6 +1,91 @@ #include "mission_manager/BeliefMap.h" -/* -Empty Constructor -*/ -BeliefMap::BeliefMap() { } \ No newline at end of file + +BeliefMap::BeliefMap() +: map_(std::vector({"probability"})) +{ } + +bool BeliefMap::reset_map(rclcpp::Logger logger, airstack_msgs::msg::SearchMissionRequest search_mission_request) +{ + 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; + for (const auto& point : search_mission_request.search_bounds.points) + { + if (point.x < min_x) + { + min_x = point.x; + } + if (point.x > max_x) + { + max_x = point.x; + } + if (point.y < min_y) + { + min_y = point.y; + } + if (point.y > max_y) + { + max_y = point.y; + } + } + // 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; + 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_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 + map_.clearAll(); + + for (auto& search_prior : search_mission_request.search_priors) + { + if (search_prior.grid_prior_type == airstack_msgs::msg::SearchPrior::POLYGON_PRIOR) + { + grid_map::Polygon polygon; + polygon.setFrameId(map_.getFrameId()); + for (const auto& point : search_prior.points_list.points) + { + polygon.addVertex(grid_map::Position(point.x, point.y)); // TODO coordinate frame + } + for (grid_map::PolygonIterator iterator(map_, polygon); + !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; + } + } + } + else if (search_prior.grid_prior_type == airstack_msgs::msg::SearchPrior::LINE_SEG_PRIOR) + { + // TODO + } + else if (search_prior.grid_prior_type == airstack_msgs::msg::SearchPrior::POINT_PRIOR) + { + // TODO + } + else + { + RCLCPP_ERROR(logger, "Unknown grid prior type"); + } + } + + + return true; +} + +// TODO subscribe to the shared search areas for updates. \ No newline at end of file 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 f693a26e..739dd5ee 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 @@ -39,9 +39,40 @@ bool MissionManager::check_agent_changes(rclcpp::Logger logger, uint8_t robot_id return change_in_agents; } +bool MissionManager::check_target_changes(rclcpp::Logger logger, std::string target_list, rclcpp::Time current_time) +{ + // TODO + RCLCPP_INFO_STREAM(logger, "Checking target changes at time " + << current_time.nanoseconds() << " with target list " << target_list); + return false; +} + // TODO -void MissionManager::assign_tasks(rclcpp::Logger logger) const +std::vector MissionManager::assign_tasks(rclcpp::Logger logger) const { 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); + + // 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); + RCLCPP_INFO_STREAM(logger, "Assigning " << number_of_search_tasks << " search tasks and " << number_of_track_tasks << " track tasks"); + + // 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 + + // return the msg to be published + std::vector task_assignments(this->max_number_agents_); + for (int i = 0; i < this->max_number_agents_; i++) + { + task_assignments[i].assigned_task_type = airstack_msgs::msg::TaskAssignment::SEARCH; + task_assignments[i].assigned_task_number = i; + } + + 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 new file mode 100644 index 00000000..f44c03bf --- /dev/null +++ b/ros_ws/src/ground_control_station/mission_manager/src/example_search_request.cpp @@ -0,0 +1,224 @@ +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include "airstack_msgs/msg/search_mission_request.hpp" +#include "airstack_msgs/msg/search_prior.hpp" +#include "airstack_msgs/msg/keep_out_zone.hpp" +#include "geometry_msgs/msg/polygon.hpp" +#include "geometry_msgs/msg/point32.hpp" + + +using namespace std::chrono_literals; + +/* This example creates a subclass of Node and uses std::bind() to register a +* member function as a callback from the timer. */ + +class MinimalPublisher : public rclcpp::Node +{ + public: + MinimalPublisher() + : Node("minimal_publisher") + { + publisher_ = this->create_publisher("search_mission_request", 10); + airstack_msgs::msg::SearchMissionRequest msg; + msg.header.stamp = this->now(); + msg.header.frame_id = "map"; // TODO + + geometry_msgs::msg::Polygon search_bounds; + geometry_msgs::msg::Point32 point; + point.x = 0; + point.y = 0; + point.z = 0; + search_bounds.points.push_back(point); + point.x = 100; + point.y = -28; + point.z = 0; + search_bounds.points.push_back(point); + point.x = 230; + point.y = 0; + point.z = 0; + search_bounds.points.push_back(point); + point.x = 250; + point.y = 100; + point.z = 0; + search_bounds.points.push_back(point); + point.x = 130; + point.y = 156; + point.z = 0; + search_bounds.points.push_back(point); + point.x = 110; + point.y = 191; + point.z = 0; + search_bounds.points.push_back(point); + point.x = -40; + point.y = 141; + point.z = 0; + search_bounds.points.push_back(point); + 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.grid_prior_type = 1; + search_prior.header.frame_id = "local_enu"; + geometry_msgs::msg::Polygon grid_bounds; + point.x = 0; + point.y = 160; + point.z = 0; + grid_bounds.points.push_back(point); + point.x = 12; + point.y = 135; + point.z = 0; + grid_bounds.points.push_back(point); + point.x = 92; + point.y = 150; + point.z = 0; + grid_bounds.points.push_back(point); + point.x = 80; + point.y = 175; + point.z = 0; + grid_bounds.points.push_back(point); + search_prior.points_list = grid_bounds; + 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.grid_prior_type = 1; + search_prior2.header.frame_id = "local_enu"; + geometry_msgs::msg::Polygon points_list; + point.x = 0; + point.y = 0; + point.z = 0; + points_list.points.push_back(point); + point.x = 100; + point.y = -28; + point.z = 0; + points_list.points.push_back(point); + point.x = 230; + point.y = 0; + point.z = 0; + points_list.points.push_back(point); + point.x = 250; + point.y = 100; + point.z = 0; + points_list.points.push_back(point); + point.x = 130; + point.y = 156; + point.z = 0; + points_list.points.push_back(point); + point.x = 110; + point.y = 191; + point.z = 0; + points_list.points.push_back(point); + point.x = -40; + point.y = 141; + point.z = 0; + points_list.points.push_back(point); + search_prior2.points_list = points_list; + msg.search_priors.push_back(search_prior2); + + + + publisher_->publish(msg); + RCLCPP_INFO(this->get_logger(), "Publishing example search request"); + // Shutdown after publishing + // rclcpp::shutdown(); + } + + private: + rclcpp::Publisher::SharedPtr publisher_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} + + +/* + +target_priors: +- grid_prior: + bounds: + points: + - x: 0 + y: 0 + z: 0 + - x: 100 + y: -28 + z: 0 + - x: 230 + y: 0 + z: 0 + - x: 250 + y: 100 + z: 0 + - x: 130 + y: 156 + z: 0 + - x: 110 + y: 191 + z: 0 + - x: -40 + y: 141 + z: 0 + confidence: 0.2 + grid_prior_type: 1 + header: + frame_id: local_enu + priority: 1.0 + sensor_model_id: 0 +- grid_prior: + bounds: + points: + - x: 0 + y: 160 + z: 0 + - x: 12 + y: 135 + z: 0 + - x: 92 + y: 150 + z: 0 + - x: 80 + y: 175 + z: 0 + confidence: 0.5 + grid_prior_type: 1 + header: + frame_id: local_enu + priority: 10.0 + sensor_model_id: 0 + +# keep out zone +keep_out_zones: + - + header: + frame_id: "local_enu" + x: 0 + y: 139 + radius: 15 + - + header: + frame_id: "local_enu" + x: 192 + y: -21 + radius: 10 + - + header: + frame_id: "local_enu" + x: 210 + y: -10 + radius: 8 + +*/ \ No newline at end of file