From 2d780c33c453217c25525b867293c287b949c126 Mon Sep 17 00:00:00 2001 From: wingdeans <66850754+wingdeans@users.noreply.github.com> Date: Wed, 13 May 2026 16:17:58 -0400 Subject: [PATCH] remove unused packages --- .gitignore | 1 - .gitmodules | 3 - ext/FTXUI | 1 - src/mil_common/mil_preflight/CMakeLists.txt | 59 -- src/mil_common/mil_preflight/cfg/config.json | 107 --- .../include/mil_preflight/common.h | 10 - .../mil_preflight/include/mil_preflight/job.h | 449 ---------- .../include/mil_preflight/plugin.h | 134 --- .../mil_preflight/include/mil_preflight/ui.h | 59 -- .../include/mil_preflight/uis/ftxui/dialog.h | 96 --- .../mil_preflight/uis/ftxui/reportsPage.h | 30 - .../mil_preflight/uis/ftxui/testsPage.h | 46 - src/mil_common/mil_preflight/package.xml | 23 - src/mil_common/mil_preflight/src/backend.cpp | 21 - src/mil_common/mil_preflight/src/frontend.cpp | 17 - .../src/plugins/actuator_plugin.cpp | 137 --- .../mil_preflight/src/plugins/node_plugin.cpp | 55 -- .../src/plugins/setup_plugin.cpp | 46 - .../src/plugins/topic_plugin.cpp | 56 -- .../mil_preflight/src/uis/ftxui/ui.cpp | 148 ---- .../mil_preflight/src/uis/ftxui/widgets.cpp | 811 ------------------ .../gnc/subjugator_centroids/package.xml | 18 - .../random_code/green_tracker_cv2_example.py | 91 -- .../resource/subjugator_centroids | 0 .../gnc/subjugator_centroids/setup.cfg | 4 - .../gnc/subjugator_centroids/setup.py | 25 - .../subjugator_centroids/__init__.py | 0 .../subjugator_centroids/centroid_finder.py | 19 - .../subjugator_centroids/green_tracker.py | 51 -- .../subjugator_centroids/orange_tracker.py | 50 -- .../subjugator_centroids/red_tracker.py | 76 -- .../subjugator_centroids_node.py | 57 -- .../config/wrench_tuner_params.yaml | 11 - .../launch/wrench_tuner_launch.py | 26 - .../gnc/subjugator_wrench_tuner/package.xml | 22 - .../resource/subjugator_wrench_tuner | 0 .../gnc/subjugator_wrench_tuner/setup.cfg | 4 - .../gnc/subjugator_wrench_tuner/setup.py | 27 - .../subjugator_wrench_tuner/__init__.py | 0 .../subjugator_wrench_tuner/wrench_tuner.py | 164 ---- .../subjugator_mission_planner/MANIFEST.in | 1 - .../subjugator_mission_planner/in | 0 .../launch/mission_planner_launch.py | 26 - .../launch/task_server_launch.py | 51 -- .../missions/mechanism_test.yaml | 37 - .../missions/mission1_example.yaml | 19 - .../missions/move_test.yaml | 13 - .../missions/nav_channel_test.yaml | 5 - .../missions/navigate_around_test.yaml | 6 - .../missions/navigate_channel.yaml | 9 - .../missions/prequal.yaml | 63 -- .../missions/prequal_mission.yaml | 21 - .../missions/sonar_follower_test.yaml | 5 - .../missions/start_gate.yaml | 8 - .../missions/test_all.yaml | 27 - .../missions/wait_test.yaml | 5 - .../missions/yaw_tracker_test.yaml | 5 - .../subjugator_mission_planner/package.xml | 33 - .../resource/subjugator_mission_planner | 0 .../subjugator_mission_planner/setup.cfg | 4 - .../subjugator_mission_planner/setup.py | 47 - .../subjugator_mission_planner/__init__.py | 0 .../mechanisms_server.py | 67 -- .../mission_planner.py | 181 ---- .../nav_channel_server.py | 282 ------ .../navigate_around_server.py | 171 ---- .../navigation_channel_server.py | 505 ----------- .../sonar_follower_server.py | 174 ---- .../start_gate_server.py | 144 ---- .../subjugator_mission_planner/wait_server.py | 53 -- .../yaw_tracker_server.py | 144 ---- .../centroid_yaw_tracker/__init__.py | 0 .../centroid_yaw_tracker_node.py | 94 -- .../testing/centroid_yaw_tracker/package.xml | 18 - .../resource/centroid_yaw_tracker | 0 .../testing/centroid_yaw_tracker/setup.cfg | 4 - .../testing/centroid_yaw_tracker/setup.py | 25 - .../nav_channel/nav_channel/__init__.py | 0 .../nav_channel/nav_channel/nav_channel.py | 268 ------ .../testing/nav_channel/package.xml | 18 - .../testing/nav_channel/resource/nav_channel | 0 src/subjugator/testing/nav_channel/setup.cfg | 4 - src/subjugator/testing/nav_channel/setup.py | 23 - 83 files changed, 5514 deletions(-) delete mode 160000 ext/FTXUI delete mode 100644 src/mil_common/mil_preflight/CMakeLists.txt delete mode 100644 src/mil_common/mil_preflight/cfg/config.json delete mode 100644 src/mil_common/mil_preflight/include/mil_preflight/common.h delete mode 100644 src/mil_common/mil_preflight/include/mil_preflight/job.h delete mode 100644 src/mil_common/mil_preflight/include/mil_preflight/plugin.h delete mode 100644 src/mil_common/mil_preflight/include/mil_preflight/ui.h delete mode 100644 src/mil_common/mil_preflight/include/mil_preflight/uis/ftxui/dialog.h delete mode 100644 src/mil_common/mil_preflight/include/mil_preflight/uis/ftxui/reportsPage.h delete mode 100644 src/mil_common/mil_preflight/include/mil_preflight/uis/ftxui/testsPage.h delete mode 100644 src/mil_common/mil_preflight/package.xml delete mode 100644 src/mil_common/mil_preflight/src/backend.cpp delete mode 100644 src/mil_common/mil_preflight/src/frontend.cpp delete mode 100644 src/mil_common/mil_preflight/src/plugins/actuator_plugin.cpp delete mode 100644 src/mil_common/mil_preflight/src/plugins/node_plugin.cpp delete mode 100644 src/mil_common/mil_preflight/src/plugins/setup_plugin.cpp delete mode 100644 src/mil_common/mil_preflight/src/plugins/topic_plugin.cpp delete mode 100644 src/mil_common/mil_preflight/src/uis/ftxui/ui.cpp delete mode 100644 src/mil_common/mil_preflight/src/uis/ftxui/widgets.cpp delete mode 100644 src/subjugator/gnc/subjugator_centroids/package.xml delete mode 100644 src/subjugator/gnc/subjugator_centroids/random_code/green_tracker_cv2_example.py delete mode 100644 src/subjugator/gnc/subjugator_centroids/resource/subjugator_centroids delete mode 100644 src/subjugator/gnc/subjugator_centroids/setup.cfg delete mode 100644 src/subjugator/gnc/subjugator_centroids/setup.py delete mode 100644 src/subjugator/gnc/subjugator_centroids/subjugator_centroids/__init__.py delete mode 100644 src/subjugator/gnc/subjugator_centroids/subjugator_centroids/centroid_finder.py delete mode 100644 src/subjugator/gnc/subjugator_centroids/subjugator_centroids/green_tracker.py delete mode 100644 src/subjugator/gnc/subjugator_centroids/subjugator_centroids/orange_tracker.py delete mode 100644 src/subjugator/gnc/subjugator_centroids/subjugator_centroids/red_tracker.py delete mode 100644 src/subjugator/gnc/subjugator_centroids/subjugator_centroids/subjugator_centroids_node.py delete mode 100644 src/subjugator/gnc/subjugator_wrench_tuner/config/wrench_tuner_params.yaml delete mode 100644 src/subjugator/gnc/subjugator_wrench_tuner/launch/wrench_tuner_launch.py delete mode 100644 src/subjugator/gnc/subjugator_wrench_tuner/package.xml delete mode 100644 src/subjugator/gnc/subjugator_wrench_tuner/resource/subjugator_wrench_tuner delete mode 100644 src/subjugator/gnc/subjugator_wrench_tuner/setup.cfg delete mode 100644 src/subjugator/gnc/subjugator_wrench_tuner/setup.py delete mode 100644 src/subjugator/gnc/subjugator_wrench_tuner/subjugator_wrench_tuner/__init__.py delete mode 100644 src/subjugator/gnc/subjugator_wrench_tuner/subjugator_wrench_tuner/wrench_tuner.py delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/MANIFEST.in delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/in delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/launch/mission_planner_launch.py delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/launch/task_server_launch.py delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/mechanism_test.yaml delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/mission1_example.yaml delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/move_test.yaml delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/nav_channel_test.yaml delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/navigate_around_test.yaml delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/navigate_channel.yaml delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/prequal.yaml delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/prequal_mission.yaml delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/sonar_follower_test.yaml delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/start_gate.yaml delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/test_all.yaml delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/wait_test.yaml delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/yaw_tracker_test.yaml delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/package.xml delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/resource/subjugator_mission_planner delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.cfg delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.py delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/__init__.py delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mechanisms_server.py delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mission_planner.py delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/nav_channel_server.py delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/navigate_around_server.py delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/navigation_channel_server.py delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/sonar_follower_server.py delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/start_gate_server.py delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/wait_server.py delete mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/yaw_tracker_server.py delete mode 100644 src/subjugator/testing/centroid_yaw_tracker/centroid_yaw_tracker/__init__.py delete mode 100644 src/subjugator/testing/centroid_yaw_tracker/centroid_yaw_tracker/centroid_yaw_tracker_node.py delete mode 100644 src/subjugator/testing/centroid_yaw_tracker/package.xml delete mode 100644 src/subjugator/testing/centroid_yaw_tracker/resource/centroid_yaw_tracker delete mode 100644 src/subjugator/testing/centroid_yaw_tracker/setup.cfg delete mode 100644 src/subjugator/testing/centroid_yaw_tracker/setup.py delete mode 100644 src/subjugator/testing/nav_channel/nav_channel/__init__.py delete mode 100644 src/subjugator/testing/nav_channel/nav_channel/nav_channel.py delete mode 100644 src/subjugator/testing/nav_channel/package.xml delete mode 100644 src/subjugator/testing/nav_channel/resource/nav_channel delete mode 100644 src/subjugator/testing/nav_channel/setup.cfg delete mode 100644 src/subjugator/testing/nav_channel/setup.py diff --git a/.gitignore b/.gitignore index b4a768c4..7ca5406b 100644 --- a/.gitignore +++ b/.gitignore @@ -191,5 +191,4 @@ cython_debug/ #.idea/ bag_files -src/mil_common/perception/vision_stack/ src/subjugator/mission_planner/logs/mission_debug.txt diff --git a/.gitmodules b/.gitmodules index 82e015af..173d1906 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,9 +4,6 @@ [submodule "src/subjugator/drivers/waterlinked_dvl"] path = src/subjugator/drivers/waterlinked_dvl url = https://github.com/uf-mil/waterlinked_dvl.git -[submodule "ext/FTXUI"] - path = ext/FTXUI - url = https://github.com/ArthurSonzogni/FTXUI.git [submodule "ext/au"] path = ext/au url = https://github.com/uf-mil/au diff --git a/ext/FTXUI b/ext/FTXUI deleted file mode 160000 index cdf28903..00000000 --- a/ext/FTXUI +++ /dev/null @@ -1 +0,0 @@ -Subproject commit cdf28903a7781f97ba94d30b79c3a4b0c97ccce7 diff --git a/src/mil_common/mil_preflight/CMakeLists.txt b/src/mil_common/mil_preflight/CMakeLists.txt deleted file mode 100644 index 4e24d415..00000000 --- a/src/mil_common/mil_preflight/CMakeLists.txt +++ /dev/null @@ -1,59 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(mil_preflight) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(subjugator_msgs REQUIRED) -find_package(Boost REQUIRED COMPONENTS thread chrono filesystem system) -find_package(ftxui REQUIRED) - -add_executable(${PROJECT_NAME} src/frontend.cpp) -include_directories(include ${ftxui_SOURCE_DIR}/include ${Boost_INCLUDE_DIRS}) - -target_link_libraries(${PROJECT_NAME} Boost::system Boost::filesystem) - -add_executable(mil_preflight_backend src/backend.cpp) - -ament_target_dependencies(mil_preflight_backend rclcpp std_msgs Boost) - -install(TARGETS ${PROJECT_NAME} mil_preflight_backend - DESTINATION bin # Install to install/bin/ -) - -install(FILES cfg/config.json DESTINATION bin) - -add_library(ftx_ui SHARED src/uis/ftxui/ui.cpp src/uis/ftxui/widgets.cpp) - -target_link_libraries( - ftx_ui - PRIVATE ftxui::screen - PRIVATE ftxui::dom - PRIVATE ftxui::component Boost::thread Boost::chrono Boost::filesystem - Boost::system) - -add_library(topic_plugin SHARED src/plugins/topic_plugin.cpp) - -ament_target_dependencies(topic_plugin rclcpp Boost) - -add_library(setup_plugin SHARED src/plugins/setup_plugin.cpp) - -ament_target_dependencies(setup_plugin rclcpp Boost) - -add_library(node_plugin SHARED src/plugins/node_plugin.cpp) - -ament_target_dependencies(node_plugin rclcpp Boost) - -add_library(actuator_plugin SHARED src/plugins/actuator_plugin.cpp) - -ament_target_dependencies(actuator_plugin rclcpp subjugator_msgs Boost) - -install(TARGETS topic_plugin setup_plugin node_plugin actuator_plugin ftx_ui - LIBRARY DESTINATION lib) - -ament_package() diff --git a/src/mil_common/mil_preflight/cfg/config.json b/src/mil_common/mil_preflight/cfg/config.json deleted file mode 100644 index 36c543f4..00000000 --- a/src/mil_common/mil_preflight/cfg/config.json +++ /dev/null @@ -1,107 +0,0 @@ -{ - "actuators": { - "plugin": "actuator_plugin", - "actions": { - "FLH Thruster": [ - "/thruster_efforts", - "FLH", - "0.2" - ], - "FRH Thruster": [ - "/thruster_efforts", - "FRH", - "0.2" - ], - "BLH Thruster": [ - "/thruster_efforts", - "BLH", - "0.2" - ], - "BRH Thruster": [ - "/thruster_efforts", - "BRH", - "0.2" - ], - "FLV Thruster": [ - "/thruster_efforts", - "FLV", - "0.2" - ], - "FRV Thruster": [ - "/thruster_efforts", - "FRV", - "0.2" - ], - "BLV Thruster": [ - "/thruster_efforts", - "BLV", - "0.2" - ], - "BRV Thruster": [ - "/thruster_efforts", - "BRV", - "0.2" - ] - } - }, - "setup": { - "plugin": "setup_plugin", - "actions": { - "O-rings": [ - "Grease O-rings with Molykote 55 every time a pressure vessel is closed." - ], - "Deploy sub": [ - "Check for bubbles coming out of every pressure vessel, make sure buoyancy is correct" - ] - } - }, - "nodes": { - "plugin": "node_plugin", - "actions": { - "/odom_estimator": [ - "/odom_estimator" - ], - "/transform_odometry": [ - "/transform_odometry" - ], - "/c3_trajectory_generator": [ - "/c3_trajectory_generator" - ], - "/adaptive_controller": [ - "/adaptive_controller" - ], - "/thruster_mapper": [ - "/thruster_mapper" - ], - "/mission_runner": [ - "/mission_runner" - ] - } - }, - "topics": { - "plugin": "topic_plugin", - "actions": { - "/camera/front/right/image_raw": [ - "/camera/front/right/image_raw" - ], - "/camera/down/image_raw": [ - "/camera/down/image_raw" - ], - "/camera/front/left/image_raw": [ - "/camera/front/left/image_raw" - ], - "/dvl": [ - "/dvl" - ], - "/depth": [ - "/depth" - ], - "/imu/data_raw": [ - "/imu/data_raw" - ], - "/imu/mag": [ - "/imu/mag" - ] - } - } -} diff --git a/src/mil_common/mil_preflight/include/mil_preflight/common.h b/src/mil_common/mil_preflight/include/mil_preflight/common.h deleted file mode 100644 index 1b19516b..00000000 --- a/src/mil_common/mil_preflight/include/mil_preflight/common.h +++ /dev/null @@ -1,10 +0,0 @@ -namespace mil_preflight -{ -static constexpr char ETX = 0x03; -static constexpr char EOT = 0x04; -static constexpr char ACK = 0x06; -static constexpr char BEL = 0x07; -static constexpr char NCK = 0x15; -static constexpr char GS = 0x1D; -static constexpr char US = 0x1F; -} // namespace mil_preflight diff --git a/src/mil_common/mil_preflight/include/mil_preflight/job.h b/src/mil_common/mil_preflight/include/mil_preflight/job.h deleted file mode 100644 index 30bc57f8..00000000 --- a/src/mil_common/mil_preflight/include/mil_preflight/job.h +++ /dev/null @@ -1,449 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "mil_preflight/common.h" - -namespace mil_preflight -{ - -class Action -{ - public: - struct Report - { - bool success; - std::string summery; - std::vector stdouts; - std::vector stderrs; - - Report() : success(false) - { - } - ~Report() = default; - Report(Report const& report) = default; - Report(Report&& report) - { - success = report.success; - report.success = false; - summery = std::move(report.summery); - stdouts = std::move(report.stdouts); - stderrs = std::move(report.stderrs); - } - - Report& operator=(Report const& report) - { - success = report.success; - summery = report.summery; - stdouts = report.stdouts; - stderrs = report.stderrs; - - return *this; - } - - Report& operator=(Report&& report) - { - success = report.success; - report.success = false; - summery = std::move(report.summery); - stdouts = std::move(report.stdouts); - stderrs = std::move(report.stderrs); - - return *this; - } - }; - - // class Feedback - // { - // public: - // Feedback() : impl_(std::make_shared()) - // { - // } - - // Feedback(Feedback&& feedback) - // { - // impl_ = std::move(feedback.impl_); - // } - // Feedback(Feedback const& feedback) - // { - // impl_ = feedback.impl_; - // } - // Feedback& operator=(Feedback&& feedback) - // { - // impl_ = std::move(feedback.impl_); - // return *this; - // } - // Feedback& operator=(Feedback const& feedback) - // { - // impl_ = feedback.impl_; - // return *this; - // } - - // ~Feedback() - // { - // } - - // void set(int index) - // { - // std::unique_lock lock(impl_->mutex_); - // if (!impl_->answered_) - // { - // impl_->index_ = index; - // impl_->answered_ = true; - // impl_->cond_.notify_all(); - // } - // } - - // int get() const - // { - // std::unique_lock lock(impl_->mutex_); - // impl_->cond_.wait(lock, [this] { return impl_->answered_; }); - // return impl_->index_; - // } - - // private: - // struct Impl - // { - // int index_ = -1; - // bool answered_ = false; - // std::condition_variable cond_; - // std::mutex mutex_; - // }; - - // std::shared_ptr impl_; - // }; - - Action() {}; - ~Action() {}; - - virtual void onStart() = 0; - virtual void onFinish(Report const& report) = 0; - virtual std::string const& getName() const = 0; - virtual std::vector const& getParameters() const = 0; - virtual std::shared_future onQuestion(std::string&& question, std::vector&& options) = 0; - - protected: - std::vector stdouts_; - std::vector stderrs_; -}; - -class Test -{ - public: - using Report = std::unordered_map; - - Test() {}; - ~Test() {}; - - virtual std::optional> createAction(std::string&& name, - std::vector&& parameters) = 0; - virtual std::optional> nextAction() = 0; - virtual void onFinish(Report const& report) = 0; - virtual std::string const& getName() const = 0; - virtual std::string const& getPlugin() const = 0; -}; - -class Question -{ - public: - Question() - { - } - ~Question() - { - answer(-1); - } - - inline void answer(int index) - { - std::unique_lock lock(mutex_); - if (!answered_) - { - index_ = index; - answered_ = true; - cond_.notify_all(); - } - } - - inline int ask() - { - std::unique_lock lock(mutex_); - cond_.wait(lock, [this] { return answered_; }); - return index_; - } - - private: - int index_ = -1; - bool answered_ = false; - std::condition_variable cond_; - std::mutex mutex_; -}; - -class Job -{ - public: - using Report = std::unordered_map; - - Job() {}; - Job(std::string const& filePath) - { - initialize(filePath); - } - ~Job() {}; - - bool initialize(std::string const& filePath) - { - std::ifstream file(filePath); - if (!file.is_open()) - { - return false; - } - - // Parse the configuration file - boost::property_tree::ptree root; - boost::property_tree::read_json(file, root); - - for (auto& testPair : root) - { - std::string testName = std::move(testPair.first); - boost::property_tree::ptree& testNode = testPair.second; - - std::optional> testOptional = - createTest(std::move(testName), testNode.get("plugin")); - if (!testOptional.has_value()) - continue; - - Test& test = testOptional.value(); - - for (auto& actionPair : testNode.get_child("actions")) - { - std::string actionName = std::move(actionPair.first); - boost::property_tree::ptree& paramsArray = actionPair.second; - - std::vector parameters; - for (auto& param : paramsArray) - { - parameters.push_back(std::move(param.second.get_value())); - } - - test.createAction(std::move(actionName), std::move(parameters)); - } - } - - file.close(); - return true; - } - - void run() - { - if (isRunning()) - return; - - future_ = std::async(std::launch::async, std::bind(&Job::runJob, this)); - } - - void cancel() - { - if (backend_.valid() && backend_.joinable()) - backend_.terminate(); - } - - bool isRunning() - { - if (!future_.valid()) - return false; - - if (future_.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) - return false; - - return true; - } - - protected: - virtual std::optional> createTest(std::string&& name, std::string&& plugin) = 0; - virtual std::optional> nextTest() = 0; - virtual void onFinish(Report&& report) = 0; - - private: - enum class State - { - START, - STDOUT, - SUMMERY, - QUESTION, - OPTIONS, - }; - - std::future future_; - boost::process::child backend_; - std::filesystem::path binPath_ = std::filesystem::canonical("/proc/self/exe").parent_path(); - - void runJob() - { - Job::Report jobReport; - while (true) - { - std::optional> testOptional = nextTest(); - if (!testOptional.has_value()) - break; - - Test& test = testOptional.value(); - - boost::process::ipstream childOut; - boost::process::ipstream childErr; - boost::process::opstream childIn; - std::vector args; - - backend_ = boost::process::child((binPath_ / "mil_preflight_backend").string(), test.getPlugin(), - boost::process::std_in childOut, - boost::process::std_err > childErr); - - std::string line; - - std::string question; - std::vector options; - - Test::Report testReport; - while (true) - { - std::optional> actionOptional = test.nextAction(); - if (!actionOptional.has_value()) - break; - - Action& action = actionOptional.value(); - action.onStart(); - - Action::Report actionReport; - State state = State::START; - - while (true) - { - if (state == State::START) - { - try - { - childIn << action.getName() << std::endl; - for (std::string const& parameter : action.getParameters()) - { - childIn << parameter << std::endl; - } - childIn << GS << std::endl; - } - catch (std::exception const& e) - { - actionReport.summery = "Broken pipe: " + std::string(e.what()); - break; - } - state = State::STDOUT; - } - else if (state == State::STDOUT) - { - if (!std::getline(childOut, line)) - { - actionReport.summery = "Broken pipe"; - break; - } - - if (line[0] == ACK) - { - state = State::SUMMERY; - actionReport.success = true; - } - else if (line[0] == NCK) - { - state = State::SUMMERY; - } - else if (line[0] == BEL) - { - state = State::QUESTION; - } - else - { - actionReport.stdouts.push_back(std::move(line)); - } - } - else if (state == State::QUESTION) - { - if (!std::getline(childOut, question, GS)) - { - actionReport.summery = "Broken pipe"; - break; - } - - state = State::OPTIONS; - } - else if (state == State::OPTIONS) - { - if (!std::getline(childOut, line, GS)) - { - actionReport.summery = "Broken pipe"; - break; - } - - if (line[0] == EOT) - { - std::shared_future feedback = - action.onQuestion(std::move(question), std::move(options)); - try - { - childIn << feedback.get() << std::endl; - options.clear(); - } - catch (std::exception const& e) - { - actionReport.summery = "Broken pipe: " + std::string(e.what()); - break; - } - state = State::STDOUT; - } - else - { - options.push_back(std::move(line)); - } - } - else if (state == State::SUMMERY) - { - if (!std::getline(childOut, actionReport.summery)) - { - break; - } - - while (std::getline(childErr, line)) - { - if (line[0] == EOT) - break; - actionReport.stderrs.push_back(std::move(line)); - } - - break; - } - } - - action.onFinish(actionReport); - testReport.emplace(action.getName(), std::move(actionReport)); - } - - if (!childIn.fail()) - childIn << EOT << std::endl; - - backend_.join(); - test.onFinish(testReport); - jobReport.emplace(test.getName(), std::move(testReport)); - } - - onFinish(std::move(jobReport)); - } -}; -} // namespace mil_preflight diff --git a/src/mil_common/mil_preflight/include/mil_preflight/plugin.h b/src/mil_common/mil_preflight/include/mil_preflight/plugin.h deleted file mode 100644 index 162e2a3d..00000000 --- a/src/mil_common/mil_preflight/include/mil_preflight/plugin.h +++ /dev/null @@ -1,134 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "mil_preflight/common.h" - -namespace mil_preflight -{ -class PluginBase : public rclcpp::Node -{ - public: - PluginBase() : rclcpp::Node("mil_preflight_node"), workThread_(boost::bind(&PluginBase::runTest, this)) - { - } - - virtual ~PluginBase() - { - workThread_.join(); - } - - static std::shared_ptr create(std::string const& pluginName) - { - try - { - creator_ = boost::dll::import_alias(pluginName, pluginName, - boost::dll::load_mode::append_decorations | - boost::dll::load_mode::search_system_folders); - } - catch (boost::system::system_error const& e) - { - error_ = "Failed to load plugin: " + pluginName; - std::shared_ptr plugin = std::make_shared(); - RCLCPP_ERROR(plugin->get_logger(), e.what()); - return plugin; - } - - return creator_(); - } - - protected: - virtual bool runAction([[maybe_unused]] std::vector&& parameters) - { - boost::this_thread::sleep_for(boost::chrono::milliseconds(200)); - return false; - } - - virtual std::string const& getSummery() - { - return error_; - } - - int askQuestion(std::string const& question, std::vector const& options) - { - std::ostringstream oss; - oss << BEL << std::endl << question << GS; - for (std::string const& option : options) - { - oss << option << GS; - } - - oss << EOT << GS; - - std::cout << std::move(oss.str()); - - std::string line; - if (!std::getline(std::cin, line)) - return -1; - - int index = -1; - try - { - index = std::stoi(line); - } - catch (std::exception const& e) - { - } - - return index; - } - - private: - using Creator = std::shared_ptr(); - - boost::thread workThread_; - static std::string error_; - static boost::function creator_; - - void runTest() - { - std::string line; - std::vector parameters; - while (std::getline(std::cin, line)) - { - if (line[0] == EOT) - { - break; - } - else if (line[0] == GS) - { - bool success = runAction(std::move(parameters)); - std::ostringstream stdoutss; - stdoutss << (success ? ACK : NCK) << std::endl; - stdoutss << getSummery() << std::endl; - std::cout << std::move(stdoutss.str()); - - std::ostringstream stderrss; - stderrss << EOT << std::endl; - std::cerr << std::move(stderrss.str()); - - parameters.clear(); - } - else - { - parameters.push_back(std::move(line)); - } - } - - rclcpp::shutdown(); - } -}; - -std::string PluginBase::error_ = "success"; -boost::function PluginBase::creator_; -} // namespace mil_preflight diff --git a/src/mil_common/mil_preflight/include/mil_preflight/ui.h b/src/mil_common/mil_preflight/include/mil_preflight/ui.h deleted file mode 100644 index 1e5e4690..00000000 --- a/src/mil_common/mil_preflight/include/mil_preflight/ui.h +++ /dev/null @@ -1,59 +0,0 @@ -#pragma once - -#include -#include - -#include -#include - -namespace mil_preflight -{ - -class UIBase -{ - public: - UIBase() - { - } - - virtual ~UIBase() - { - } - - virtual int spin() - { - return -1; - } - - virtual void initialize([[maybe_unused]] int argc, [[maybe_unused]] char* argv[]) - { - std::cout << error_ << std::endl; - } - - static std::shared_ptr create(std::string const& uiName) - { - try - { - creator_ = boost::dll::import_alias(uiName, uiName, - boost::dll::load_mode::append_decorations | - boost::dll::load_mode::search_system_folders); - } - catch (boost::system::system_error const& e) - { - error_ = "Failed to load the ui: " + uiName + ": " + e.code().message(); - return std::make_shared(); - } - - return creator_(); - } - - private: - using Creator = std::shared_ptr(); - static boost::function creator_; - static std::string error_; -}; - -std::string UIBase::error_ = "success"; -boost::function UIBase::creator_; - -} // namespace mil_preflight diff --git a/src/mil_common/mil_preflight/include/mil_preflight/uis/ftxui/dialog.h b/src/mil_common/mil_preflight/include/mil_preflight/uis/ftxui/dialog.h deleted file mode 100644 index db779827..00000000 --- a/src/mil_common/mil_preflight/include/mil_preflight/uis/ftxui/dialog.h +++ /dev/null @@ -1,96 +0,0 @@ -#pragma once - -#include -#include - -#include -#include - -namespace mil_preflight -{ -using namespace ftxui; - -class Dialog : public ComponentBase, public std::enable_shared_from_this -{ - public: - struct Option - { - std::string title; - std::string question; - std::vector buttonLabels; - }; - - Dialog(Option const& option) : option_(option), screen_(ScreenInteractive::Fullscreen()) - { - create(); - } - - Dialog(Option&& option) : option_(std::move(option)), screen_(ScreenInteractive::Fullscreen()) - { - create(); - } - - ~Dialog() - { - } - - int show() - { - screen_.Loop(shared_from_this()); - return index_; - } - - Element Render() override - { - return vbox({ hbox({ text(option_.title) | flex, closeButton_->Render() }), separator(), - vbox(paragraphs_) | flex, separator(), buttonsContainer_->Render() | center }) | - border; - } - - private: - Component buttonsContainer_; - Component closeButton_; - Elements paragraphs_; - Option option_; - ScreenInteractive screen_; - int index_ = -1; - - void create() - { - std::istringstream iss(option_.question); - std::string line; - while (std::getline(iss, line)) - { - paragraphs_.push_back(paragraph(line)); - } - - Components buttons; - for (size_t i = 0; i < option_.buttonLabels.size(); i++) - { - buttons.push_back(Button( - option_.buttonLabels[i], - [=] - { - index_ = i; - screen_.Exit(); - }, - ButtonOption::Border())); - } - buttonsContainer_ = Container::Horizontal(buttons); - - closeButton_ = Button( - "X", - [=] - { - index_ = -1; - screen_.Exit(); - }, - ButtonOption::Ascii()); - - Component dialogContainer = Container::Vertical({ closeButton_, buttonsContainer_ }); - - Add(dialogContainer); - } -}; - -} // namespace mil_preflight diff --git a/src/mil_common/mil_preflight/include/mil_preflight/uis/ftxui/reportsPage.h b/src/mil_common/mil_preflight/include/mil_preflight/uis/ftxui/reportsPage.h deleted file mode 100644 index b8ea8a2c..00000000 --- a/src/mil_common/mil_preflight/include/mil_preflight/uis/ftxui/reportsPage.h +++ /dev/null @@ -1,30 +0,0 @@ -#pragma once - -#include -#include - -#include "mil_preflight/job.h" - -namespace mil_preflight -{ -using namespace ftxui; - -class ReportsPage : public ComponentBase -{ - public: - ReportsPage(); - ~ReportsPage(); - - private: - Job::Report report_; - - bool showSuccess_ = true; - Component reportPanel_; - Component bottom_; - int selector_ = 0; - - Element Render() final; - bool OnEvent(Event event) final; -}; - -} // namespace mil_preflight diff --git a/src/mil_common/mil_preflight/include/mil_preflight/uis/ftxui/testsPage.h b/src/mil_common/mil_preflight/include/mil_preflight/uis/ftxui/testsPage.h deleted file mode 100644 index bc508d5b..00000000 --- a/src/mil_common/mil_preflight/include/mil_preflight/uis/ftxui/testsPage.h +++ /dev/null @@ -1,46 +0,0 @@ -#pragma once - -#include -#include -#include - -#include -#include - -#include "mil_preflight/job.h" - -namespace mil_preflight -{ -using namespace ftxui; - -class TestsPage : public ComponentBase, public Job -{ - public: - TestsPage(std::string const& filePath); - ~TestsPage(); - - private: - Component tabsContainer_; - Component pagesContainer_; - Component main_; - - int selector_ = 0; - std::vector actionSelectors_; - bool selectAll_ = false; - size_t nSelected_ = 0; - int mainSize_ = 20; - - std::atomic currentTest_ = 0; - std::atomic running_ = false; - - std::string const buttonLabels_[4] = { " Run ", " · ", " · ", " · " }; - size_t ticker_ = 0; - - std::optional> nextTest() final; - std::optional> createTest(std::string&& name, std::string&& plugin) final; - void onFinish(Job::Report&& report) final; - - bool OnEvent(Event event) final; -}; - -} // namespace mil_preflight diff --git a/src/mil_common/mil_preflight/package.xml b/src/mil_common/mil_preflight/package.xml deleted file mode 100644 index 3857ca86..00000000 --- a/src/mil_common/mil_preflight/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - mil_preflight - 0.1.3 - Preflight is an automated testing tool that should be run after turning on and connecting to the robot to run a prelaunch hardware checklist and automated software checklist. - zhongzheng - MIL - - ament_cmake - - ftxui - rclcpp - std_msgs - subjugator_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/mil_common/mil_preflight/src/backend.cpp b/src/mil_common/mil_preflight/src/backend.cpp deleted file mode 100644 index e3bbb58a..00000000 --- a/src/mil_common/mil_preflight/src/backend.cpp +++ /dev/null @@ -1,21 +0,0 @@ -#include - -#include -#include -#include - -#include "mil_preflight/plugin.h" - -int main(int argc, char* argv[]) -{ - if (argc <= 1) - return 1; - - rclcpp::init(argc - 1, argv); - std::shared_ptr node = mil_preflight::PluginBase::create(argv[argc - 1]); - - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/src/mil_common/mil_preflight/src/frontend.cpp b/src/mil_common/mil_preflight/src/frontend.cpp deleted file mode 100644 index 5024b098..00000000 --- a/src/mil_common/mil_preflight/src/frontend.cpp +++ /dev/null @@ -1,17 +0,0 @@ - -#include "mil_preflight/ui.h" - -int main(int argc, char* argv[]) -{ - if (argc <= 1) - { - std::cout << "Usage: mil_preflight [ui_parameters...]" << std::endl; - return -1; - } - - std::shared_ptr ui = mil_preflight::UIBase::create(argv[1]); - - ui->initialize(argc - 1, &argv[1]); - - return ui->spin(); -} diff --git a/src/mil_common/mil_preflight/src/plugins/actuator_plugin.cpp b/src/mil_common/mil_preflight/src/plugins/actuator_plugin.cpp deleted file mode 100644 index 21f750b2..00000000 --- a/src/mil_common/mil_preflight/src/plugins/actuator_plugin.cpp +++ /dev/null @@ -1,137 +0,0 @@ -#include -#include -#include - -#include - -#include "mil_preflight/plugin.h" -#include "subjugator_msgs/msg/thruster_efforts.hpp" - -namespace mil_preflight -{ -class ActuatorPlugin : public PluginBase -{ - public: - ActuatorPlugin() - { - } - - ~ActuatorPlugin() - { - } - - static std::shared_ptr create() - { - return std::shared_ptr(new ActuatorPlugin()); - } - - private: - // std::vector nodes_; - std::string summery_; - - subjugator_msgs::msg::ThrusterEfforts assignThrust(subjugator_msgs::msg::ThrusterEfforts cmd, std::string thruster, - float thrust) - { - if (thruster == "FLH") - cmd.thrust_flh = thrust; - - else if (thruster == "FRH") - cmd.thrust_frh = thrust; - - else if (thruster == "BLH") - cmd.thrust_blh = thrust; - - else if (thruster == "BRH") - cmd.thrust_brh = thrust; - - else if (thruster == "FLV") - cmd.thrust_flv = thrust; - - else if (thruster == "FRV") - cmd.thrust_frv = thrust; - - else if (thruster == "BLV") - cmd.thrust_blv = thrust; - - else if (thruster == "BRV") - cmd.thrust_brv = thrust; - - return cmd; - } - - bool runAction(std::vector&& parameters) final - { - std::string question = - "Ensure that all fingers are clear of the area!\nIs it safe to operate the actuator: " + parameters[0] + - " ?"; - - if (askQuestion(question, { "Yes", "No" }) != 0) - { - summery_ = "User did not clear the area"; - return false; - } - - std::vector infos = get_subscriptions_info_by_topic(parameters[1]); - if (infos.size() == 0) - { - summery_ = "No subscriber subscribing to the topic: " + parameters[1]; - return false; - } - - std::string& topicType = infos[0].topic_type(); - if (topicType != "subjugator_msgs/msg/ThrusterEfforts") - { - summery_ = "Mismatched message type : " + infos[0].topic_type(); - return false; - } - - subjugator_msgs::msg::ThrusterEfforts cmd; - std::string thruster = parameters[2]; - float thrust = std::stof(parameters[3]); - - try - { - cmd = this->assignThrust(cmd, thruster, thrust); - } - catch (std::exception const& e) - { - summery_ = "Invalid thrust parameter: " + parameters[3]; - return false; - } - - rclcpp::Publisher::SharedPtr pub = - create_publisher(parameters[1], 10); - - auto start_time = std::chrono::steady_clock::now(); - std::chrono::seconds timeout(2); // TODO: Make into a variable - int thruster_did_spin = 0; - - while (std::chrono::steady_clock::now() - start_time < timeout) - { - pub->publish(cmd); - } - - cmd = this->assignThrust(cmd, thruster, 0); - - pub->publish(cmd); - - thruster_did_spin = askQuestion("Did the " + parameters[2] + " thruster spin?", { "Yes", "No" }) == 0; - - if (!thruster_did_spin) - { - summery_ = "User said the thruster didn't spin"; - return false; - } - - summery_ = "success"; - return true; - } - - std::string const& getSummery() final - { - return summery_; - } -}; -} // namespace mil_preflight - -BOOST_DLL_ALIAS(mil_preflight::ActuatorPlugin::create, actuator_plugin); diff --git a/src/mil_common/mil_preflight/src/plugins/node_plugin.cpp b/src/mil_common/mil_preflight/src/plugins/node_plugin.cpp deleted file mode 100644 index a623f055..00000000 --- a/src/mil_common/mil_preflight/src/plugins/node_plugin.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include -#include - -#include - -#include "mil_preflight/plugin.h" - -namespace mil_preflight -{ -class NodePlugin : public PluginBase -{ - public: - NodePlugin() - { - nodes_ = get_node_names(); - } - - ~NodePlugin() - { - } - - static std::shared_ptr create() - { - return std::shared_ptr(new NodePlugin()); - } - - private: - std::vector nodes_; - std::string summery_; - - bool runAction(std::vector&& parameters) final - { - if (std::find(nodes_.begin(), nodes_.end(), parameters[1]) == nodes_.end()) - { - nodes_ = get_node_names(); - if (std::find(nodes_.begin(), nodes_.end(), parameters[1]) == nodes_.end()) - { - summery_ = "Node " + parameters[0] + " does not exist in 100 ms"; - return false; - } - } - - summery_ = "Found node " + parameters[1]; - - return true; - } - - std::string const& getSummery() final - { - return summery_; - } -}; -} // namespace mil_preflight - -BOOST_DLL_ALIAS(mil_preflight::NodePlugin::create, node_plugin); diff --git a/src/mil_common/mil_preflight/src/plugins/setup_plugin.cpp b/src/mil_common/mil_preflight/src/plugins/setup_plugin.cpp deleted file mode 100644 index 220eade2..00000000 --- a/src/mil_common/mil_preflight/src/plugins/setup_plugin.cpp +++ /dev/null @@ -1,46 +0,0 @@ -#include - -#include "mil_preflight/plugin.h" - -namespace mil_preflight -{ -class SetupPlugin : public PluginBase -{ - public: - SetupPlugin() - { - } - - ~SetupPlugin() - { - } - - static std::shared_ptr create() - { - return std::shared_ptr(new SetupPlugin()); - } - - private: - std::map> topics_; - std::string summery_; - - bool runAction(std::vector&& parameters) final - { - if (askQuestion(parameters[1], { "Yes", "No" }) != 0) - { - summery_ = "User said No"; - return false; - } - - summery_ = "success"; - return true; - } - - std::string const& getSummery() final - { - return summery_; - } -}; -} // namespace mil_preflight - -BOOST_DLL_ALIAS(mil_preflight::SetupPlugin::create, setup_plugin); diff --git a/src/mil_common/mil_preflight/src/plugins/topic_plugin.cpp b/src/mil_common/mil_preflight/src/plugins/topic_plugin.cpp deleted file mode 100644 index 4840ad68..00000000 --- a/src/mil_common/mil_preflight/src/plugins/topic_plugin.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#include - -#include - -#include "mil_preflight/plugin.h" - -namespace mil_preflight -{ -class TopicPlugin : public PluginBase -{ - public: - TopicPlugin() - { - topics_ = get_topic_names_and_types(); - } - - ~TopicPlugin() - { - } - - static std::shared_ptr create() - { - return std::shared_ptr(new TopicPlugin()); - } - - private: - std::map> topics_; - std::string summery_; - bool runAction(std::vector&& parameters) final - { - auto it = topics_.find(parameters[1]); - if (it == topics_.end()) - { - boost::this_thread::sleep_for(boost::chrono::milliseconds(100)); - topics_ = get_topic_names_and_types(); - - it = topics_.find(parameters[1]); - if (it == topics_.end()) - { - summery_ = "Topic " + parameters[0] + " does not exist in 100 ms"; - return false; - } - } - - summery_ = "Found topic " + parameters[0] + " with type " + it->second[0]; - return true; - } - - std::string const& getSummery() final - { - return summery_; - } -}; -} // namespace mil_preflight - -BOOST_DLL_ALIAS(mil_preflight::TopicPlugin::create, topic_plugin); diff --git a/src/mil_common/mil_preflight/src/uis/ftxui/ui.cpp b/src/mil_common/mil_preflight/src/uis/ftxui/ui.cpp deleted file mode 100644 index 259a655d..00000000 --- a/src/mil_common/mil_preflight/src/uis/ftxui/ui.cpp +++ /dev/null @@ -1,148 +0,0 @@ -#include "mil_preflight/ui.h" - -#include -#include -#include -#include - -#include "mil_preflight/uis/ftxui/reportsPage.h" -#include "mil_preflight/uis/ftxui/testsPage.h" - -using namespace ftxui; -auto screen = ScreenInteractive::Fullscreen(); - -namespace mil_preflight -{ -class Root : public ComponentBase -{ - public: - Root() - { - Component back_button = Button( - "<", [this] { current_page = 0; }, ButtonOption::Ascii()) | - Maybe([this] { return current_page != 0; }); - Component head = - Renderer(back_button, - [this, back_button] - { - return hbox({ back_button->Render(), text(titles[current_page]) | bold | center | - flex }); // Back button & title in same line - }); - - body = Container::Tab({}, ¤t_page); - menu = Container::Vertical({}); - body->Add(menu | center); - Add(Container::Vertical({ head, Renderer([] { return separator(); }), body | flex })); - } - ~Root() - { - } - - void add_page(std::string const menu_entry, Component page, std::string const& title) - { - ButtonOption menu_option = ButtonOption::Ascii(); - menu_option.transform = [](EntryState const& s) - { - std::string const t = s.active ? "> " + s.label + " ⋯" // - : - " " + s.label + " ⋯"; - return text(t) | (s.focused ? bold : nothing); - }; - - int index = body->ChildCount(); - menu->Add(Button(menu_entry, [this, index] { current_page = index; }, menu_option)); - body->Add(page); - titles.push_back(title); - } - - void add_callback(std::string const menu_entry, std::function action) - { - ButtonOption menu_option = ButtonOption::Ascii(); - menu_option.transform = [](EntryState const& s) - { - std::string const t = s.active ? "> " + s.label // - : - " " + s.label; - return text(t) | (s.focused ? bold : nothing); - }; - - menu->Add(Button(menu_entry, action, menu_option)); - } - - private: - int current_page = 0; - std::vector titles = { "MIL Preflight" }; - Component body; - Component menu; -}; - -class FTXUI : public UIBase -{ - public: - FTXUI() - { - } - - void initialize(int argc, char* argv[]) final - { - std::string filename; - if (argc > 1) - { - filename = argv[1]; - } - else - { - boost::filesystem::path filepath = boost::process::search_path("mil_preflight"); - filename = (filepath.parent_path() / "config.json").string(); - } - - root = std::make_shared(); - - Component tests_page = std::make_shared(filename) | flex; - Component reports_page = std::make_shared() | flex; - - Component about_page = Renderer( - [this] - { - return vbox({ text("MIL Preflight") | bold, text("v0.1.3"), text("University of Florida"), - hbox({ text("Machine Intelligence Laboratory") | hyperlink("https://mil.ufl.edu/") }), - hbox({ text("Powered by "), text("FTXUI") | hyperlink("https://github.com/ArthurSonzogni/" - "FTXUI/") }), - separatorEmpty(), - paragraph("MIL Preflight is a tool inspired by the preflight checklists used by " - "pilots before flying a plane."), - paragraph("This program is designed to verify the functionality of all software and " - "hardware systems on your autonomous robot."), - paragraph("It ensures that everything is in working order, allowing you to safely " - "deploy your robot with confidence.") }) | - flex; - }); - - root->add_page("Run Tests", tests_page, "Tests"); - root->add_page("View Reports", reports_page, "Reports"); - root->add_page("About", about_page, "About"); - root->add_callback("Quit", [this] { screen.Exit(); }); - } - - ~FTXUI() final - { - } - - int spin() final - { - screen.Loop(root); - return 0; - } - - static std::shared_ptr create() - { - return std::make_shared(); - } - - private: - std::shared_ptr root; -}; - -} // namespace mil_preflight - -BOOST_DLL_ALIAS(mil_preflight::FTXUI::create, ftx_ui); diff --git a/src/mil_common/mil_preflight/src/uis/ftxui/widgets.cpp b/src/mil_common/mil_preflight/src/uis/ftxui/widgets.cpp deleted file mode 100644 index 1520112b..00000000 --- a/src/mil_common/mil_preflight/src/uis/ftxui/widgets.cpp +++ /dev/null @@ -1,811 +0,0 @@ -#include -#include -#include -#include - -#include -#include - -#include "mil_preflight/uis/ftxui/dialog.h" -#include "mil_preflight/uis/ftxui/reportsPage.h" -#include "mil_preflight/uis/ftxui/testsPage.h" - -using namespace ftxui; -extern ScreenInteractive screen; - -namespace mil_preflight -{ - -static std::queue reportQueue; - -static std::queue> questionQueue; - -struct ActionBoxOption -{ - std::string name; - std::vector parameters; - std::function onChange; - std::function transform; -}; - -class ActionBox : public ComponentBase, public Action -{ - public: - ActionBox(ActionBoxOption&& option); - ~ActionBox(); - - bool isChecked() const - { - return option_.transform(checked_); - } - void check() - { - checked_ = true; - } - void uncheck() - { - checked_ = false; - } - void reset() - { - state_ = State::NONE; - } - - std::string const& getName() const final - { - return option_.name; - } - std::vector const& getParameters() const final - { - return option_.parameters; - } - - private: - enum class State - { - NONE, - RUNNING, - SUCCESS, - FAILED - }; - - bool checked_ = false; - bool hovered_ = false; - Box box_; - std::atomic state_ = State::NONE; - ActionBoxOption option_; - - Element Render() final; - bool OnEvent(Event event) final; - inline bool OnMouseEvent(Event event); - bool Focusable() const final; - - void onStart() final; - void onFinish(Action::Report const& report) final; - std::shared_future onQuestion(std::string&& question, std::vector&& options) final; -}; - -ActionBox::ActionBox(ActionBoxOption&& option) : option_(std::move(option)) -{ -} - -ActionBox::~ActionBox() -{ -} - -Element ActionBox::Render() -{ - bool focused = Focused(); - // bool active = Active(); - - char const* indicator; - Color textColor = Color::White; - switch (state_) - { - case State::RUNNING: - indicator = " ▶ "; - textColor = Color::White; - break; - case State::SUCCESS: - indicator = " ✔ "; - textColor = Color::Green; - break; - case State::FAILED: - indicator = " ✘ "; - textColor = Color::Red; - break; - default: - indicator = " "; - textColor = Color::White; - break; - } - - auto labelEle = text(option_.name); - - if (focused || hovered_) - { - labelEle |= inverted; - } - - if (focused) - { - labelEle |= bold; - } - - bool checked = option_.transform(checked_); - - auto element = hbox( - { text(checked ? "☑ " : "☐ "), labelEle, filler() | flex, text(indicator) | color(textColor) | align_right }); - - return element | (focused ? focus : nothing) | reflect(box_); -} - -bool ActionBox::OnEvent(Event event) -{ - if (!CaptureMouse(event)) - { - return false; - } - - if (event.is_mouse()) - { - return OnMouseEvent(event); - } - - hovered_ = false; - if (event == Event::Character(' ') || event == Event::Return) - { - checked_ = !checked_; - option_.onChange(checked_); - TakeFocus(); - return true; - } - - return false; -} - -bool ActionBox::OnMouseEvent(Event event) -{ - hovered_ = box_.Contain(event.mouse().x, event.mouse().y); - - if (!CaptureMouse(event)) - { - return false; - } - - if (!hovered_) - { - return false; - } - - if (event.mouse().button == Mouse::Left && event.mouse().motion == Mouse::Pressed) - { - if (Focused()) - { - checked_ = !checked_; - option_.onChange(checked_); - } - else - TakeFocus(); - return true; - } - - return false; -} - -bool ActionBox::Focusable() const -{ - return true; -} - -void ActionBox::onStart() -{ - state_ = State::RUNNING; -} - -void ActionBox::onFinish(Action::Report const& report) -{ - state_ = report.success ? State::SUCCESS : State::FAILED; - screen.PostEvent(Event::Character("ActionFinish")); -} - -std::shared_future ActionBox::onQuestion(std::string&& question, std::vector&& options) -{ - std::shared_ptr> feedback = std::make_shared>(); - - Dialog::Option option; - option.title = "Question for action " + option_.name; - option.question = std::move(question); - option.buttonLabels = std::move(options); - - std::shared_ptr dialog = std::make_shared(std::move(option)); - screen.Post( - [=] - { - int index = dialog->show(); - feedback->set_value(index); - }); - - return feedback->get_future().share(); -} - -struct TestTabOption -{ - std::string name; - std::string plugin; - std::function onChange; - std::function transform; - Component childContainer; -}; - -class TestTab : public ComponentBase, public Test -{ - public: - TestTab(TestTabOption&& option) : option_(std::move(option)) - { - } - ~TestTab() - { - } - - bool isChecked() - { - return option_.transform(checked_) || nChecked_ > 0; - } - - bool transform(bool checked) - { - return checked || option_.transform(checked_); - } - - virtual std::string const& getPlugin() const - { - return option_.plugin; - } - virtual std::string const& getName() const - { - return option_.name; - } - - std::optional> nextAction() final; - std::optional> createAction(std::string&& name, - std::vector&& parameters) final; - void onFinish(Test::Report const& report) final; - - private: - bool hovered_ = false; - bool toggle_ = false; - - size_t nChecked_ = 0; - size_t currentAction_ = 0; - bool checked_ = false; - - TestTabOption option_; - - Box box_; - - Element Render() final; - bool OnEvent(Event event) final; - bool OnMouseEvent(Event event); - - bool Focusable() const final - { - return true; - } -}; - -std::optional> TestTab::nextAction() -{ - Component child = option_.childContainer; - while (currentAction_ < child->ChildCount()) - { - std::shared_ptr action = std::dynamic_pointer_cast(child->ChildAt(currentAction_++)); - child->SetActiveChild(action); - if (action->isChecked()) - return *action; - - action->reset(); - } - - return std::nullopt; -} - -std::optional> TestTab::createAction(std::string&& name, - std::vector&& parameters) -{ - ActionBoxOption option; - option.name = std::move(name); - option.parameters = std::move(parameters); - option.onChange = [&](bool checked) - { - if (checked) - nChecked_++; - else - nChecked_--; - }; - - option.transform = [&](bool checked) { return checked || option_.transform(checked_); }; - - std::shared_ptr action = std::make_shared(std::move(option)); - option_.childContainer->Add(action); - return *action; -} - -void TestTab::onFinish([[maybe_unused]] Test::Report const& report) -{ - currentAction_ = 0; - screen.PostEvent(Event::Character("TestFinish")); -} - -Element TestTab::Render() -{ - bool focused = Focused(); - - auto labelEle = text(option_.name); - - if (focused || hovered_) - { - labelEle |= inverted; - } - - if (focused) - { - labelEle |= bold; - } - - char const* prefix; - bool checked = option_.transform(checked_); - if (checked || nChecked_ == option_.childContainer->ChildCount()) - { - prefix = "☑ "; - } - else if (nChecked_ == 0) - { - prefix = "☐ "; - } - else - { - prefix = "▣ "; - } - - auto element = hbox({ text(prefix), labelEle }); - - return element | (focused ? focus : nothing) | reflect(box_); -} - -bool TestTab::OnEvent(Event event) -{ - if (!CaptureMouse(event)) - { - return false; - } - - if (event.is_mouse()) - { - return OnMouseEvent(event); - } - - hovered_ = false; - if (event == Event::Character(' ') || event == Event::Return) - { - checked_ = !checked_; - option_.onChange(checked_); - TakeFocus(); - return true; - } - - return false; -} - -bool TestTab::OnMouseEvent(Event event) -{ - hovered_ = box_.Contain(event.mouse().x, event.mouse().y); - - if (!CaptureMouse(event)) - { - return false; - } - - if (!hovered_) - { - return false; - } - - if (event.mouse().button == Mouse::Left && event.mouse().motion == Mouse::Pressed) - { - if (Focused()) - { - checked_ = !checked_; - option_.onChange(checked_); - } - else - TakeFocus(); - return true; - } - - return false; -} - -TestsPage::TestsPage(std::string const& filePath) -{ - Components pages; - Components tabs; - - tabsContainer_ = Container::Vertical(tabs, &selector_); - pagesContainer_ = Container::Tab(pages, &selector_); - main_ = ResizableSplitLeft(tabsContainer_ | vscroll_indicator | frame, pagesContainer_ | vscroll_indicator | frame, - &mainSize_); - - ButtonOption buttonOption = ButtonOption::Simple(); - buttonOption.transform = [&](EntryState const& s) - { - auto element = (running_ ? text(buttonLabels_[ticker_ % 3 + 1]) : text(s.label)) | border; - if (s.active) - { - element |= bold; - } - if (s.focused) - { - element |= inverted; - } - return element; - }; - - Component runButton = Button( - buttonLabels_[0], - [=] - { - main_->TakeFocus(); - run(); - }, - buttonOption); - - CheckboxOption option = CheckboxOption::Simple(); - option.transform = [&](EntryState const& s) - { - char const* prefix; - - if (s.state || nSelected_ == pagesContainer_->ChildCount()) - { - prefix = "☑ "; - } - else if (nSelected_ == 0) - { - prefix = "☐ "; - } - else - { - prefix = "▣ "; - } - - auto t = text(s.label); - if (s.active) - { - t |= bold; - } - if (s.focused) - { - t |= inverted; - } - return hbox({ text(prefix), t }); - }; - Component checkBox = Checkbox("Select all", &selectAll_, option); - Component bottom = Container::Horizontal({ checkBox | vcenter | flex, runButton }); - Add(Container::Vertical({ main_ | flex, Renderer([] { return separator(); }), bottom })); - - if (!initialize(filePath)) - { - Dialog::Option option; - option.buttonLabels = { "Ok" }; - option.title = "Error"; - option.question = "Failed to read the config file: " + filePath; - std::shared_ptr dialog = std::make_shared(std::move(option)); - dialog->show(); - } -} - -TestsPage::~TestsPage() -{ - if (running_) - cancel(); -} - -std::optional> TestsPage::nextTest() -{ - running_ = true; - - while (currentTest_ < tabsContainer_->ChildCount()) - { - std::shared_ptr test = std::dynamic_pointer_cast(tabsContainer_->ChildAt(currentTest_++)); - if (test->isChecked()) - { - tabsContainer_->SetActiveChild(tabsContainer_->ChildAt(currentTest_ - 1)); - return *test; - } - } - return std::nullopt; -} - -std::optional> TestsPage::createTest(std::string&& name, std::string&& plugin) -{ - actionSelectors_.push_back(0); - Component list = Container::Vertical({}, &actionSelectors_.back()); - - TestTabOption option; - option.name = std::move(name); - option.plugin = std::move(plugin); - option.transform = [&](bool checked) { return checked || selectAll_; }; - option.onChange = [&](bool checked) - { - if (checked) - nSelected_++; - else - nSelected_--; - }; - option.childContainer = list; - - std::shared_ptr tab = std::make_shared(std::move(option)); - - tabsContainer_->Add(tab); - pagesContainer_->Add(list); - - return *tab; -} - -void TestsPage::onFinish(Job::Report&& report) -{ - running_ = false; - currentTest_ = 0; - reportQueue.push(std::move(report)); - screen.PostEvent(Event::Character("JobFinish")); -} - -bool TestsPage::OnEvent(Event event) -{ - if (event == Event::Character("JobFinish")) - { - return false; - } - - if (event == Event::Character("TestFinish")) - return false; - - if (event == Event::Character("ActionFinish")) - { - ticker_++; - return false; - } - - if (running_ && main_->Focused()) - return false; - - return ComponentBase::OnEvent(event); -} - -class ActionReportPanel : public ComponentBase -{ - public: - ActionReportPanel(Action::Report&& report) : report_(std::move(report)) - { - for (std::string const& line : report_.stdouts) - { - stdouts_.push_back(paragraph(line)); - } - - for (std::string const& line : report_.stderrs) - { - stderrs_.push_back(paragraph(line)); - } - - Component stdoutsRenderer = Renderer([=] { return vbox(stdouts_); }); - - Component stdoutsCollap = Collapsible("stdout", stdoutsRenderer); - - Component stderrsRenderer = Renderer([=] { return vbox(stderrs_); }); - - Component stderrsCollap = Collapsible("stderr", stderrsRenderer); - - Add(Container::Vertical( - { Renderer([&] { return paragraph(report_.summery); }), stdoutsCollap, stderrsCollap })); - } - ~ActionReportPanel() - { - } - - private: - Action::Report&& report_; - Elements summeries_; - Elements stdouts_; - Elements stderrs_; -}; - -class TestReportPanel : public ComponentBase -{ - public: - TestReportPanel(std::string const& name, Test::Report&& report, bool* errorOnly) : errorOnly_(errorOnly) - { - Component panelsContainer = Container::Tab({}, &selector_); - Component tabsContainer = Container::Vertical({}, &selector_); - int errorCount = 0; - for (auto& pair : report) - { - ButtonOption option = ButtonOption::Simple(); - option.transform = option.transform = [success = pair.second.success](EntryState const& s) - { - Element element = text(s.label) | color(success ? Color::Green : Color::Red); - if (s.focused) - element |= inverted; - if (s.active) - element |= bold; - return element; - }; - - Component button = Button(pair.first, [] {}, option); - if (pair.second.success) - button = Maybe(button, [=] { return !(*errorOnly_); }); - tabsContainer->Add(button); - names_.push_back(pair.first); - panelsContainer->Add(std::make_shared(std::move(pair.second))); - - if (!pair.second.success) - errorCount++; - } - - tab_ = Collapsible(name, Renderer(tabsContainer, [=] { return hbox({ text(" "), tabsContainer->Render() }); }), - &show_); - - if (errorCount == 0) - tab_ = Maybe(tab_, [=] { return !(*errorOnly_); }); - - Add(panelsContainer); - } - ~TestReportPanel() - { - } - - Component getTab() - { - return tab_; - } - - bool isShown() - { - return show_; - } - - private: - int selector_ = 0; - bool show_ = false; - std::vector names_; - Component tab_; - bool* errorOnly_; -}; - -class JobReportPanel : public ComponentBase -{ - public: - JobReportPanel(Job::Report&& report, bool* errorOnly) : report_(std::move(report)), errorOnly_(errorOnly) - { - } - - ~JobReportPanel() - { - } - - Element Render() final - { - if (!rendered_) - { - left_ = Container::Vertical({}, &selector_); - right_ = Container::Tab({}, &selector_); - for (auto& pair : report_) - { - std::shared_ptr panel = - std::make_shared(pair.first, std::move(pair.second), errorOnly_); - left_->Add(panel->getTab()); - right_->Add(panel); - } - - Component maybe = Maybe(right_, - [=] - { - auto panel = - std::dynamic_pointer_cast(right_->ChildAt(selector_)); - return panel->isShown(); - }); - - Add(ResizableSplitLeft(left_ | vscroll_indicator | frame, maybe | flex | vscroll_indicator | yframe, - &mainSize_)); - - rendered_ = true; - } - - return ChildAt(0)->Render(); - } - - private: - Job::Report report_; - Component left_; - Component right_; - int selector_ = 0; - bool rendered_ = false; - bool* errorOnly_; - int mainSize_ = 20; -}; - -ReportsPage::ReportsPage() -{ - reportPanel_ = Container::Tab({}, &selector_); - Component clearButton = Button( - "Delete", - [=] - { - if (reportPanel_->ChildCount() > 0) - { - reportPanel_->ChildAt(selector_)->Detach(); - selector_ = std::max(selector_ - 1, 0); - } - }, - ButtonOption::Border()); - Component bottomMiddle = Container::Horizontal({ - Button( - "<", [=] { selector_ = std::min(selector_ + 1, static_cast(reportPanel_->ChildCount() - 1)); }, - ButtonOption::Ascii()) | - vcenter, - Renderer( - [=] - { - return text(std::to_string(reportPanel_->ChildCount() - selector_) + "/" + - std::to_string(reportPanel_->ChildCount())); - }) | - vcenter, - Button( - ">", [=] { selector_ = std::max(selector_ - 1, 0); }, ButtonOption::Ascii()) | - vcenter, - }); - bottom_ = - Container::Horizontal({ Checkbox("Errors only", &showSuccess_) | vcenter, Renderer([] { return filler(); }), - bottomMiddle, Renderer([] { return filler(); }), clearButton | align_right }); - - Add(Container::Vertical({ reportPanel_, bottom_ })); -} - -ReportsPage::~ReportsPage() -{ -} - -bool ReportsPage::OnEvent(Event event) -{ - if (reportPanel_->ChildCount() == 0) - return false; - - return ComponentBase::OnEvent(event); -} - -Element ReportsPage::Render() -{ - while (reportQueue.size() > 0) - { - report_ = std::move(reportQueue.front()); - - if (report_.size() != 0) - { - reportPanel_->Add(std::make_shared(std::move(report_), &showSuccess_)); - if (reportPanel_->ChildCount() > 1) - selector_ += 1; - } - - reportQueue.pop(); - } - - if (reportPanel_->ChildCount() > 0) - return vbox({ - reportPanel_->Render() | flex, - separator(), - bottom_->Render(), - }); - - return text("No report available, please run some tests first.") | center; -} - -} // namespace mil_preflight diff --git a/src/subjugator/gnc/subjugator_centroids/package.xml b/src/subjugator/gnc/subjugator_centroids/package.xml deleted file mode 100644 index e8a9a11a..00000000 --- a/src/subjugator/gnc/subjugator_centroids/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - subjugator_centroids - 0.0.0 - TODO: Package description - sub9 - TODO: License declaration - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/src/subjugator/gnc/subjugator_centroids/random_code/green_tracker_cv2_example.py b/src/subjugator/gnc/subjugator_centroids/random_code/green_tracker_cv2_example.py deleted file mode 100644 index d1737700..00000000 --- a/src/subjugator/gnc/subjugator_centroids/random_code/green_tracker_cv2_example.py +++ /dev/null @@ -1,91 +0,0 @@ -# this file isn't used and it's just an example of how to use cv2 to find centroids - -import cv2 -import numpy as np - - -def get_lime_green_centroid(image): - """ - Detect lime green areas in the image and return the centroid (x, y). - Returns None if no lime green region is detected. - """ - # Convert BGR to HSV - hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) - - # Define HSV range for lime green - lower_green = np.array([40, 100, 100]) - upper_green = np.array([80, 255, 255]) - - # Threshold the HSV image to get only green colors - mask = cv2.inRange(hsv, lower_green, upper_green) - - # Find contours in the mask - contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - if not contours: - return None, mask - - # Find the largest contour - largest = max(contours, key=cv2.contourArea) - - # Compute centroid - M = cv2.moments(largest) - if M["m00"] == 0: - return None, mask - - cx = int(M["m10"] / M["m00"]) - cy = int(M["m01"] / M["m00"]) - return (cx, cy), mask - - -def display_debug_image(image, mask, centroid): - """ - Displays the original image, the binary mask, and overlays the centroid (if found). - """ - # Clone the original image - display_img = image.copy() - - # Overlay centroid if it exists - if centroid: - cv2.circle(display_img, centroid, 5, (0, 0, 255), -1) - cv2.putText( - display_img, - f"Centroid: {centroid}", - (centroid[0] + 10, centroid[1]), - cv2.FONT_HERSHEY_SIMPLEX, - 0.5, - (0, 0, 255), - 2, - ) - - # Show images - cv2.imshow("Lime Green Detection", display_img) - cv2.imshow("Mask", mask) - - -def main(): - cam_path = "/dev/v4l/by-id/usb-Chicony_Tech._Inc._Dell_Webcam_WB7022_4962D17A78D6-video-index0" - cap = cv2.VideoCapture(cam_path) - - if not cap.isOpened(): - print("Cannot open camera") - return - - while True: - ret, frame = cap.read() - if not ret: - print("Failed to grab frame") - break - - centroid, mask = get_lime_green_centroid(frame) - display_debug_image(frame, mask, centroid) - - if cv2.waitKey(1) & 0xFF == ord("q"): - break - - cap.release() - cv2.destroyAllWindows() - - -if __name__ == "__main__": - main() diff --git a/src/subjugator/gnc/subjugator_centroids/resource/subjugator_centroids b/src/subjugator/gnc/subjugator_centroids/resource/subjugator_centroids deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subjugator/gnc/subjugator_centroids/setup.cfg b/src/subjugator/gnc/subjugator_centroids/setup.cfg deleted file mode 100644 index 5df330d4..00000000 --- a/src/subjugator/gnc/subjugator_centroids/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/subjugator_centroids -[install] -install_scripts=$base/lib/subjugator_centroids diff --git a/src/subjugator/gnc/subjugator_centroids/setup.py b/src/subjugator/gnc/subjugator_centroids/setup.py deleted file mode 100644 index 372fca3d..00000000 --- a/src/subjugator/gnc/subjugator_centroids/setup.py +++ /dev/null @@ -1,25 +0,0 @@ -from setuptools import find_packages, setup - -package_name = "subjugator_centroids" - -setup( - name=package_name, - version="0.0.0", - packages=find_packages(exclude=["test"]), - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="sub9", - maintainer_email="jgoodman1@ufl.edu", - description="TODO: Package description", - license="TODO: License declaration", - extras_require={"test": ["pytest"]}, - entry_points={ - "console_scripts": [ - "subjugator_centroids = subjugator_centroids.subjugator_centroids_node:main", - ], - }, -) diff --git a/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/__init__.py b/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/centroid_finder.py b/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/centroid_finder.py deleted file mode 100644 index d864cb29..00000000 --- a/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/centroid_finder.py +++ /dev/null @@ -1,19 +0,0 @@ -from abc import ABC, abstractmethod - -from cv2.typing import MatLike - -""" -If you want to track some object, you need a class that implements this class -""" - - -class CentroidFinder(ABC): - @property - @abstractmethod - def topic_name(self) -> str: - pass - - # returns (x, y) pair of centroid or None if not found - @abstractmethod - def find_centroid(self, frame: MatLike) -> tuple[int, int] | None: - pass diff --git a/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/green_tracker.py b/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/green_tracker.py deleted file mode 100644 index 5b35643d..00000000 --- a/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/green_tracker.py +++ /dev/null @@ -1,51 +0,0 @@ -import cv2 -import numpy as np -from cv2.typing import MatLike - -from subjugator_centroids.centroid_finder import CentroidFinder - - -# example implementation of centroid abstract class, this one tracks green objects -class GreenTracker(CentroidFinder): - def __init__(self, topic_name: str): - self.topic_name_: str = topic_name - - @property - def topic_name(self) -> str: - return self.topic_name_ - - def find_centroid(self, frame: MatLike) -> tuple[int, int] | None: - """ - Detect lime green areas in the image and return the centroid (x, y). - Returns None if no lime green region is detected. - """ - # Convert BGR to HSV - hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) - - # Define HSV range for lime green - lower_green = np.array([40, 100, 100]) - upper_green = np.array([80, 255, 255]) - - # Threshold the HSV image to get only green colors - mask = cv2.inRange(hsv, lower_green, upper_green) - - # Find contours in the mask - contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - if not contours: - return None - - # Find the largest contour (ignore small ones) - largest = max(contours, key=cv2.contourArea) - if cv2.contourArea(largest) < 50: - return None - - # Compute centroid - M = cv2.moments(largest) - if M["m00"] == 0: - return None - - cx = int(M["m10"] / M["m00"]) - cy = int(M["m01"] / M["m00"]) - - return (cx, cy) diff --git a/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/orange_tracker.py b/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/orange_tracker.py deleted file mode 100644 index 2ae7386d..00000000 --- a/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/orange_tracker.py +++ /dev/null @@ -1,50 +0,0 @@ -import cv2 -import numpy as np -from cv2.typing import MatLike - -from subjugator_centroids.centroid_finder import CentroidFinder - - -class OrangeTracker(CentroidFinder): - def __init__(self, topic_name: str): - self.topic_name_: str = topic_name - - @property - def topic_name(self) -> str: - return self.topic_name_ - - def find_centroid(self, frame: MatLike) -> tuple[int, int] | None: - """ - Detect orange areas in the image and return the centroid (x, y). - Returns None if no orange region is detected. - """ - # Convert BGR to HSV - hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) - - # lower and upper bounds for orange in HSV - lower_orange = np.array([10, 100, 20]) - upper_orange = np.array([25, 255, 255]) - - # Threshold the HSV image to get only green colors - mask = cv2.inRange(hsv, lower_orange, upper_orange) - - # Find contours in the mask - contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - if not contours: - return None - - # Find the largest contour (ignore small ones) - largest = max(contours, key=cv2.contourArea) - if cv2.contourArea(largest) < 50: - return None - - # Compute centroid - M = cv2.moments(largest) - if M["m00"] == 0: - return None - - cx = int(M["m10"] / M["m00"]) - cy = int(M["m01"] / M["m00"]) - - return (cx, cy) diff --git a/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/red_tracker.py b/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/red_tracker.py deleted file mode 100644 index 48cbb96a..00000000 --- a/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/red_tracker.py +++ /dev/null @@ -1,76 +0,0 @@ -import cv2 -import numpy as np -from cv2.typing import MatLike - -from subjugator_centroids.centroid_finder import CentroidFinder - - -# example implementation of centroid abstract class, this one tracks green objects -class RedTracker(CentroidFinder): - def __init__(self, topic_name: str): - self.topic_name_: str = topic_name - self.debug = False - - @property - def topic_name(self) -> str: - return self.topic_name_ - - def find_centroid(self, frame: MatLike) -> tuple[int, int] | None: - """ - Detect red areas in the image and return the centroid (x, y). - Returns None if no red region is detected. - """ - # Convert BGR to HSV - hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) - - # Define HSV ranges for red (wraps around the 0° hue boundary) - lower_red1 = np.array([0, 140, 140]) - upper_red1 = np.array([8, 255, 255]) - - lower_red2 = np.array([172, 140, 140]) - upper_red2 = np.array([180, 255, 255]) - - # Create two masks and combine them - mask1 = cv2.inRange(hsv, lower_red1, upper_red1) - mask2 = cv2.inRange(hsv, lower_red2, upper_red2) - mask = cv2.bitwise_or(mask1, mask2) - - if self.debug: - cv2.imshow("Red Mask", mask) - cv2.waitKey(1) - - # Find contours in the mask - contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - if not contours: - return None - - # Find the largest contour (ignore small ones) - largest = max(contours, key=cv2.contourArea) - if cv2.contourArea(largest) < 50: - return None - - # Compute centroid - M = cv2.moments(largest) - if M["m00"] == 0: - return None - - cx = int(M["m10"] / M["m00"]) - cy = int(M["m01"] / M["m00"]) - - return (cx, cy) - - -def test(): - gt = RedTracker("testing/rn/sry") - gt.debug = True - cap = cv2.VideoCapture(0) - while True: - ret, frame = cap.read() - if not ret: - print("i hate you") - gt.find_centroid(frame) - - -if __name__ == "__main__": - test() diff --git a/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/subjugator_centroids_node.py b/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/subjugator_centroids_node.py deleted file mode 100644 index 762ccbdf..00000000 --- a/src/subjugator/gnc/subjugator_centroids/subjugator_centroids/subjugator_centroids_node.py +++ /dev/null @@ -1,57 +0,0 @@ -import rclpy -from rclpy.node import Node -from rclpy.publisher import Publisher -from yolo_msgs.msg import Detection, DetectionArray - - -class SubjugatorCentroidsNode(Node): - def __init__(self): - super().__init__("subjugator_centroids_node") - - self.topics: dict[int, Publisher] = {} - self._tracking_sub = self.create_subscription( - DetectionArray, - "yolo/tracking", - self.tracking_cb, - 10, - ) - - def tracking_cb(self, msg: DetectionArray): - for detection in msg.detections: - detection: Detection - class_id: int = detection.class_id - class_name: str = detection.class_name - # id: int = detection.id - # center_x: float = detection.bbox.center.position.x - # center_y: float = detection.bbox.center.position.y - # size_x: float = detection.bbox.size.x - # size_y: float = detection.bbox.size.y - - # check to see if topic already exists, if it doesn't, create it - pub_already_exists: bool = class_id in self.topics - if not pub_already_exists: - topic_name: str = "centroids/" + class_name - self.topics[class_id] = self.create_publisher(Detection, topic_name, 10) - - self.topics[class_id].publish(detection) - - # print("------------") - # print(detection.class_id) - # print(detection.class_name) - # print(detection.id) - # print(detection.bbox.center.position.x) - # print(detection.bbox.center.position.y) - # print(detection.bbox.size.x) - # print(detection.bbox.size.y) - # print("------------") - - -def main(): - rclpy.init() - node = SubjugatorCentroidsNode() - rclpy.spin(node) - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/subjugator/gnc/subjugator_wrench_tuner/config/wrench_tuner_params.yaml b/src/subjugator/gnc/subjugator_wrench_tuner/config/wrench_tuner_params.yaml deleted file mode 100644 index 00df0aaf..00000000 --- a/src/subjugator/gnc/subjugator_wrench_tuner/config/wrench_tuner_params.yaml +++ /dev/null @@ -1,11 +0,0 @@ -wrench_tuner: - ros__parameters: - c1: 0.0 - c2: 0.0 - c3: 0.0 - c4: 0.0 - c5: 0.0 - c6: 0.0 - rx: 0.2 - ry: 0.0 - rz: 0.0 diff --git a/src/subjugator/gnc/subjugator_wrench_tuner/launch/wrench_tuner_launch.py b/src/subjugator/gnc/subjugator_wrench_tuner/launch/wrench_tuner_launch.py deleted file mode 100644 index 0cb7f42c..00000000 --- a/src/subjugator/gnc/subjugator_wrench_tuner/launch/wrench_tuner_launch.py +++ /dev/null @@ -1,26 +0,0 @@ -from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - config_file = PathJoinSubstitution( - [ - FindPackageShare("subjugator_wrench_tuner"), - "config", - "wrench_tuner_params.yaml", - ], - ) - - return LaunchDescription( - [ - Node( - package="subjugator_wrench_tuner", - executable="wrench_tuner", - name="wrench_tuner", - output="screen", - parameters=[config_file], - ), - ], - ) diff --git a/src/subjugator/gnc/subjugator_wrench_tuner/package.xml b/src/subjugator/gnc/subjugator_wrench_tuner/package.xml deleted file mode 100644 index ca35934b..00000000 --- a/src/subjugator/gnc/subjugator_wrench_tuner/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - subjugator_wrench_tuner - 0.0.0 - TODO: Package description - adamm - Apache-2.0 - geometry_msgs - nav_msgs - numpy - rclpy - std_msgs - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/src/subjugator/gnc/subjugator_wrench_tuner/resource/subjugator_wrench_tuner b/src/subjugator/gnc/subjugator_wrench_tuner/resource/subjugator_wrench_tuner deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subjugator/gnc/subjugator_wrench_tuner/setup.cfg b/src/subjugator/gnc/subjugator_wrench_tuner/setup.cfg deleted file mode 100644 index 478646c5..00000000 --- a/src/subjugator/gnc/subjugator_wrench_tuner/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/subjugator_wrench_tuner -[install] -install_scripts=$base/lib/subjugator_wrench_tuner diff --git a/src/subjugator/gnc/subjugator_wrench_tuner/setup.py b/src/subjugator/gnc/subjugator_wrench_tuner/setup.py deleted file mode 100644 index 29c95e35..00000000 --- a/src/subjugator/gnc/subjugator_wrench_tuner/setup.py +++ /dev/null @@ -1,27 +0,0 @@ -from setuptools import find_packages, setup - -package_name = "subjugator_wrench_tuner" - -setup( - name=package_name, - version="0.0.0", - packages=find_packages(exclude=["test"]), - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - ("share/" + package_name + "/launch", ["launch/wrench_tuner_launch.py"]), - ("share/" + package_name + "/config", ["config/wrench_tuner_params.yaml"]), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="adamm", - maintainer_email="amcaleer1127@gmail.com", - description="TODO: Package description", - license="Apache-2.0", - extras_require={"test": ["pytest"]}, - entry_points={ - "console_scripts": [ - "wrench_tuner = subjugator_wrench_tuner.wrench_tuner:main", - ], - }, -) diff --git a/src/subjugator/gnc/subjugator_wrench_tuner/subjugator_wrench_tuner/__init__.py b/src/subjugator/gnc/subjugator_wrench_tuner/subjugator_wrench_tuner/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subjugator/gnc/subjugator_wrench_tuner/subjugator_wrench_tuner/wrench_tuner.py b/src/subjugator/gnc/subjugator_wrench_tuner/subjugator_wrench_tuner/wrench_tuner.py deleted file mode 100644 index e93e4a6b..00000000 --- a/src/subjugator/gnc/subjugator_wrench_tuner/subjugator_wrench_tuner/wrench_tuner.py +++ /dev/null @@ -1,164 +0,0 @@ -import numpy as np -import rclpy -from geometry_msgs.msg import Wrench -from nav_msgs.msg import Odometry -from rcl_interfaces.msg import SetParametersResult -from rclpy.node import Node - - -class WrenchTuner(Node): - - def __init__(self): - - super().__init__("wrench_tuner") - self.cmd_subscription = self.create_subscription( - Wrench, - "cmd_wrench", - self.listener_callback, - 10, - ) - - self.cmd_subscription # prevent unused variable warning - - self.odom_subscription = self.create_subscription( - Odometry, - "odometry/filtered", - self.odometry_callback, - 10, - ) - - self.control_wrench_publisher = self.create_publisher( - Wrench, - "control_wrench", - 10, - ) - self.control_wrench_publisher # prevent unused variable warning - - self.declare_parameter("c1", 0.0) - self.declare_parameter("c2", 0.0) - self.declare_parameter("c3", 0.0) - self.declare_parameter("c4", 0.0) - self.declare_parameter("c5", 0.0) - self.declare_parameter("c6", 0.0) - self.declare_parameter("rx", 0.0) - self.declare_parameter("ry", 0.0) - self.declare_parameter("rz", 0.0) - - self.params = [ - self.get_parameter("c1").value, - self.get_parameter("c2").value, - self.get_parameter("c3").value, - self.get_parameter("c4").value, - self.get_parameter("c5").value, - self.get_parameter("c6").value, - self.get_parameter("rx").value, - self.get_parameter("ry").value, - self.get_parameter("rz").value, - ] - - self.add_on_set_parameters_callback(self.parameter_callback) - - self.velocity = np.zeros(3) - - def listener_callback(self, msg): - self.cmd_wrench = np.array( - [ - msg.force.x, - msg.force.y, - msg.force.z, - msg.torque.x, - msg.torque.y, - msg.torque.z, - ], - ) - - if ( - msg.force.x == 0.0 - and msg.force.y == 0.0 - and msg.force.z == 0.0 - and msg.torque.x == 0.0 - and msg.torque.y == 0.0 - and msg.torque.z == 0.0 - ): - # self.get_logger().info("ignoring wrench") - control_wrench = Wrench() - control_wrench.force.x = 0 - control_wrench.force.y = 0 - control_wrench.force.z = 0 - control_wrench.torque.x = 0 - control_wrench.torque.y = 0 - control_wrench.torque.z = 0 - - self.control_wrench_publisher.publish(msg) - return - - # square the velocity as it has a quadratic effect on drag - self.vx = self.velocity[0] ** 2 - self.vy = self.velocity[1] ** 2 - self.vz = self.velocity[2] ** 2 - - self.rx = self.params[6] - self.ry = self.params[7] - self.rz = self.params[8] - - self.drag_wrench = np.array( - [ - self.params[0] * self.vx, - self.params[1] * self.vy, - self.params[2] * self.vz, - self.params[3] * (self.ry * self.vz - self.rz * self.vy), - self.params[4] * (-1 * self.rx * self.vz + self.rz * self.vx), - self.params[5] * (self.rx * self.vy - self.ry * self.vx), - ], - ) - - self.sum_wrench = self.cmd_wrench + self.drag_wrench - - control_wrench = Wrench() - control_wrench.force.x = self.sum_wrench[0] - control_wrench.force.y = self.sum_wrench[1] - control_wrench.force.z = self.sum_wrench[2] - control_wrench.torque.x = self.sum_wrench[3] - control_wrench.torque.y = self.sum_wrench[4] - control_wrench.torque.z = self.sum_wrench[5] - - self.control_wrench_publisher.publish(control_wrench) - - def odometry_callback(self, msg): - # stores the most recent velocity from odom - self.velocity = np.array( - [ - msg.twist.twist.linear.x, - msg.twist.twist.linear.y, - msg.twist.twist.linear.z, - ], - ) - - def parameter_callback(self, params): - for param in params: - for i, name in enumerate( - ["c1", "c2", "c3", "c4", "c5", "c6", "rx", "ry", "rz"], - ): - - if param.name == name: - self.params[i] = param.value - self.get_logger().info(f"{name} updated to {param.value}") - return SetParametersResult(successful=True) - - -def main(args=None): - rclpy.init(args=args) - - wrench_tuner = WrenchTuner() - - rclpy.spin(wrench_tuner) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - wrench_tuner.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/MANIFEST.in b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/MANIFEST.in deleted file mode 100644 index e4a12c92..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/MANIFEST.in +++ /dev/null @@ -1 +0,0 @@ -include /missions/*.yaml diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/in b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/in deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/launch/mission_planner_launch.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/launch/mission_planner_launch.py deleted file mode 100644 index 63e9b58b..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/launch/mission_planner_launch.py +++ /dev/null @@ -1,26 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description(): - # ① declare a launch-argument (default just the file-name) - declare_mission = DeclareLaunchArgument( - "mission_file", - default_value="prequal.yaml", - description="YAML file located in subjugator_mission_planner/missions", - ) - - # ② make its value available - mission = LaunchConfiguration("mission_file") - - # ③ feed that value into the Node's parameter dict - planner = Node( - package="subjugator_mission_planner", - executable="mission_planner", - name="mission_planner", - parameters=[{"mission_file": mission}], - ) - - return LaunchDescription([declare_mission, planner]) diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/launch/task_server_launch.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/launch/task_server_launch.py deleted file mode 100644 index e59abe20..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/launch/task_server_launch.py +++ /dev/null @@ -1,51 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - return LaunchDescription( - [ - Node( - package="subjugator_mission_planner", - executable="navigate_around_server", - name="navigate_around_server", - parameters=[], - ), - Node( - package="subjugator_mission_planner", - executable="movement_server", - name="movement_server", - parameters=[], - ), - Node( - package="subjugator_mission_planner", - executable="wait_server", - name="wait_server", - parameters=[], - ), - Node( - package="subjugator_mission_planner", - executable="start_gate_server", - name="start_gate_server", - parameters=[], - ), - Node( - package="subjugator_mission_planner", - executable="yawtracker", - name="yawtracker", - parameters=[], - ), - Node( - package="subjugator_mission_planner", - executable="mechanisms_server", - name="mechanisms_server", - parameters=[], - ), - Node( - package="subjugator_mission_planner", - executable="nav_channel_server", - name="nav_channel_server", - parameters=[], - ), - ], - ) diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/mechanism_test.yaml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/mechanism_test.yaml deleted file mode 100644 index 57932b98..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/mechanism_test.yaml +++ /dev/null @@ -1,37 +0,0 @@ -mission: - - task: move - parameters: - type: "Relative" - x: 2.5 - y: 0.0 - z: -0.1 - i: 0.0 - j: 0.0 - k: 0.0 - w: 1.0 - timeout: 120 - - task: move - parameters: - type: "Relative" - x: 0.0 - y: 0.0 - z: 0.0 - i: 0.0 - j: 0.0 - k: 1.0 - w: 0.0 - timeout: 120 - - task: wait - parameters: - time: 3.0 - - task: mechanism - parameters: - mechanism: "torpedo" - angle: 35 - - task: wait - parameters: - time: 1.0 - - task: mechanism - parameters: - mechanism: "torpedo" - angle: 82 diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/mission1_example.yaml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/mission1_example.yaml deleted file mode 100644 index d6201919..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/mission1_example.yaml +++ /dev/null @@ -1,19 +0,0 @@ -mission: - - task: search_for_object - parameters: - object: red_gate - - task: navigate_through_object - parameters: - object: red_gate - - task: search_for_object - parameters: - object: black_pole - - task: navigate_around_object - parameters: - object: black_pole - - task: search_for_object - parameters: - object: red_gate - - task: navigate_through_object - parameters: - object: red_gate diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/move_test.yaml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/move_test.yaml deleted file mode 100644 index fb67055d..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/move_test.yaml +++ /dev/null @@ -1,13 +0,0 @@ -mission: - - - task: move - parameters: - type: "Relative" - x: 0.0 - y: 0.0 - z: -0.7 - i: 0.0 - j: 0.0 - k: 0.0 - w: 1.0 - timeout: 120 diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/nav_channel_test.yaml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/nav_channel_test.yaml deleted file mode 100644 index eb7d117a..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/nav_channel_test.yaml +++ /dev/null @@ -1,5 +0,0 @@ -mission: - - - task: navchannel - parameters: - number_of_red_poles: 123 diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/navigate_around_test.yaml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/navigate_around_test.yaml deleted file mode 100644 index 6e95e2d8..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/navigate_around_test.yaml +++ /dev/null @@ -1,6 +0,0 @@ -mission: - - - task: navigatearound - parameters: - object: "None" - radius: 0.75 diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/navigate_channel.yaml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/navigate_channel.yaml deleted file mode 100644 index 0f1c702e..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/navigate_channel.yaml +++ /dev/null @@ -1,9 +0,0 @@ -mission: - - task: navigatechannel - parameters: - red_label: "red-pole" - white_label: "white-pole" - overall_timeout: 120.0 - row_timeout: 3.0 - max_rows: 3 - target_offset_px: -50.0 diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/prequal.yaml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/prequal.yaml deleted file mode 100644 index 2c199480..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/prequal.yaml +++ /dev/null @@ -1,63 +0,0 @@ -mission: - - - task: move - parameters: - - type: "Relative" - x: 0.0 - y: 0.0 - z: -0.7 - i: 0.0 - j: 0.0 - k: 0.0 - w: 1.0 - timeout: 120 - - task: move - parameters: - type: "Relative" - x: 5.0 - y: 0.0 - z: -0.7 - i: 0.0 - j: 0.0 - k: 0.0 - w: 1.0 - timeout: 120 - - task: navigatearound - parameters: - object: "None" - - radius: 0.75 - timeout: 60 - - task: move - parameters: - type: "Relative" - x: 8.5 - y: 0.0 - z: -0.7 - i: 0.0 - j: 0.0 - k: 1.0 - w: 0.0 - timeout: 120 - - task: move - parameters: - type: "Relative" - x: 5.0 - y: 0.0 - z: -0.7 - i: 0.0 - j: 0.0 - k: 1.0 - w: 0.0 - timeout: 120 - - task: move - parameters: - task: "Relative" - x: 0.0 - y: 0.0 - z: -0.7 - i: 0.0 - j: 0.0 - k: 1.0 - w: 0.0 diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/prequal_mission.yaml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/prequal_mission.yaml deleted file mode 100644 index 9f308e8f..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/prequal_mission.yaml +++ /dev/null @@ -1,21 +0,0 @@ -mission: - - task: search_for_object - parameters: - object: red_gate - timeout: 7.5 - - task: navigate_through_object - parameters: - object: red_gate - distance: 0.5 - - task: search_for_object - parameters: - object: black_pole - - task: navigate_around_object - parameters: - object: black_pole - - task: search_for_object - parameters: - object: red_gate - - task: navigate_through_object - parameters: - object: red_gate diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/sonar_follower_test.yaml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/sonar_follower_test.yaml deleted file mode 100644 index 97bab1f5..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/sonar_follower_test.yaml +++ /dev/null @@ -1,5 +0,0 @@ -mission: - - - task: sonarfollower - parameters: - idk: "hi" diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/start_gate.yaml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/start_gate.yaml deleted file mode 100644 index 40f0d8d9..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/start_gate.yaml +++ /dev/null @@ -1,8 +0,0 @@ -mission: - - - task: startgate - parameters: - distance: 10.0 - num_yaws: 4 - num_rolls: 0 - num_pitch: 0 diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/test_all.yaml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/test_all.yaml deleted file mode 100644 index 05659f04..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/test_all.yaml +++ /dev/null @@ -1,27 +0,0 @@ -mission: - - - task: move - parameters: - type: "Relative" - x: 0.0 - y: 0.0 - z: -0.7 - i: 0.0 - j: 0.0 - k: 0.0 - w: 1.0 - timeout: 120 - - task: navigatearound - parameters: - object: "None" - radius: 0.75 - timeout: 60 - - task: wait - parameters: - time: 5.0 - - task: startgate - parameters: - distance: 10.0 - num_yaws: 4 - num_rolls: 0 - num_pitch: 0 diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/wait_test.yaml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/wait_test.yaml deleted file mode 100644 index 7f9a6166..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/wait_test.yaml +++ /dev/null @@ -1,5 +0,0 @@ -mission: - - - task: wait - parameters: - time: 5.0 diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/yaw_tracker_test.yaml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/yaw_tracker_test.yaml deleted file mode 100644 index e0affe30..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/yaw_tracker_test.yaml +++ /dev/null @@ -1,5 +0,0 @@ -mission: - - - task: yaw_tracker_server - parameters: - topic_name: "centroids/red_pole" diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/package.xml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/package.xml deleted file mode 100644 index 48a8067e..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - subjugator_mission_planner - 0.0.0 - TODO: Package description - adamm - "TODO" - - ament_cmake - ament_python - rosidl_default_generators - - rosidl_default_generators - - action_msgs - geometry_msgs - rclpy - rosidl_default_runtime - std_msgs - - subjugator_msgs - - ament_copyright - ament_flake8 - ament_pep257 - pytest - - rosidl_interface_packages - - - ament_python - - diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/resource/subjugator_mission_planner b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/resource/subjugator_mission_planner deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.cfg b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.cfg deleted file mode 100644 index 61687a5d..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/subjugator_mission_planner -[install] -install_scripts=$base/lib/subjugator_mission_planner diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.py deleted file mode 100644 index b3c0ce26..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.py +++ /dev/null @@ -1,47 +0,0 @@ -from glob import glob - -from setuptools import find_packages, setup - -package_name = "subjugator_mission_planner" - -setup( - name=package_name, - include_package_data=True, - package_data={"": ["missions/*.yaml"]}, - version="0.0.1", - packages=find_packages(include=[package_name, f"{package_name}.*"]), - data_files=[ - ( - "share/ament_index/resource_index/packages", - ["resource/subjugator_mission_planner"], - ), - (f"share/{package_name}", ["package.xml"]), - (f"share/{package_name}/missions", glob("missions/*.yaml")), - ( - f"share/{package_name}/launch", - ["launch/mission_planner_launch.py", "launch/task_server_launch.py"], - ), - ], - install_requires=["setuptools"], - python_requires=">=3.8", - zip_safe=True, - maintainer="Adam McAleer", - maintainer_email="amcaleer1127@gmail.com", - description="Mission planner package with ROS2 action servers", - license="", - extras_require={"test": ["pytest"]}, - entry_points={ - "console_scripts": [ - "mission_planner = subjugator_mission_planner.mission_planner:main", - "navigate_around_server = subjugator_mission_planner.navigate_around_server:main", - "navigate_through_server = subjugator_mission_planner.navigate_through_server:main", - "search_server = subjugator_mission_planner.search_server:main", - "wait_server = subjugator_mission_planner.wait_server:main", - "start_gate_server = subjugator_mission_planner.start_gate_server:main", - "yawtracker = subjugator_mission_planner.yaw_tracker_server:main", - "mechanisms_server = subjugator_mission_planner.mechanisms_server:main", - "sonar_follower = subjugator_mission_planner.sonar_follower_server:main", - "nav_channel_server = subjugator_mission_planner.nav_channel_server:main", - ], - }, -) diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/__init__.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mechanisms_server.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mechanisms_server.py deleted file mode 100644 index 9cc2a2b7..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mechanisms_server.py +++ /dev/null @@ -1,67 +0,0 @@ -import rclpy -from rclpy.action import ActionServer, CancelResponse, GoalResponse -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node -from subjugator_msgs.action import Mechanism -from subjugator_msgs.srv import Servo - - -class MechanismServer(Node): - def __init__(self): - super().__init__("mechanism_server") - - # Action server - self._action_server = ActionServer( - self, - Mechanism, - "mechanism", - execute_callback=self.execute_callback, - goal_callback=self.goal_callback, - cancel_callback=self.cancel_callback, - ) - - self.dropper_client = self.create_client(Servo, "dropper") - self.torpedo_client = self.create_client(Servo, "torpedo") - self.gripper_client = self.create_client(Servo, "gripper") - - def goal_callback(self, goal_request): - self.mechanism = goal_request.mechanism - self.angle = goal_request.angle - self.get_logger().info( - f"Received goal to move {self.mechanism} to {self.angle} degrees", - ) - return GoalResponse.ACCEPT - - def cancel_callback(self, goal_handle): - self.get_logger().info("Received cancel request") - return CancelResponse.ACCEPT - - def execute_callback(self, goal_handle): - - msg = Servo.Request() - msg.angle = self.angle - if self.mechanism == "dropper": - self.dropper_client.call(msg) - elif self.mechanism == "gripper": - self.gripper_client.call(msg) - elif self.mechanism == "torpedo": - self.torpedo_client.call(msg) - - goal_handle.succeed() - result = Mechanism.Result() - result.success = True - result.message = "Mechanism move complete" - return result - - -def main(args=None): - rclpy.init(args=args) - node = MechanismServer() - executor = MultiThreadedExecutor() - executor.add_node(node) - executor.spin() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mission_planner.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mission_planner.py deleted file mode 100644 index 4207335e..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mission_planner.py +++ /dev/null @@ -1,181 +0,0 @@ -import inspect -import os - -import rclpy -import yaml -from ament_index_python.packages import get_package_share_directory -from geometry_msgs.msg import Pose -from rclpy.action.client import ActionClient -from rclpy.node import Node -from subjugator_msgs import action as action_interfaces - - -class MissionPlanner(Node): - def __init__(self): - super().__init__("mission_planner") - - package_share = get_package_share_directory("subjugator_mission_planner") - print(f"PATH: {package_share}") - - # Find a mission file, if none is specified use prequal - default_mission_file = os.path.join("wait_test.yaml") - self.declare_parameter("mission_file", default_mission_file) - mission_file = os.path.join( - package_share, - "missions", - self.get_parameter("mission_file").get_parameter_value().string_value, - ) - - self.mission = self.load_mission_file(mission_file) - self.current_task_index = 0 - self.executing_task = False - - # Create a dictionary to store all of the action clients - self.action_clients = {} - - # Dictionary is populated by parsing through actions in the subjugator_msgs.action directory - # The name of the task (i.e what should be used in the mission .yaml file) is an all lowercase of the action name - # E.g to use the action NavigateAround.action, the mission yaml should contain navigatearound. To use StartGate.action, the mission should use startgate - self.available_actions = { - name.lower(): action - for name, action in inspect.getmembers(action_interfaces, inspect.isclass) - if hasattr(action, "Goal") - } - - # Timer to periodically check mission progress and start tasks - self.timer = self.create_timer(0.5, self.execute_mission) - - # Load mission file from yaml - def load_mission_file(self, filepath): - try: - with open(filepath) as f: - mission_data = yaml.safe_load(f) - self.get_logger().info(f"Mission file loaded: {filepath}") - return mission_data["mission"] - except Exception as e: - self.get_logger().error(f"Failed to load mission file: {e}") - return [] - - def execute_mission(self): - if self.executing_task: - # Currently running a task; wait for it to complete - return - - if self.current_task_index >= len(self.mission): - self.get_logger().info("Mission complete!") - return - - # Iterate through the mission yaml by loading each task - - # get the task, task name, and the task parameters at each index - task = self.mission[self.current_task_index] - task_name = task.get("task") - params = task.get("parameters", {}) - - self.get_logger().info( - f"Starting task {self.current_task_index + 1}/{len(self.mission)}: {task_name}", - ) - - action_class = self.available_actions.get(task_name.lower()) - if not action_class: - self.get_logger().error(f"Unknown task: {task_name}") - self.current_task_index += 1 - return - - client = self.get_or_create_client(task_name, action_class) - - if not client.wait_for_server(timeout_sec=2.0): - self.get_logger().error(f"{task_name} server not available") - self.current_task_index += 1 - return - self.get_logger().info(f"Goal parameters: {params}") - - goal_msg = self.build_goal_message(action_class, params) - - self.executing_task = True - self._send_goal(client, goal_msg) - - # Create action clients for each action that was found - def get_or_create_client(self, task_name, action_class): - if task_name not in self.action_clients: - self.action_clients[task_name] = ActionClient(self, action_class, task_name) - return self.action_clients[task_name] - - # Create a generalizable goal message - def build_goal_message(self, action_class, params): - goal = action_class.Goal() - print("Goal fields:", goal.__slots__) - print("Params received:", params) - - # Populate fields automatically from YAML parameters - for field_name in goal.__slots__: - clean_name = field_name.lstrip("_") # Remove leading underscore - val = params.get(clean_name) - if val is not None: - setattr(goal, clean_name, val) - print(f"Set {clean_name} to {val}") - # Handle nested Pose fields - elif isinstance(getattr(goal, field_name), Pose): - pose = Pose() - pose.position.x = params.get("x", 0.0) - pose.position.y = params.get("y", 0.0) - pose.position.z = params.get("z", 0.0) - pose.orientation.x = params.get("i", 0.0) - pose.orientation.y = params.get("j", 0.0) - pose.orientation.z = params.get("k", 0.0) - pose.orientation.w = params.get("w", 1.0) - setattr(goal, field_name, pose) - - return goal - - # Send the goal message to the relevant action client - def _send_goal(self, client: ActionClient, goal_msg): - send_goal_future = client.send_goal_async( - goal_msg, - feedback_callback=self.feedback_callback, - ) - send_goal_future.add_done_callback(self.goal_response_callback) - - # Handle the goal response from the action client - def goal_response_callback(self, future): - goal_handle = future.result() - if not goal_handle.accepted: - self.get_logger().error("Goal rejected by server") - self.executing_task = False - self.current_task_index += 1 - return - - self.get_logger().info("Goal accepted, executing...") - goal_handle.get_result_async().add_done_callback(self.get_result_callback) - - # Handle feedback from the action clients - currently not used - def feedback_callback(self, feedback_msg): - self.get_logger().info(f"Feedback: {feedback_msg.feedback}") - - # Handles results feedback from the action clients - currently not used but would use for behavior tree type behavior - def get_result_callback(self, future): - result = future.result().result - success = getattr(result, "success", False) - message = getattr(result, "message", "") - - if success: - self.get_logger().info(f"Task {self.current_task_index + 1} succeeded") - else: - self.get_logger().error( - f"Task {self.current_task_index + 1} failed: {message}", - ) - - self.executing_task = False - self.current_task_index += 1 - - -def main(args=None): - rclpy.init(args=args) - node = MissionPlanner() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/nav_channel_server.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/nav_channel_server.py deleted file mode 100644 index 3d67665a..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/nav_channel_server.py +++ /dev/null @@ -1,282 +0,0 @@ -import numpy as np -import rclpy -from geometry_msgs.msg import Pose -from nav_msgs.msg import Odometry -from rclpy.action.client import ActionClient -from rclpy.action.server import ActionServer, CancelResponse, GoalResponse -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node -from subjugator_msgs.action import Move, NavChannel, YawTracker -from tf_transformations import euler_from_quaternion -from yolo_msgs.msg import Detection - - -class ActionUser: - """ - class to make calling other missions easier, you do still kinda have to construct the goals on your own sry - """ - - def __init__(self, node: Node, action_type, action_name: str): - self.node = node - self.ac = ActionClient(node, action_type, action_name) - - # 0 timeout seconds implies no timeout - # TODO rn there is nothing for timeout_sec, would be a great first issue for someone :) (use self.node.get_clock().now()) - # _=0 should be timeout_sec = 0 - def send_goal_and_block_until_done(self, goal, _=0): - future = self.ac.send_goal_async(goal) # rn no feedback - running = True - while running: - rclpy.spin_once(self.node, timeout_sec=0.1) - - if future.done(): - result = future.result() - result_future = result.get_result_async() - - while not result_future.done(): # TODO this could be better - rclpy.spin_once(self.node, timeout_sec=0.1) - running = False - - def send_goal_and_return_future(self, goal): - return self.ac.send_goal_async(goal) - - -# algo: spot red pole and draw line -# move right and - - -# TODO -# 1. this -# 2. pose relative -# 3. call other missions !?!!? - - -def closest_points_between_lines(a1, v1, b1, v2): - """ - Finds the closest points on two lines defined by: - - Line 1: a1 + t * v1 - - Line 2: b1 + s * v2 - - Returns: - p1_closest: closest point on line 1 - p2_closest: closest point on line 2 - midpoint: the average of the two (midpoint between the lines) - """ - # Ensure numpy arrays - a1 = np.array(a1, dtype=np.float64) - v1 = np.array(v1, dtype=np.float64) - b1 = np.array(b1, dtype=np.float64) - v2 = np.array(v2, dtype=np.float64) - - # Define some dot products - r = a1 - b1 - v1_dot_v1 = np.dot(v1, v1) - v2_dot_v2 = np.dot(v2, v2) - v1_dot_v2 = np.dot(v1, v2) - v1_dot_r = np.dot(v1, r) - v2_dot_r = np.dot(v2, r) - - denom = v1_dot_v1 * v2_dot_v2 - v1_dot_v2**2 - - # If denom is zero, lines are parallel — handle gracefully - if np.isclose(denom, 0.0): - # Pick arbitrary point on line 1, project onto line 2 - t = 0 - s = v2_dot_r / v2_dot_v2 - else: - t = (v1_dot_v2 * v2_dot_r - v2_dot_v2 * v1_dot_r) / denom - s = (v1_dot_v2 * t + v2_dot_r) / v2_dot_v2 - - # Closest points - p1_closest = a1 + t * v1 - p2_closest = b1 + s * v2 - midpoint = (p1_closest + p2_closest) / 2.0 - - return midpoint - - -class NavChannelServer(Node): - def __init__(self): - super().__init__("navchannel") - - # odom and image data - self._odom_sub = self.create_subscription( - Odometry, - "odometry/filtered", - self.odom_cb, - 10, - ) - self.recent_odom: Odometry = Odometry() - - self.centroid_sub_ = self.create_subscription( - Detection, - "centroids/red_pole", - self.red_pole_cb, - 10, - ) - self.recent_detection: Detection = Detection() - self.detection_count = 0 - - # Action server - self._action_server = ActionServer( - self, - NavChannel, - "navchannel", - execute_callback=self.execute_callback, - goal_callback=self.goal_callback, - cancel_callback=self.cancel_callback, - ) - - self.move_client = ActionUser(self, Move, "move") - self.yaw_tracker_client = ActionUser(self, YawTracker, "yawtracker") - - def odom_cb(self, msg: Odometry): - self.recent_odom = msg - - def red_pole_cb(self, msg: Detection): - self.recent_detection = msg - self.detection_count += 1 - - def goal_callback(self, goal_request: NavChannel.Goal): - self.number_of_red_poles = goal_request.number_of_red_poles - self.detection_count = 0 - self.get_logger().info( - f"Received goal saying there are {self.number_of_red_poles} red_poles", - ) - return GoalResponse.ACCEPT - - def cancel_callback(self, _): - self.get_logger().info("Received cancel request") - return CancelResponse.ACCEPT - - def spot_and_center_red_pole(self): - # p represents a 10 degree positive yaw - p = Pose() - p.orientation.z = 0.0872 - p.orientation.w = 0.9962 - - self.detection_count = 0 - - # turn left until you see a red pole: - goal = Move.Goal() - goal.type = "Relative" - goal.goal_pose = p - while self.detection_count < 5: - self.move_client.send_goal_and_block_until_done(goal) - self.sleep_for(0.5) - - # now that we see a red pole, we should center on it?? - # goal = YawTracker.Goal() - # goal.topic_name = "centroids/red_pole" - # self.yaw_tracker_client.send_goal_and_block_until_done(goal) - - def sleep_for(self, time: float): - start_time = self.get_clock().now() - duration = rclpy.duration.Duration(seconds=time) - - while (self.get_clock().now() - start_time) < duration: - rclpy.spin_once(self, timeout_sec=0.1) # Allow callbacks while waiting - - def try_find_red_pole(self, p1: Odometry, p2: Odometry): - pose1 = [ - p1.pose.pose.position.x, - p1.pose.pose.position.y, - p1.pose.pose.position.z, - ] - - # Get Euler angles (roll, pitch, yaw) - euler_angles1 = euler_from_quaternion( - [ - p1.pose.pose.orientation.x, - p1.pose.pose.orientation.y, - p1.pose.pose.orientation.z, - p1.pose.pose.orientation.w, - ], - ) - - # Convert yaw angle to direction vector (x,y,z) - yaw1 = euler_angles1[2] # The third element is yaw - dir1 = np.array([np.cos(yaw1), np.sin(yaw1), 0.0]) - - pose2 = [ - p2.pose.pose.position.x, - p2.pose.pose.position.y, - p2.pose.pose.position.z, - ] - - # Get Euler angles (roll, pitch, yaw) - euler_angles2 = euler_from_quaternion( - [ - p2.pose.pose.orientation.x, - p2.pose.pose.orientation.y, - p2.pose.pose.orientation.z, - p2.pose.pose.orientation.w, - ], - ) - - # Convert yaw angle to direction vector (x,y,z) - yaw2 = euler_angles2[2] # The third element is yaw - dir2 = np.array([np.cos(yaw2), np.sin(yaw2), 0.0]) - - return closest_points_between_lines(pose1, dir1, pose2, dir2) - - def execute_callback(self, goal_handle): - self.get_logger().warn("spot 1") - self.spot_and_center_red_pole() - - goal = YawTracker.Goal() - goal.topic_name = "centroids/red_pole" - self.yaw_tracker_client.send_goal_and_block_until_done(goal) - - looking_at_red_pole1 = self.recent_odom - - self.get_logger().warn("move") - # move forward 0.5 and right by 1 - goal = Move.Goal() - goal.type = "Relative" - goal.goal_pose = Pose() - goal.goal_pose.orientation.w = 1.0 - goal.goal_pose.position.x = 0.5 - goal.goal_pose.position.y = -1.0 - self.move_client.send_goal_and_block_until_done(goal) - - self.get_logger().warn("spot 2") - self.spot_and_center_red_pole() - - goal = YawTracker.Goal() - goal.topic_name = "centroids/red_pole" - self.yaw_tracker_client.send_goal_and_block_until_done(goal) - - looking_at_red_pole2 = self.recent_odom - - red_pole_pose = self.try_find_red_pole( - looking_at_red_pole1, - looking_at_red_pole2, - ) - - self.get_logger().warn(f"red pole is at {red_pole_pose}") - - goal = Move.Goal() - goal.goal_pose.position.x = red_pole_pose[0] - goal.goal_pose.position.y = red_pole_pose[1] - 0.2 - goal.goal_pose.orientation.w = 1.0 - self.move_client.send_goal_and_block_until_done(goal) - - goal_handle.succeed() - result = NavChannel.Result() - result.success = True - result.message = "NavChannel complete" - return result - - -def main(args=None): - rclpy.init(args=args) - node = NavChannelServer() - executor = MultiThreadedExecutor() - executor.add_node(node) - executor.spin() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/navigate_around_server.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/navigate_around_server.py deleted file mode 100644 index a640e61b..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/navigate_around_server.py +++ /dev/null @@ -1,171 +0,0 @@ -import math - -import rclpy -from geometry_msgs.msg import Pose -from nav_msgs.msg import Odometry -from rclpy.action import ActionServer, CancelResponse, GoalResponse -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node -from std_msgs.msg import String -from subjugator_msgs.action import NavigateAround - - -class NavigateAroundObjectServer(Node): - def __init__(self): - super().__init__("navigatearound") - - # Action server - self._action_server = ActionServer( - self, - NavigateAround, - "navigatearound", - execute_callback=self.execute_callback, - goal_callback=self.goal_callback, - cancel_callback=self.cancel_callback, - ) - - # Subscribers - self.create_subscription( - String, - "/detected_objects", - self.perception_callback, - 10, - ) - self.create_subscription(Odometry, "/odometry/filtered", self.odom_callback, 10) - - # Publisher for goal poses - self.goal_pub = self.create_publisher(Pose, "/goal_pose", 10) - - # initialize pose - self.current_pose = Pose() - - # Called when a goal is received. Determines if using vision or dead-reckoning to orbit target - def goal_callback(self, goal_request): - self.distance_to_orbit = goal_request.radius - if goal_request.object == "None": - self.get_logger().info( - f"Received no object to orbit. Goal set to orbit about point ahead of current pose by {goal_request.radius}", - ) - self.use_vision = False - else: - self.get_logger().info( - f"Received goal to navigate around {goal_request.object} at a distance of {goal_request.radius}", - ) - self.use_vision = True - return GoalResponse.ACCEPT - - def cancel_callback(self, goal_handle): - self.get_logger().info("Received cancel request") - return CancelResponse.ACCEPT - - def perception_callback(self, msg): - # TODO depending on how we implement perception - pass - - # Gets current pose - def odom_callback(self, msg): - self.current_pose = msg.pose.pose - - def generate_poses(self, currentPose): - # Generate array of 4 poses that will complete an orbit around the target: - # 2 - # | - # | - # 3 - - Target - - 1 - # | - # | - # 0/4 - poses = [] - - # Find poses for completing orbit - - angle_increments = [45, 90, 135, 180, 225, 270, 315, 360] - - starting_z = currentPose.position.z - - for angle in angle_increments: - angle_rad = angle * math.pi / 180 - x_position = 2 * math.sin(angle_rad / 2) - y_position = -1 * math.sin(angle_rad) - pose = Pose() - pose.position.x = ( - currentPose.position.x + x_position * self.distance_to_orbit - ) - pose.position.y = ( - currentPose.position.y + y_position * self.distance_to_orbit - ) - pose.position.z = starting_z - - pose.orientation.w = 1.0 - poses.append(pose) - - return poses - - def check_at_goal_pose(self, currentPose, goalPose, acceptableDist=0.05): - x_dist = currentPose.position.x - goalPose.position.x - y_dist = currentPose.position.y - goalPose.position.y - z_dist = currentPose.position.z - goalPose.position.z - - distance_to_goal = math.sqrt(x_dist**2 + y_dist**2 + z_dist**2) - return distance_to_goal < acceptableDist - - def execute_callback(self, goal_handle): - - orbit_distance = goal_handle.request.radius - # If using vision to navigate around object, keep object at center of camera frame - if self.use_vision: - target_object = goal_handle.request.object - - self.get_logger().info( - f"Executing Navigate Around for: {target_object} at distance {orbit_distance} using vision", - ) - - # If using dead-reckoning to navigate around object, generate a set of goal poses, then follow them around the object. - else: - self.get_logger().info( - f"Executing Navigate Around at distance {orbit_distance} using dead-reckoning", - ) - - # Generate goal poses for orbit - goal_poses = self.generate_poses(self.current_pose) - - # Move to the poses - for pose in goal_poses: - self.goal_pub.publish(pose) - self.get_logger().info( - f"Published pose: x={pose.position.x:.2f}, y={pose.position.y:.2f},z={pose.position.z:.2f}", - ) - near_goal_pose = False - while not near_goal_pose: - near_goal_pose = self.check_at_goal_pose( - self.current_pose, - pose, - 0.2, - ) - - # slow down the loop - self.get_clock().sleep_for(rclpy.duration.Duration(seconds=0.05)) - self.get_logger().info("Arrived at goal pose!") - - # pause at each goal pose for 3 seconds - self.get_clock().sleep_for(rclpy.duration.Duration(seconds=3.0)) - self.get_logger().info("Completed orbit!") - - goal_handle.succeed() - result = NavigateAround.Result() - result.success = True - result.message = "Successfully navigated around object" - return result - - -def main(args=None): - rclpy.init(args=args) - node = NavigateAroundObjectServer() - executor = MultiThreadedExecutor() - executor.add_node(node) - executor.spin() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/navigation_channel_server.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/navigation_channel_server.py deleted file mode 100644 index 572ebfa1..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/navigation_channel_server.py +++ /dev/null @@ -1,505 +0,0 @@ -#!/usr/bin/env python3 -import math -from typing import List - -import rclpy -from geometry_msgs.msg import Pose -from mil_msgs.msg import PerceptionTargetArray -from rclpy.action import ActionClient, ActionServer, CancelResponse, GoalResponse -from rclpy.node import Node -from scipy.spatial.transform import Rotation as R -from subjugator_msgs.action import Move -from subjugator_msgs.action import NavigateChannel as NavigateChannelAction - -IMAGE_W = 960 -CX = IMAGE_W // 2 -HFOV_RAD = math.radians(50.0) - -SURGE_STEP = 0.40 # normal stride -SURGE_STEP_SMALL = 0.35 # closing on small poles -NO_POLES_SURGE = 2.00 # surge when no poles ever seen - -SEARCH_YAW_STEP = 0.35 -Kp_LIMIT = math.radians(50) - -MIN_AREA_CLOSE = 16000 # big pole threshold to avoid getting too close -MIN_AREA_DETECT = 400 # smallest we consider "real" -MIN_AREA_STEER = 1500 # minimum area to use pole for steering - -OFFSET_PX = 100 # INCREASED: lateral offset (pixels) for safer clearance -LOST_TIMEOUT = 4.0 # seconds before exit scan -LOOK_OFFSET_RAD = math.radians(20) -LOOK_DWELL_SEC = 0.3 # seconds to pause when scanning -HEADING_EPS = 0.05 # rad tolerance for yaw_to - -IDLE_SEC = 0.02 # loop breath - -# small-poles gains to keep turning even when far -SMALL_TAU_GAIN = 2.4 -MIN_YAW_SMALL = math.radians(10) # floor so it doesn't sit at ~0 - - -def px2yaw(px_err: float) -> float: - """+px means gap centre to right → need CW (-yaw).""" - return -px_err * HFOV_RAD / IMAGE_W - - -class NavigateChannelServer(Node): - def __init__(self) -> None: - super().__init__("navigation_channel_server") - - self._as = ActionServer( - self, - NavigateChannelAction, - "navigatechannel", - execute_callback=self._execute, - goal_callback=lambda _: GoalResponse.ACCEPT, - cancel_callback=lambda _: CancelResponse.ACCEPT, - ) - - self._move = ActionClient(self, Move, "move") - - self._detections: List = [] - self.create_subscription( - PerceptionTargetArray, - "/perception/targets", - lambda msg: setattr(self, "_detections", list(msg.targets)), - qos_profile=30, - ) - - self._ever_seen = False - self._ever_seen_steerable = False - self._lost_since = None - - async def _sleep(self, sec=IDLE_SEC): - fut = rclpy.task.Future() - self.create_timer(sec, lambda: fut.set_result(True)) - await fut - - async def move_rel(self, surge, yaw): - pose = Pose() - pose.position.x = surge - pose.position.y = 0.0 - pose.position.z = 0.0 - - # Create quaternion from yaw - q = R.from_euler("z", yaw).as_quat() - pose.orientation.x = q[0] - pose.orientation.y = q[1] - pose.orientation.z = q[2] - pose.orientation.w = q[3] - - goal = Move.Goal(type="Relative", goal_pose=pose) - self.get_logger().info( - f"Sending move command: surge={surge:.2f}m, yaw={yaw:.3f}rad", - ) - self.get_logger().debug( - f"Full pose: pos=({pose.position.x:.2f}, {pose.position.y:.2f}, {pose.position.z:.2f})", - ) - - gh = await self._move.send_goal_async(goal) - result = await gh.get_result_async() - if result and result.result.success: - self.get_logger().info("Move completed successfully") - return True - else: - self.get_logger().warning( - f"Move failed: {result.result.message if result else 'No result'}", - ) - return False - - # Look left and right for object, return true if found - async def inspection(self, inspection_rad, precision=3): - self.get_logger().info("Starting scanning area") - - step = inspection_rad / max(1, precision) - - # Look left first - self.get_logger().info("Looking left...") - for _ in range(precision): - success = await self.move_rel(0.0, step) - if success: - await self._sleep(LOOK_DWELL_SEC) - if self._small() or self._big(): - self.get_logger().info("Found poles on left side") - return True - - # Back to center - self.get_logger().info("TESTING1!!!!!") - await self.move_rel(0.0, -inspection_rad) - self.get_logger().info("TESTING2!!!!!") - - # Look right from center - self.get_logger().info("Looking right...") - for _ in range(precision): - - success = await self.move_rel(0.0, -step) - if success: - await self._sleep(LOOK_DWELL_SEC) - if self._small() or self._big(): - self.get_logger().info("Found poles on right side") - return True - - # Back to center - await self.move_rel(0.0, inspection_rad) - - self.get_logger().info("No poles found in inspection") - return False - - def _filter(self, label, min_area) -> List: - return [ - d - for d in self._detections - if d.label == label and d.width * d.height > min_area - ] - - def _big(self): - return self._filter("red-pole", MIN_AREA_CLOSE) + self._filter( - "white-pole", - MIN_AREA_CLOSE, - ) - - def _small(self): - return self._filter("red-pole", MIN_AREA_DETECT) + self._filter( - "white-pole", - MIN_AREA_DETECT, - ) - - def _log_areas(self): - if self._detections: - areas = ", ".join( - f"{d.label}:{d.width*d.height:.0f}" for d in self._detections - ) - self.get_logger().info(f"Pole areas → {areas}") - - def inRangeWhite(self, objectCenter, imageWidth=960, acceptanceWidth=6): - return objectCenter >= imageWidth / acceptanceWidth - - def inRangeRed(self, objectCenter, imageWidth=960, acceptanceWidth=6): - return objectCenter <= (acceptanceWidth - 1) * (imageWidth / acceptanceWidth) - - @staticmethod - def clamp01(x: float) -> float: - return 0.0 if x <= 0.0 else 1.0 if x >= 1.0 else x - - @staticmethod - def smoothstep01(t: float) -> float: - t = NavigateChannelServer.clamp01(t) - return t * t * (3.0 - 2.0 * t) - - def area_weight(self, area: float, a0: float = 400.0, a1: float = 8000.0) -> float: - t = (area - a0) / (a1 - a0) - t = max(0.0, min(1.0, t)) # Clamp to [0, 1] - # Exponential curve: rises quickly then levels off - weight = min(1.3, (1 - math.exp(-3 * t)) * 1.45) - self.get_logger().info(f"Area {area:.0f} → weight: {weight:.3f}") - return weight - - @staticmethod - def encroachment_weight( - label: str, - cx: float, - image_w: int = 960, - deadband_px: float = 0, - overshoot_gain: float = 1.2, - overshoot_exp: float = 1.6, - ) -> float: - cx_mid = image_w * 0.5 - half = image_w * 0.5 - - if label == "red-pole": - on_correct_side = cx >= cx_mid - dist_from_center = abs(cx - cx_mid) - else: - on_correct_side = cx <= cx_mid - dist_from_center = abs(cx - cx_mid) - - if on_correct_side: - # Linear response, not smoothstep! - norm_dist = dist_from_center / half - return 1.0 - (norm_dist * 0.5) # Only reduce by half at edge - else: - # Wrong side - stronger response - norm_dist = dist_from_center / half - return 1.0 + overshoot_gain * norm_dist - - async def final_inspection(self, inspection_rad, precision=3): - - self.get_logger().info("Starting FINAL inspection (will return to center)") - - step = inspection_rad / max(1, precision) - found_any = False - - # Look left - self.get_logger().info("Final inspection: Looking left...") - for i in range(precision): - success = await self.move_rel(0.0, step) - if success: - await self._sleep(LOOK_DWELL_SEC) - if (self._small() or self._big()) and not found_any: - found_any = True - self.get_logger().info("Final inspection: Found poles on left") - - # return to center from left - await self.move_rel(0.0, -inspection_rad) - - # Look right - self.get_logger().info("Final inspection: Looking right...") - for i in range(precision): - success = await self.move_rel(0.0, -step) - if success: - await self._sleep(LOOK_DWELL_SEC) - if (self._small() or self._big()) and not found_any: - found_any = True - self.get_logger().info("Final inspection: Found poles on right") - - # ALWAYS return to center from right - await self.move_rel(0.0, inspection_rad) - - self.get_logger().info(f"Final inspection complete. Found poles: {found_any}") - return found_any - - async def _execute(self, gh, absenceSurge=1.5, imageWidth=960): - if not self._move.wait_for_server(timeout_sec=5.0): - gh.abort() - return NavigateChannelAction.Result( - success=False, - message="move server unavailable", - ) - - self._ever_seen = False - self._lost_since = self.get_clock().now() - - while rclpy.ok(): - # current detections - real = [ - d for d in self._detections if d.width * d.height >= MIN_AREA_DETECT - ] - big = [d for d in real if d.width * d.height >= MIN_AREA_CLOSE] - steerable = [d for d in real if d.width * d.height >= MIN_AREA_STEER] - total = len(real) - self._log_areas() - - # Initial search pattern - keep going until we find poles - if not self._ever_seen and total == 0: - self.get_logger().info("=== SEARCH PATTERN: No poles ever seen ===") - self.get_logger().info(f"Moving forward {absenceSurge}m...") - await self.move_rel(absenceSurge, 0.0) - found = await self.inspection(math.radians(30)) - if found: - self._ever_seen = True - self._lost_since = self.get_clock().now() - self.get_logger().info("Poles found! Exiting search pattern") - else: - self.get_logger().info( - "No poles found, continuing search pattern...", - ) - continue - - # First sighting or re-sighting - if total > 0: - if not self._ever_seen: - self.get_logger().info("FIRST POLE SIGHTING!") - self._ever_seen = True - if len(steerable) > 0: - self._ever_seen_steerable = True - self._lost_since = self.get_clock().now() - - # Lost after seen, how we say the task is finished - if self._ever_seen_steerable and total == 0: - elapsed = (self.get_clock().now() - self._lost_since).nanoseconds * 1e-9 - self.get_logger().info( - f"Poles lost for {elapsed:.1f}s (timeout={LOST_TIMEOUT}s)", - ) - - if elapsed > LOST_TIMEOUT: - self.get_logger().info( - "Lost timeout reached, performing FINAL scan", - ) - - # First, move forward a bit to see if we just need to advance - await self.move_rel(0.5, 0.0) # Small forward move - await self._sleep(0.5) # Give time for detection - - # Check if we can see poles now - if ( - len( - [ - d - for d in self._detections - if d.width * d.height >= MIN_AREA_DETECT - ], - ) - > 0 - ): - self.get_logger().info( - "Found poles after moving forward, continuing navigation", - ) - self._lost_since = self.get_clock().now() - continue - - # If still no poles, final inspection time - found = await self.final_inspection(math.radians(30)) - - # Most likely means we have not completely passed through the channel - if found: - self.get_logger().info( - "Poles detected in final scan, pushing through straight", - ) - await self.move_rel(1.0, 0.0) - self._lost_since = self.get_clock().now() - else: - # We're done! - self.get_logger().info( - "No poles found in final scan, channel cleared!", - ) - gh.succeed() - return NavigateChannelAction.Result( - success=True, - message="channel cleared", - ) - else: - # Before timeout, just go straight (don't rotate looking for poles) - self.get_logger().info("Moving straight while waiting for timeout") - await self.move_rel(absenceSurge, 0.0) - continue - - # too close protection - if len(big) > 0: - max_pole = max(big, key=lambda d: d.width * d.height) - max_area = max_pole.width * max_pole.height - if max_area > 6000: - self.get_logger().warning( - f"VERY close to {max_pole.label} (area={max_area:.0f}), steering away", - ) - yaw_cmd = Kp_LIMIT if max_pole.label == "red-pole" else -Kp_LIMIT - await self.move_rel(0.0, yaw_cmd) - await self.move_rel(0.5, 0.0) - gh.publish_feedback( - NavigateChannelAction.Feedback( - distance_to_gap=float(max_pole.cx - CX), - ), - ) - continue - - # approach small poles (original behavior) - if not self._ever_seen_steerable and ( - len(steerable) == 0 and len(real) > 0 - ): - # For small poles, steer TOWARD them (center them), don't push to sides - weighted_cx_sum = 0.0 - weight_sum = 0.0 - - for d in real: - area = float(d.width * d.height) - # Simple weight based on area - bigger poles matter more - weight = area / MIN_AREA_DETECT - weighted_cx_sum += d.cx * weight - weight_sum += weight - - if weight_sum > 0: - # Find the weighted center of all poles - avg_cx = weighted_cx_sum / weight_sum - # Steer to center the poles - err_px = ( - avg_cx - CX - ) # Positive = poles to right, need to turn right - yaw_cmd = px2yaw(err_px) - - # Apply minimum yaw if needed - if abs(yaw_cmd) < MIN_YAW_SMALL: - yaw_cmd = math.copysign(MIN_YAW_SMALL, yaw_cmd) - yaw_cmd = max(-Kp_LIMIT, min(Kp_LIMIT, yaw_cmd)) - else: - yaw_cmd = 0.0 - - self.get_logger().info( - f"Approaching {len(real)} small poles: yaw={yaw_cmd:.3f}", - ) - await self.move_rel(0.0, yaw_cmd) - await self.move_rel(SURGE_STEP_SMALL, 0.0) - continue - - # τ-steering using largest detection - if len(steerable) == 0: - self.get_logger().info( - "No poles large enough for steering, moving straight", - ) - await self.move_rel(SURGE_STEP, 0.0) - continue - - # Find the largest pole - it's our primary reference - largest = max(steerable, key=lambda d: d.width * d.height) - max_area = largest.width * largest.height - - self.get_logger().info( - f"Using largest pole: {largest.label} at cx={largest.cx}, area={max_area:.0f}", - ) - - # Calculate weights - area = float(largest.width * largest.height) - w_a = self.area_weight(area, a0=MIN_AREA_STEER, a1=MIN_AREA_CLOSE) - w_c = self.encroachment_weight( - largest.label, - largest.cx, - image_w=IMAGE_W, - deadband_px=(0), - overshoot_gain=1.0, - overshoot_exp=0.9, - ) - tau = w_a * w_c - - # CORRECTED: Steer to keep poles on their proper sides - # Red poles belong on the RIGHT - always turn LEFT (positive yaw) - # The further left the pole, the stronger we turn left - # White poles belong on the LEFT - always turn RIGHT (negative yaw) - # The further right the pole, the stronger we turn right - err_px = CX - largest.cx if largest.label == "red-pole" else largest.cx - CX - # Ensure err_px is positive - - yaw_cmd = px2yaw(err_px) * tau - - # Apply limits - yaw_cmd = max(-Kp_LIMIT, min(Kp_LIMIT, yaw_cmd)) - - # Reduce forward speed when poles are large (we're close) - surge = SURGE_STEP if max_area <= MIN_AREA_CLOSE else SURGE_STEP_SMALL * 0.9 - - self.get_logger().info( - f"Steering based on largest {largest.label}: tau={tau:.2f}, yaw={yaw_cmd:.3f}, surge={surge:.2f}", - ) - await self.move_rel(0.0, yaw_cmd) - await self.move_rel(surge, 0.0) - gh.publish_feedback( - NavigateChannelAction.Feedback( - distance_to_gap=float(err_px if tau > 1e-6 else 0.0), - ), - ) - continue - - gh.abort() - return NavigateChannelAction.Result(success=False, message="node shutdown") - - -def main() -> None: - rclpy.init() - rclpy.spin(NavigateChannelServer()) - rclpy.shutdown() - - -if __name__ == "__main__": - main() - - -""" -- If we see clumpse of poles that are too far(small) go to the average point of all those poles -- Once we see poles that are above the threshold size stop and begin the nav channel logic - -- If we see red on our left we yaw right and go straight -- If we see white on our right we yaw left and go straight - - Amount of yaw could be a function of distance(area) and position on screen -- If we only see a red pole and it is on our right, go straight -- If we only see a white pole and it is on our left, go straight - - Need to make center range - - Each point in the range will need to take into account size of area -""" diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/sonar_follower_server.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/sonar_follower_server.py deleted file mode 100644 index 1b5041f2..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/sonar_follower_server.py +++ /dev/null @@ -1,174 +0,0 @@ -import math - -import rclpy -from geometry_msgs.msg import Pose -from mil_msgs.msg import ProcessedPing -from rclpy.action.client import ActionClient -from rclpy.action.server import ActionServer, CancelResponse, GoalResponse -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node -from subjugator_msgs.action import Move, SonarFollower - - -class ActionUser: - """ - class to make calling other missions easier, you do still kinda have to construct the goals on your own sry - """ - - def __init__(self, node: Node, action_type, action_name: str): - self.node = node - self.ac = ActionClient(node, action_type, action_name) - - # 0 timeout seconds implies no timeout - # TODO rn there is nothing for timeout_sec, would be a great first issue for someone :) (use self.node.get_clock().now()) - # _=0 should be timeout_sec = 0 - def send_goal_and_block_until_done(self, goal, _=0): - future = self.ac.send_goal_async(goal) # rn no feedback - running = True - while running: - rclpy.spin_once(self.node, timeout_sec=0.1) - - if future.done(): - result = future.result() - result_future = result.get_result_async() - - while not result_future.done(): # TODO this could be better - rclpy.spin_once(self.node, timeout_sec=0.1) - running = False - - def send_goal_and_return_future(self, goal): - return self.ac.send_goal_async(goal) - - -class SonarFollowerNode(Node): - def __init__(self): - super().__init__("sonarfollower") - - # Action server - self._action_server = ActionServer( - self, - SonarFollower, - "sonarfollower", - execute_callback=self.execute_callback, - goal_callback=self.goal_callback, - cancel_callback=self.cancel_callback, - ) - - self.move_client = ActionUser(self, Move, "move") - - self.ping_sub = self.create_subscription( - ProcessedPing, - "hydrophones/solved", - self.ping_cb, - 10, - ) - self.last_ping = ProcessedPing() - self.heard_ping = False - - def sleep_for(self, time: float): - start_time = self.get_clock().now() - duration = rclpy.duration.Duration(seconds=time) - - while (self.get_clock().now() - start_time) < duration: - rclpy.spin_once(self, timeout_sec=0.1) # Allow callbacks while waiting - - def ping_cb(self, msg: ProcessedPing): - self.last_ping = msg - self.heard_ping = True - - def goal_callback(self, goal_request): - self.get_logger().info("goal to follow sonar") - return GoalResponse.ACCEPT - - def cancel_callback(self, _): - self.get_logger().info("Received cancel request") - return CancelResponse.ACCEPT - - def compare_pings(self, p1: ProcessedPing | None, p2: ProcessedPing): - if p1 is None: - return False - - # normalize both - x1 = float(p1.origin_direction_body.x) - y1 = float(p1.origin_direction_body.y) - p1_mag = math.sqrt(x1 * x1 + y1 * y1) - x1 = x1 / p1_mag - y1 = y1 / p1_mag - - x2 = float(p2.origin_direction_body.x) - y2 = float(p2.origin_direction_body.y) - p2_mag = math.sqrt(x2 * x2 + y2 * y2) - x2 = x2 / p2_mag - y2 = y2 / p2_mag - - # dot product - dot_prodcut = ( - x1 * x2 + y1 * y2 - ) # this IS the cos of the angle between them (so unitless) - - # inverse cos - angle_rad = math.acos(dot_prodcut) # in radians!! - angle_degrees = angle_rad * (180 / math.pi) - - # check angle - return angle_degrees > 100 - - def execute_callback(self, goal_handle): - passed_pinger = False - previous_ping = None - while not passed_pinger: - while not self.heard_ping: - self.sleep_for(0.5) - self.heard_ping = False # this is lowk stupid TODO - - # check and see if the current ping is pointing the opposite direction from the past ping - passed_pinger = self.compare_pings(previous_ping, self.last_ping) - if passed_pinger: - break - - # just move towards it no rotation - x = self.last_ping.origin_direction_body.x - y = self.last_ping.origin_direction_body.y - _ = self.last_ping.origin_direction_body.z - - p = Pose() - p.orientation.w = 1.0 - p.position.x = x - p.position.y = y - goal = Move.Goal() - goal.type = "Relative" - goal.goal_pose = p - self.move_client.send_goal_and_block_until_done(goal) - self.sleep_for(0.5) - previous_ping = self.last_ping - - # move to surface - p = Pose() - p.orientation.w = 1.0 - p.position.x = 0.0 - p.position.y = 0.0 - p.position.z = 0.7 # todo this is dependent on how far down we moved :)) - goal = Move.Goal() - goal.type = "Relative" - goal.goal_pose = p - self.move_client.send_goal_and_block_until_done(goal) - self.sleep_for(0.5) - - goal_handle.succeed() - result = SonarFollower.Result() - result.success = True - result.message = "Wait complete" - return result - - -def main(args=None): - rclpy.init(args=args) - node = SonarFollowerNode() - executor = MultiThreadedExecutor() - executor.add_node(node) - executor.spin() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/start_gate_server.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/start_gate_server.py deleted file mode 100644 index 8919c443..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/start_gate_server.py +++ /dev/null @@ -1,144 +0,0 @@ -import math -import time - -import rclpy -from geometry_msgs.msg import Pose -from nav_msgs.msg import Odometry -from rclpy.action import ActionServer, CancelResponse, GoalResponse -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node -from subjugator_msgs.action import StartGate - - -class StartGateServer(Node): - def __init__(self): - super().__init__("startgate") - - # Action server - self._action_server = ActionServer( - self, - StartGate, - "startgate", - execute_callback=self.execute_callback, - goal_callback=self.goal_callback, - cancel_callback=self.cancel_callback, - ) - - # Subscribers - - self.create_subscription(Odometry, "/odometry/filtered", self.odom_callback, 10) - - # Publisher for goal poses - self.goal_pub = self.create_publisher(Pose, "/goal_pose", 10) - - # initialize pose - self.current_pose = Pose() - - # Called when a goal is received. Parses parameters (from yaml) from mission planner - def goal_callback(self, goal_request): - self.total_dist = goal_request.distance - self.numYaws = goal_request.num_yaws - self.numRolls = goal_request.num_rolls - self.numPitch = goal_request.num_pitch - - self.get_logger().info( - f"Received goal to move through start gate with {self.numYaws} yaws, {self.numRolls} rolls, and {self.numPitch} pitches", - ) - - return GoalResponse.ACCEPT - - def cancel_callback(self, goal_handle): - self.get_logger().info("Received cancel request") - return CancelResponse.ACCEPT - - # Gets current pose - def odom_callback(self, msg): - self.current_pose = msg.pose.pose - - def check_at_goal_pose(self, currentPose, goalPose, acceptableDist=0.05): - x_dist = currentPose.position.x - goalPose.position.x - y_dist = currentPose.position.y - goalPose.position.y - z_dist = currentPose.position.z - goalPose.position.z - - i_dist = currentPose.orientation.x - goalPose.orientation.x - j_dist = currentPose.orientation.y - goalPose.orientation.y - k_dist = currentPose.orientation.z - goalPose.orientation.z - w_dist = currentPose.orientation.w - goalPose.orientation.w - - distance_to_goal = math.sqrt(x_dist**2 + y_dist**2 + z_dist**2) - orientation_to_goal = math.sqrt(i_dist**2 + j_dist**2 + k_dist**2 + w_dist**2) - return distance_to_goal < acceptableDist and orientation_to_goal < 0.1 - - def execute_callback(self, goal_handle): - self.get_logger().info( - "Executing move through start gate", - ) - - # Align with start gate - - # TODO with vision - - # After aligning with start gate, use params from yaml to determine how far to go and how to r/p/y - - dist_per_rotation = self.total_dist / ( - self.numPitch + self.numRolls + self.numYaws - ) - - # Generate a set of poses to accomplish the desired number of - - goal_poses = [] - - # Generate the goal poses for yawing the sub successive 90 degrees - for yaw in range(self.numPitch): - pose = Pose() - pose.position.x = dist_per_rotation * yaw - pose.position.y = 0 - pose.position.z = 0 - - pose.orientation.x = 0 - pose.orientation.y = 0 - pose.orientation.z = math.cos((yaw + 1) * math.pi / 4 - math.pi / 2) - pose.orientation.w = math.sin((yaw + 1) * math.pi / 4 - math.pi / 2) - - goal_poses.append(pose) - - print(goal_poses) - for goal_pose in goal_poses: - self.goal_pub.publish(goal_pose) - - self.get_logger().info(f"Sending goal pose {goal_pose}") - self.goal_pub.publish(goal_pose) - - near_goal_pose = False - while not near_goal_pose: - near_goal_pose = self.check_at_goal_pose( - self.current_pose, - goal_pose, - 0.2, - ) - - # slow down the loop and give update odom callback - self.get_clock().sleep_for(rclpy.duration.Duration(seconds=0.05)) - self.get_logger().info("Arrived at goal pose!") - - self.get_logger().info("Completed start gate!") - - goal_handle.succeed() - result = StartGate.Result() - result.success = True - result.message = "Successfully moved through start gate (with style!)" - time.sleep(1.0) - return result - - -def main(args=None): - rclpy.init(args=args) - node = StartGateServer() - executor = MultiThreadedExecutor() - executor.add_node(node) - executor.spin() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/wait_server.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/wait_server.py deleted file mode 100644 index aa91d4b7..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/wait_server.py +++ /dev/null @@ -1,53 +0,0 @@ -import rclpy -from rclpy.action import ActionServer, CancelResponse, GoalResponse -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node -from subjugator_msgs.action import Wait - - -class WaitServer(Node): - def __init__(self): - super().__init__("wait_server") - - # Action server - self._action_server = ActionServer( - self, - Wait, - "wait", - execute_callback=self.execute_callback, - goal_callback=self.goal_callback, - cancel_callback=self.cancel_callback, - ) - - def goal_callback(self, goal_request): - self.wait_time = goal_request.time - self.get_logger().info(f"Received goal to wait for {self.wait_time} seconds") - return GoalResponse.ACCEPT - - def cancel_callback(self, goal_handle): - self.get_logger().info("Received cancel request") - return CancelResponse.ACCEPT - - def execute_callback(self, goal_handle): - - self.get_clock().sleep_for(rclpy.duration.Duration(seconds=self.wait_time)) - self.get_logger().info("Timer complete!") - - goal_handle.succeed() - result = Wait.Result() - result.success = True - result.message = "Wait complete" - return result - - -def main(args=None): - rclpy.init(args=args) - node = WaitServer() - executor = MultiThreadedExecutor() - executor.add_node(node) - executor.spin() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/yaw_tracker_server.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/yaw_tracker_server.py deleted file mode 100644 index e8cf475d..00000000 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/yaw_tracker_server.py +++ /dev/null @@ -1,144 +0,0 @@ -import time - -import rclpy -from geometry_msgs.msg import Pose -from nav_msgs.msg import Odometry -from rclpy.action.server import ActionServer, CancelResponse, GoalResponse -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node -from rclpy.subscription import Subscription -from subjugator_msgs.action import YawTracker -from tf_transformations import euler_from_quaternion, quaternion_from_euler -from yolo_msgs.msg import Detection - -IMAGE_WIDTH = 840 -IMAGE_HEIGHT = 680 - - -class YawTrackerServer(Node): - def __init__(self): - super().__init__("yawtracker") - - self.spotted: bool = False - - self.odom_sub_ = self.create_subscription( - Odometry, - "odometry/filtered", - self.odom_cb, - 10, - ) - self.last_odom: Odometry = Odometry() - - self.centroid_sub_: Subscription - self.last_detection: Detection = Detection() - - self.goal_pose_pub = self.create_publisher(Pose, "goal_pose", 10) - - # Action server - self._action_server = ActionServer( - self, - YawTracker, - "yawtracker", - execute_callback=self.execute_callback, - goal_callback=self.goal_callback, - cancel_callback=self.cancel_callback, - ) - - def odom_cb(self, msg: Odometry): - self.last_odom = msg - - def centroid_cb(self, msg: Detection): - self.last_detection = msg - self.spotted = True - - def goal_callback(self, goal_request: YawTracker.Goal): - self.topic_name = goal_request.topic_name - self.spotted = False - self.detection_sub = self.create_subscription( - Detection, - self.topic_name, - self.centroid_cb, - 10, - ) - - self.get_logger().info(f"Told to try and center on {self.topic_name}") - return GoalResponse.ACCEPT - - def cancel_callback(self, _): - self.get_logger().info("Received cancel request") - self.detection_sub.destroy() - return CancelResponse.ACCEPT - - def execute_callback(self, goal_handle): - time.sleep(0.1) - current_x = self.last_odom.pose.pose.position.x - current_y = self.last_odom.pose.pose.position.y - while not self.control_loop(current_x, current_y): - time.sleep(1) - - # self.detection_sub.destroy() - - goal_handle.succeed() - result = YawTracker.Result() - result.success = True - result.message = "centered on object" - print("destroyed") - return result - - def control_loop(self, current_x: float, current_y: float) -> bool: - if not self.spotted: - return False - - x_error_pixels = IMAGE_WIDTH / 2 - self.last_detection.bbox.center.position.x - kp = -2.5 / (2 * IMAGE_WIDTH) - - if abs(x_error_pixels) < 50: - return True - - print("---------") - print("x error pixels: ", x_error_pixels) - yaw_command = -x_error_pixels * kp - print("yaw command: ", yaw_command) - print("---------") - - # Get current orientation from odometry - current_quat = self.last_odom.pose.pose.orientation - - # Convert quaternion to Euler angles - (roll, pitch, yaw) = euler_from_quaternion( - [current_quat.x, current_quat.y, current_quat.z, current_quat.w], - ) - - # Apply yaw command - desired_yaw = yaw + yaw_command - - # Convert back to quaternion - quat = quaternion_from_euler(roll, pitch, desired_yaw) - - # Create goal pose - goal_pose = Pose() - goal_pose.position.x = current_x - goal_pose.position.y = current_y - goal_pose.position.z = -0.1 # TODO add current z too :O - goal_pose.orientation.x = quat[0] - goal_pose.orientation.y = quat[1] - goal_pose.orientation.z = quat[2] - goal_pose.orientation.w = quat[3] - - # Publish the goal pose - self.goal_pose_pub.publish(goal_pose) - self.spotted = False - return False - - -def main(args=None): - rclpy.init(args=args) - node = YawTrackerServer() - executor = MultiThreadedExecutor() - executor.add_node(node) - executor.spin() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/subjugator/testing/centroid_yaw_tracker/centroid_yaw_tracker/__init__.py b/src/subjugator/testing/centroid_yaw_tracker/centroid_yaw_tracker/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subjugator/testing/centroid_yaw_tracker/centroid_yaw_tracker/centroid_yaw_tracker_node.py b/src/subjugator/testing/centroid_yaw_tracker/centroid_yaw_tracker/centroid_yaw_tracker_node.py deleted file mode 100644 index b417c59e..00000000 --- a/src/subjugator/testing/centroid_yaw_tracker/centroid_yaw_tracker/centroid_yaw_tracker_node.py +++ /dev/null @@ -1,94 +0,0 @@ -import rclpy -from geometry_msgs.msg import Pose -from nav_msgs.msg import Odometry -from rclpy.node import Node -from tf_transformations import euler_from_quaternion, quaternion_from_euler -from yolo_msgs.msg import Detection - -IMAGE_WIDTH = 840 -IMAGE_HEIGHT = 680 - - -class CentroidYawTracker(Node): - def __init__(self): - super().__init__("centroid_yaw_tracker") - self.spotted: bool = False - - self.odom_sub_ = self.create_subscription( - Odometry, - "odometry/filtered", - self.odom_cb, - 10, - ) - self.last_odom: Odometry = Odometry() - - self.centroid_sub_ = self.create_subscription( - Detection, - "centroids/Green", - self.centroid_cb, - 10, - ) - self.last_detection: Detection = Detection() - - self.goal_pose_pub = self.create_publisher(Pose, "goal_pose", 10) - - self.timer = self.create_timer(1, self.control_loop) - - def odom_cb(self, msg: Odometry): - self.last_odom = msg - - def centroid_cb(self, msg: Detection): - self.last_detection = msg - self.spotted = True - - def control_loop(self): - if not self.spotted: - return - - x_error_pixels = IMAGE_WIDTH / 2 - self.last_detection.bbox.center.position.x - kp = -2.5 / (2 * IMAGE_WIDTH) - - print("---------") - print("x error pixels: ", x_error_pixels) - yaw_command = -x_error_pixels * kp - print("yaw command: ", yaw_command) - print("---------") - - # Get current orientation from odometry - current_quat = self.last_odom.pose.pose.orientation - - # Convert quaternion to Euler angles - (roll, pitch, yaw) = euler_from_quaternion( - [current_quat.x, current_quat.y, current_quat.z, current_quat.w], - ) - - # Apply yaw command - desired_yaw = yaw + yaw_command - - # Convert back to quaternion - quat = quaternion_from_euler(roll, pitch, desired_yaw) - - # Create goal pose - goal_pose = Pose() - goal_pose.position.x = 0.0 - goal_pose.position.y = 0.0 - goal_pose.position.z = -0.1 - goal_pose.orientation.x = quat[0] - goal_pose.orientation.y = quat[1] - goal_pose.orientation.z = quat[2] - goal_pose.orientation.w = quat[3] - - # Publish the goal pose - self.goal_pose_pub.publish(goal_pose) - self.spotted = False - - -def main(): - rclpy.init() - node = CentroidYawTracker() - rclpy.spin(node) - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/subjugator/testing/centroid_yaw_tracker/package.xml b/src/subjugator/testing/centroid_yaw_tracker/package.xml deleted file mode 100644 index 3ada2fa3..00000000 --- a/src/subjugator/testing/centroid_yaw_tracker/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - centroid_yaw_tracker - 0.0.0 - TODO: Package description - sub9 - TODO: License declaration - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/src/subjugator/testing/centroid_yaw_tracker/resource/centroid_yaw_tracker b/src/subjugator/testing/centroid_yaw_tracker/resource/centroid_yaw_tracker deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subjugator/testing/centroid_yaw_tracker/setup.cfg b/src/subjugator/testing/centroid_yaw_tracker/setup.cfg deleted file mode 100644 index 7a6566d7..00000000 --- a/src/subjugator/testing/centroid_yaw_tracker/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/centroid_yaw_tracker -[install] -install_scripts=$base/lib/centroid_yaw_tracker diff --git a/src/subjugator/testing/centroid_yaw_tracker/setup.py b/src/subjugator/testing/centroid_yaw_tracker/setup.py deleted file mode 100644 index 30a070b0..00000000 --- a/src/subjugator/testing/centroid_yaw_tracker/setup.py +++ /dev/null @@ -1,25 +0,0 @@ -from setuptools import find_packages, setup - -package_name = "centroid_yaw_tracker" - -setup( - name=package_name, - version="0.0.0", - packages=find_packages(exclude=["test"]), - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="sub9", - maintainer_email="jgoodman1@ufl.edu", - description="TODO: Package description", - license="TODO: License declaration", - extras_require={"test": ["pytest"]}, - entry_points={ - "console_scripts": [ - "centroid_yaw_tracker = centroid_yaw_tracker.centroid_yaw_tracker_node:main", - ], - }, -) diff --git a/src/subjugator/testing/nav_channel/nav_channel/__init__.py b/src/subjugator/testing/nav_channel/nav_channel/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subjugator/testing/nav_channel/nav_channel/nav_channel.py b/src/subjugator/testing/nav_channel/nav_channel/nav_channel.py deleted file mode 100644 index 68fc880f..00000000 --- a/src/subjugator/testing/nav_channel/nav_channel/nav_channel.py +++ /dev/null @@ -1,268 +0,0 @@ -import math - -import numpy as np -import rclpy -from geometry_msgs.msg import Pose -from nav_msgs.msg import Odometry -from rclpy.node import Node -from scipy.spatial.transform import Rotation as R -from tf_transformations import euler_from_quaternion, quaternion_from_euler -from yolo_msgs.msg import Detection - -IMAGE_WIDTH = 840 -IMAGE_HEIGHT = 680 - - -class NavChannel(Node): - def __init__(self): - super().__init__("nav_channel") - - # centroid cb - self._detection_sub = self.create_subscription( - Detection, - "centroids/Green", - self.detection_cb, - 10, - ) - self.recent_detection: Detection = Detection() - self.detection_cb_counter = 0 # for detecting if we are seeing something rn - self.spotted = False - - # odom cb - self._odom_cb = self.create_subscription( - Odometry, - "odometry/filtered", - self.odom_cb, - 10, - ) - self.recent_odom = Odometry() - - # goal pub - self.goal_pub = self.create_publisher(Pose, "goal_pose", 10) - - def detection_cb(self, msg: Detection): - self.recent_detection = msg - self.detection_cb_counter += 1 - self.spotted = True - - def odom_cb(self, msg: Odometry): - self.recent_odom = msg - - # generate a move in base_link, adam made this :) - def move_relative(self, movement_goal: Pose) -> Pose: - goal_pose = Pose() - - current_quat = [ - self.recent_odom.pose.pose.orientation.x, - self.recent_odom.pose.pose.orientation.y, - self.recent_odom.pose.pose.orientation.z, - self.recent_odom.pose.pose.orientation.w, - ] - current_rot = R.from_quat(current_quat) - - # Relative position from goal - rel_position = np.array( - [ - movement_goal.position.x, - movement_goal.position.y, - movement_goal.position.z, - ], - ) - - # Rotate relative position into world frame - rotated_position = current_rot.apply(rel_position) - - # Add to current position - goal_position = ( - np.array( - [ - self.recent_odom.pose.pose.position.x, - self.recent_odom.pose.pose.position.y, - self.recent_odom.pose.pose.position.z, - ], - ) - + rotated_position - ) - - # Relative orientation (as Rotation) - rel_quat = [ - movement_goal.orientation.x, - movement_goal.orientation.y, - movement_goal.orientation.z, - movement_goal.orientation.w, - ] - rel_rot = R.from_quat(rel_quat) - - # Compose rotations: world_rot * relative_rot - goal_rot = current_rot * rel_rot - goal_quat = goal_rot.as_quat(True) # [x, y, z, w] - - # Create absolute Pose - goal_pose = Pose() - goal_pose.position.x = goal_position[0] - goal_pose.position.y = goal_position[1] - goal_pose.position.z = goal_position[2] - goal_pose.orientation.x = goal_quat[0] - goal_pose.orientation.y = goal_quat[1] - goal_pose.orientation.z = goal_quat[2] - goal_pose.orientation.w = goal_quat[3] - - return goal_pose - - def control_loop(self): - if not self.spotted: - return - - x_error_pixels = IMAGE_WIDTH / 2 - self.recent_detection.bbox.center.position.x - kp = -2.5 / (2 * IMAGE_WIDTH) - - print("---------") - print("x error pixels: ", x_error_pixels) - yaw_command = -x_error_pixels * kp - print("yaw command: ", yaw_command) - print("---------") - - # Get current orientation from odometry - current_quat = self.recent_odom.pose.pose.orientation - - # Convert quaternion to Euler angles - (roll, pitch, yaw) = euler_from_quaternion( - [current_quat.x, current_quat.y, current_quat.z, current_quat.w], - ) - - # Apply yaw command - desired_yaw = yaw + yaw_command - - # Convert back to quaternion - quat = quaternion_from_euler(roll, pitch, desired_yaw) - - # Create goal pose - goal_pose = Pose() - goal_pose.position.x = 0.0 - goal_pose.position.y = 0.0 - goal_pose.position.z = -0.1 - goal_pose.orientation.x = quat[0] - goal_pose.orientation.y = quat[1] - goal_pose.orientation.z = quat[2] - goal_pose.orientation.w = quat[3] - - # Publish the goal pose - self.goal_pub.publish(goal_pose) - self.spotted = False - - def send_goal_and_wait(self, goal: Pose): - self.goal_pub.publish(goal) - position_tolerance = 0.2 # in meters - orientation_tolerance = 0.1 # in radians - - while True: - rclpy.spin_once(self, timeout_sec=0.25) - # Calculate position error - current_pos = self.recent_odom.pose.pose.position - dx = current_pos.x - goal.position.x - dy = current_pos.y - goal.position.y - dz = current_pos.z - goal.position.z - position_error = math.sqrt(dx * dx + dy * dy + dz * dz) - - # Calculate orientation error using euler angles - current_q = self.recent_odom.pose.pose.orientation - goal_q = goal.orientation - # Convert quaternions to euler angles (roll, pitch, yaw) - current_euler = euler_from_quaternion( - [current_q.x, current_q.y, current_q.z, current_q.w], - ) - goal_euler = euler_from_quaternion([goal_q.x, goal_q.y, goal_q.z, goal_q.w]) - - roll_error = abs(current_euler[0] - goal_euler[0]) - pitch_error = abs(current_euler[1] - goal_euler[1]) - yaw_error = abs(current_euler[2] - goal_euler[2]) - - # Handle angle wrapping for all angles - roll_error = min(roll_error, 2 * math.pi - roll_error) - pitch_error = min(pitch_error, 2 * math.pi - pitch_error) - yaw_error = min(yaw_error, 2 * math.pi - yaw_error) - - # choose max - orientation_error = max(roll_error, pitch_error, yaw_error) - - # Check if close enough to goal - if ( - position_error < position_tolerance - and orientation_error < orientation_tolerance - ): - self.get_logger().info("Goal reached!") - break - - # -180 < degrees < 180 or ur gonna PMO - def take_current_odom_and_yaw_by_n_degrees(self, degrees: int) -> Pose: - yaw_request = Pose() - yaw_request.orientation.z = math.sin(degrees / 2) - yaw_request.orientation.w = math.cos(degrees / 2) - - return self.move_relative(yaw_request) - - def take_current_odom_and_move_in_plus_y(self, base_link_y_offset) -> Pose: - y_translate_request = Pose() - y_translate_request.orientation.w = 1 - y_translate_request.position.y = base_link_y_offset - - return self.move_relative(y_translate_request) - - def rotate_left_until_15_centroids(self): - self.detection_cb_counter = ( - 0 # reset counter, it will increase when red is in frame - ) - while True: - rclpy.spin_once(self, timeout_sec=0.1) - # rotate left 10 degrees - new_pose = self.take_current_odom_and_yaw_by_n_degrees(10) - self.send_goal_and_wait(new_pose) - if ( - self.detection_cb_counter > 15 - ): # implies we have 15 frames of red implies we are seeing pvc pipe - return - - def slow_forward_until_centroid(self): - self.detection_cb_counter = ( - 0 # reset counter, it will increase when red is in frame - ) - while True: - rclpy.spin_once(self, timeout_sec=0.1) - # rotate left 10 degrees - new_pose = self.take_current_odom_and_move_in_plus_y(0.45) - self.send_goal_and_wait(new_pose) - if ( - self.detection_cb_counter > 15 - ): # implies we have 15 frames of red implies we are seeing pvc pipe - return - - def do_nav_channel(self): - # turn until red in frame - self.rotate_left_until_15_centroids() - - # center on red - while True: - if self.control_loop(): - break - - # store current bearing (bearing where we are looking at the red pole - # looking_at_red_pole: Odometry = deepcopy(self.recent_odom) - - # turn left a little (but also yaw right by 90 - new_pose = self.take_current_odom_and_yaw_by_n_degrees(13 - 90) - self.send_goal_and_wait(new_pose) - - # slowly go straight until red in view - self.slow_forward_until_centroid() - - -def main(): - rclpy.init() - nc = NavChannel() - rclpy.spin_once(nc, timeout_sec=0.25) - nc.do_nav_channel() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/subjugator/testing/nav_channel/package.xml b/src/subjugator/testing/nav_channel/package.xml deleted file mode 100644 index d299b589..00000000 --- a/src/subjugator/testing/nav_channel/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - nav_channel - 0.0.0 - TODO: Package description - jh - TODO: License declaration - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/src/subjugator/testing/nav_channel/resource/nav_channel b/src/subjugator/testing/nav_channel/resource/nav_channel deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subjugator/testing/nav_channel/setup.cfg b/src/subjugator/testing/nav_channel/setup.cfg deleted file mode 100644 index 9095ea61..00000000 --- a/src/subjugator/testing/nav_channel/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/nav_channel -[install] -install_scripts=$base/lib/nav_channel diff --git a/src/subjugator/testing/nav_channel/setup.py b/src/subjugator/testing/nav_channel/setup.py deleted file mode 100644 index dd1a232a..00000000 --- a/src/subjugator/testing/nav_channel/setup.py +++ /dev/null @@ -1,23 +0,0 @@ -from setuptools import find_packages, setup - -package_name = "nav_channel" - -setup( - name=package_name, - version="0.0.0", - packages=find_packages(exclude=["test"]), - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="jh", - maintainer_email="nottellingu@gmail.com", - description="TODO: Package description", - license="TODO: License declaration", - extras_require={"test": ["pytest"]}, - entry_points={ - "console_scripts": ["nav_channel = nav_channel.nav_channel:main"], - }, -)