diff --git a/CMakeLists.txt b/CMakeLists.txt index 95fbaa2..d650eae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,10 +2,21 @@ cmake_minimum_required(VERSION 3.25) project(detection_pot VERSION 1.0.1) set(CMAKE_CXX_STANDARD 17) +set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTORCC ON) +set(CMAKE_AUTOUIC ON) find_package( OpenCV REQUIRED ) include_directories( ${OpenCV_INCLUDE_DIRS} ) +find_package(PkgConfig REQUIRED) +pkg_check_modules(TCPSocket REQUIRED TCPSocket) + +find_package(Qt6 COMPONENTS + Core + Network + REQUIRED) + set(COMMON_SOURCES utils/utils.h utils/utils.cpp @@ -28,8 +39,23 @@ set(arucoDetectionSources aruco/ArucoTag.h aruco/ArucoDetector.cpp aruco/ArucoDetector.h + tcp/MyClient.cpp + tcp/MyClient.h ) add_executable(arucoDetector ${arucoDetectionSources}) -target_link_libraries( arucoDetector ${OpenCV_LIBS} ) \ No newline at end of file +target_link_libraries( arucoDetector ${OpenCV_LIBS} ) + +target_link_libraries( arucoDetector Qt6::Core Qt6::Network ) + +target_link_libraries( arucoDetector TCPSocket ) + +set(photo + ${COMMON_SOURCES} + photo.cpp +) + +add_executable(photo ${photo}) + +target_link_libraries( photo ${OpenCV_LIBS} ) \ No newline at end of file diff --git a/aruco/ArucoDetector.cpp b/aruco/ArucoDetector.cpp index f93c68f..bf81b3d 100644 --- a/aruco/ArucoDetector.cpp +++ b/aruco/ArucoDetector.cpp @@ -1,20 +1,21 @@ #include "ArucoDetector.h" -ArucoDetector::ArucoDetector(const Type::RobotPose& pose, const std::string& calibrationPath, const Team team, const int cameraId, const bool headless) : robotPose(pose), headless(headless), team(team) +ArucoDetector::ArucoDetector(Type::RobotPose* pose, const std::string& calibrationPath, const Team team, const int cameraId, const bool headless) : robotPose(pose), headless(headless), team(team) { + // opencv 4.8 - // this->detector = cv::aruco::ArucoDetector(getPredefinedDictionary(cv::aruco::DICT_4X4_50), cv::aruco::DetectorParameters()); - // this->dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); - // this->parameters = cv::aruco::DetectorParameters(); + this->detector = cv::aruco::ArucoDetector(getPredefinedDictionary(cv::aruco::DICT_4X4_50), cv::aruco::DetectorParameters()); + this->dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); + this->parameters = cv::aruco::DetectorParameters(); // 4.6 - this->dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); + // this->dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); this->transformationMatrix = (cv::Mat_(4, 4) << - cos(pose.theta), 0, sin(pose.theta), pose.position.x, - 0, 1, 0, pose.position.y, - -sin(pose.theta), 0, cos(pose.theta), pose.position.z, + cos(pose->theta), 0, sin(pose->theta), pose->position.x, + 0, 1, 0, pose->position.y, + -sin(pose->theta), 0, cos(pose->theta), pose->position.z, 0, 0, 0, 1 ); this->readCameraParameters(calibrationPath); @@ -43,10 +44,6 @@ ArucoDetector::ArucoDetector(const Type::RobotPose& pose, const std::string& cal this->addArucoTag(ArucoTag(47, "Solar panel", 50, SOLAR_PANEL)); } -ArucoDetector::ArucoDetector(const float x, const float y, const float z, const float theta, const std::string& calibrationPath, const Team team, const int cameraId, const bool headless) : ArucoDetector(Type::RobotPose{cv::Point3f(x, y, z), theta}, calibrationPath, team, cameraId, headless) -{ -} - ArucoDetector::~ArucoDetector() { cap.release(); @@ -102,10 +99,10 @@ std::pair>>> Ar std::vector> markerCorners; // 4.6 - cv::aruco::detectMarkers(frame, this->dictionary, markerCorners, markerIds); + // cv::aruco::detectMarkers(frame, this->dictionary, markerCorners, markerIds); // opencv 4.8 - // detector.detectMarkers(frame, markerCorners, markerIds); + detector.detectMarkers(frame, markerCorners, markerIds); if (!markerIds.empty()) { @@ -127,7 +124,7 @@ std::pair>>> Ar cv::Mat rvec, tvec; - solvePnP(tag.objectRepresenation, markerCorners.at(i), cameraMatrix, distCoeffs, rvec, tvec); + solvePnP(tag.objectRepresenation, markerCorners.at(i), cameraMatrix, distCoeffs, rvec, tvec, false, cv::SOLVEPNP_IPPE_SQUARE); if (!headless) { @@ -159,7 +156,7 @@ std::pair>>> Ar std::sort(result.second.begin(), result.second.end(), [this](const std::pair>& a, const std::pair>& b) { - return distanceBetweenRobotAndTag(robotPose, a.second.first) < distanceBetweenRobotAndTag(robotPose, b.second.first); + return distanceBetweenRobotAndTag(*robotPose, a.second.first) < distanceBetweenRobotAndTag(*robotPose, b.second.first); }); if (!headless) diff --git a/aruco/ArucoDetector.h b/aruco/ArucoDetector.h index 3f05eeb..43ece4c 100644 --- a/aruco/ArucoDetector.h +++ b/aruco/ArucoDetector.h @@ -6,7 +6,7 @@ class ArucoDetector { std::vector arucoTags; - Type::RobotPose robotPose; + Type::RobotPose* robotPose; cv::Mat cameraMatrix; cv::Mat distCoeffs; @@ -14,12 +14,12 @@ class ArucoDetector { cv::VideoCapture cap; // 4.6 - cv::Ptr dictionary; + // cv::Ptr dictionary; // 4.8 - // cv::aruco::Dictionary dictionary; - // cv::aruco::DetectorParameters parameters; - // cv::aruco::ArucoDetector detector; + cv::aruco::Dictionary dictionary; + cv::aruco::DetectorParameters parameters; + cv::aruco::ArucoDetector detector; bool headless; @@ -30,9 +30,7 @@ class ArucoDetector { bool started = false; public: - ArucoDetector(const Type::RobotPose& pose, const std::string& calibrationPath, Team team, int cameraId = 0, bool headless = false); - - ArucoDetector(float x, float y, float z, float theta, const std::string& calibrationPath, Team team, int cameraId = 0, bool headless = false); + ArucoDetector(Type::RobotPose* pose, const std::string& calibrationPath, Team team, int cameraId = 0, bool headless = false); ~ArucoDetector(); @@ -45,5 +43,4 @@ public: static void flowerDetector(const ArucoTag& type, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix); static void solarPanelDetector(const ArucoTag& type, cv::Mat translationMatrix, const cv::Mat& rotationMatrix, const Type::RobotPose& robotPose); - }; diff --git a/arucoDetector.cpp b/arucoDetector.cpp index c719083..f0ff94a 100644 --- a/arucoDetector.cpp +++ b/arucoDetector.cpp @@ -1,5 +1,7 @@ #include "aruco/ArucoDetector.h" +#include "tcp/MyClient.h" +#include #include #include #include @@ -17,6 +19,7 @@ void userInputThread() { int main(int argc, char *argv[]) { // Settup argument parser + QCoreApplication app(argc, argv); bool headless = false; @@ -61,19 +64,32 @@ int main(int argc, char *argv[]) userInput = std::thread(userInputThread); } - const auto robotPose = Type::RobotPose{cv::Point3f(0, 0, 0), CV_PI/2}; + auto* robotPose = new Type::RobotPose{cv::Point3f(0, 0, 0), CV_PI/2}; ArucoDetector detector(robotPose, calibrationPath, BLUE, cameraId, headless); - auto whiteFlower = ArucoTag(36, "White flower", 20, FLOWER); - whiteFlower.setFlowerObjectRepresentation(); - auto purpleFlower = ArucoTag(13, "Purple flower", 20, FLOWER); - purpleFlower.setFlowerObjectRepresentation(); + auto whiteFlower = ArucoTag(36, "White_flower", 20, FLOWER); + //whiteFlower.setFlowerObjectRepresentation(); + auto purpleFlower = ArucoTag(13, "Purple_flower", 20, FLOWER); + //purpleFlower.setFlowerObjectRepresentation(); - auto solarPanel = ArucoTag(47, "Solar panel", 50, SOLAR_PANEL); + auto solarPanel = ArucoTag(47, "Solar_panel", 50, SOLAR_PANEL); + + int code; + + MyClient client(robotPose, "127.0.0.1", 8080); + + client.start(); + + client.sendMessage("request robotPose"); while (true) { - const auto [code, res] = detector.detectArucoTags({whiteFlower, purpleFlower, solarPanel}); + + auto r = detector.detectArucoTags({whiteFlower, purpleFlower, solarPanel}); + + code = r.first; + + client.setArucoTags(r.second); if (code == -2) { @@ -86,14 +102,44 @@ int main(int argc, char *argv[]) stopRequested = true; } - for (auto [tags, matrix] : res) + for (auto [tags, matrix] : r.second) { if (tags.type == FLOWER) { - ArucoDetector::flowerDetector(tags, matrix.first, matrix.first); + constexpr double distanceToPot = 21; + + const double distanceXFlower = matrix.first.at(0, 0);// + (distanceToPot * sin(rotationMatrix.at(1, 0))); + const double distanceZFlower = matrix.first.at(2, 0);// + (distanceToPot * cos(rotationMatrix.at(1, 0))); + + //std::cout << tags.name << " Pos : x: " << distanceXFlower << " z: " << distanceZFlower << " " << std::endl; + } else if (tags.type == SOLAR_PANEL) { - ArucoDetector::solarPanelDetector(tags, matrix.first, matrix.first, robotPose); + std::cout << tags.name << " Pos : x: " << matrix.first.at(0, 0) << " z: " << matrix.first.at(2, 0) << " " << std::endl; + const auto yaw = matrix.second.at(2, 0); + + const auto rotationBaseTable = (-yaw) + robotPose->theta; + + std::cout << " Rotation: " << rotationBaseTable * (180 / CV_PI) << std::endl; + + if (rotationBaseTable > 70 && rotationBaseTable <= 110) + { + std::cout << "Mid" << std::endl; + } + else if (rotationBaseTable > 30 && rotationBaseTable <= 70) + { + std::cout << "Blue side" << std::endl; + } + else if (rotationBaseTable > 110 && rotationBaseTable <= 150) + { + std::cout << "Yellow side" << std::endl; + } + else + { + std::cout << "Mid" << std::endl; + } + + // BLUE => 90, YELLOW => -90 } } @@ -109,5 +155,5 @@ int main(int argc, char *argv[]) userInput.value().join(); } - return 0; + return QCoreApplication::exec(); } \ No newline at end of file diff --git a/calibration.cpp b/calibration.cpp index a2e67a3..13deca2 100644 --- a/calibration.cpp +++ b/calibration.cpp @@ -20,7 +20,7 @@ int main(int argc, char *argv[]) } // Set the chessboard size (number of inner corners in width and height) - cv::Size chessboardSize(9, 6); + cv::Size chessboardSize(6, 9); cv::Size imgSize; @@ -63,7 +63,7 @@ int main(int argc, char *argv[]) if (findChessboardCorners(gray, chessboardSize, corners)) { // Refine corner locations cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1), - cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1)); + cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.001)); // Store object and image points objectPoints.push_back(worldPoints); diff --git a/photo.cpp b/photo.cpp new file mode 100644 index 0000000..f4ac9f9 --- /dev/null +++ b/photo.cpp @@ -0,0 +1,27 @@ +#include +#include + +void sleep(int ms){ + std::this_thread::sleep_for(std::chrono::milliseconds(ms)); +} + +int main() +{ + auto cap = cv::VideoCapture(0); + + int nbPhoto = 20; + + for(int i=0;i> image; + if(!image.empty()){ + cv::imwrite("../calibration_images/"+std::to_string(i)+".jpg", image); + + sleep(100); + } + } + cap.release(); + + return 0; +} \ No newline at end of file diff --git a/tcp/MyClient.cpp b/tcp/MyClient.cpp new file mode 100644 index 0000000..1cf7bf1 --- /dev/null +++ b/tcp/MyClient.cpp @@ -0,0 +1,48 @@ +#include "MyClient.h" + +MyClient::~MyClient() +{ + this->stop(); +} + +MyClient::MyClient(Type::RobotPose* robotPose, const char* ip, const int port) : TCPClient(ip, port), robotPose(robotPose) +{ + +} + +void MyClient::handleMessage(const std::string& message) +{ + std::cout << "Message From My Client" << std::endl; + std::cout << message << std::endl; + if (startWith(message, "request aruco")) + { + std::string res; + + for (auto& [tag, pos] : arucoTags) + { + res += std::to_string(tag.id) + " "; + res += std::to_string(pos.first.at(0, 0)) + " "; + res += std::to_string(pos.first.at(1, 0)) + " "; + res += std::to_string(pos.first.at(2, 0)) + " "; + } + + this->sendMessage(res.c_str()); + } else if (startWith(message, "ping aruco") || startWith(message, "ping all")) + { + this->sendMessage("pong aruco"); + } else if (startWith(message, "robotPose")) + { + // cut the string with space and take the first, second, third and fourth element + std::vector tokens = split(message, " "); + + robotPose->position.x = std::stof(tokens[1]); + robotPose->position.y = std::stof(tokens[2]); + robotPose->position.z = std::stof(tokens[3]); + robotPose->theta = std::stof(tokens[4]); + } +} + +void MyClient::setArucoTags(const std::vector>>& arucoTags) +{ + this->arucoTags = arucoTags; +} diff --git a/tcp/MyClient.h b/tcp/MyClient.h new file mode 100644 index 0000000..c3e2457 --- /dev/null +++ b/tcp/MyClient.h @@ -0,0 +1,21 @@ +#pragma once + +#include + +#include "../utils/utils.h" +#include "../aruco/ArucoTag.h" + +class MyClient : public TCPClient { +Type::RobotPose* robotPose; +std::vector>> arucoTags; + +public: + + ~MyClient() override; + + explicit MyClient(Type::RobotPose* robotPose, const char* ip = "127.0.0.1", int port = 8080); + + void handleMessage(const std::string& message) override; + + void setArucoTags(const std::vector>>& arucoTags); +}; diff --git a/utils/utils.h b/utils/utils.h index bb5f6f4..79c43f1 100644 --- a/utils/utils.h +++ b/utils/utils.h @@ -24,4 +24,38 @@ enum Team YELLOW }; -double distanceBetweenRobotAndTag(Type::RobotPose robotPose, const cv::Mat& a); \ No newline at end of file +double distanceBetweenRobotAndTag(Type::RobotPose robotPose, const cv::Mat& a); + +inline bool startWith(const std::string& str, const std::string& start) +{ + return str.rfind(start, 0) == 0; +} + +inline bool endsWith(const std::string& str, const std::string& end) +{ + if (str.length() >= end.length()) + { + return (0 == str.compare(str.length() - end.length(), end.length(), end)); + } + return false; +} + +inline bool contains(const std::string& str, const std::string& sub) +{ + return str.find(sub) != std::string::npos; +} + +inline std::vector split(const std::string& str, const std::string& delimiter) +{ + std::vector tokens; + size_t prev = 0, pos = 0; + do + { + pos = str.find(delimiter, prev); + if (pos == std::string::npos) pos = str.length(); + std::string token = str.substr(prev, pos - prev); + if (!token.empty()) tokens.push_back(token); + prev = pos + delimiter.length(); + } while (pos < str.length() && prev < str.length()); + return tokens; +} \ No newline at end of file