From 9bf5502d64f5a88b4109565734a79ebd6f525736 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 20 Mar 2024 20:20:00 +0100 Subject: [PATCH] update with socket --- CMakeLists.txt | 5 +++ aruco/ArucoDetector.cpp | 10 ++---- aruco/ArucoDetector.h | 4 +-- arucoDetector.cpp | 70 ++++++++++++++++++++++++++++++++++------- calibration.cpp | 4 +-- tcp/MyClient.cpp | 45 ++++++++++++++++++++++++++ tcp/MyClient.h | 21 +++++++++++++ utils/utils.cpp | 4 +-- 8 files changed, 138 insertions(+), 25 deletions(-) create mode 100644 tcp/MyClient.cpp create mode 100644 tcp/MyClient.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 3bb9cc1..594485f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,6 +6,7 @@ set(CMAKE_CXX_STANDARD 17) find_package( OpenCV REQUIRED ) find_package(PkgConfig REQUIRED) pkg_check_modules(LIBCAMERA REQUIRED libcamera) +pkg_check_modules(TCPSocket REQUIRED TCPSocket) include_directories(${LIBCAMERA_INCLUDE_DIRS} ${OPENCV_INCLUDE_DIRS}) @@ -30,12 +31,16 @@ 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 -llccv ${OpenCV_LIBS} ) +target_link_libraries( arucoDetector TCPSocket ) + set(photo ${COMMON_SOURCES} photo.cpp diff --git a/aruco/ArucoDetector.cpp b/aruco/ArucoDetector.cpp index 8a8cc25..3b33d7b 100644 --- a/aruco/ArucoDetector.cpp +++ b/aruco/ArucoDetector.cpp @@ -1,6 +1,6 @@ #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()); @@ -12,9 +12,9 @@ ArucoDetector::ArucoDetector(const Type::RobotPose& pose, const std::string& cal this->transformationMatrix = (cv::Mat_(4, 4) << - cos(pose.theta), 0, sin(pose.theta), pose.position.x, + 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, + -sin(pose->theta), 0, cos(pose->theta), pose->position.z, 0, 0, 0, 1 ); this->readCameraParameters(calibrationPath); @@ -51,10 +51,6 @@ ArucoDetector::ArucoDetector(const Type::RobotPose& pose, const std::string& cal cam->startVideo(); } -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() { cam->stopVideo(); diff --git a/aruco/ArucoDetector.h b/aruco/ArucoDetector.h index 2ed6104..06bd364 100644 --- a/aruco/ArucoDetector.h +++ b/aruco/ArucoDetector.h @@ -7,7 +7,7 @@ class ArucoDetector { std::vector arucoTags; - Type::RobotPose robotPose; + Type::RobotPose* robotPose; cv::Mat cameraMatrix; cv::Mat distCoeffs; @@ -34,8 +34,6 @@ class ArucoDetector { 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(); std::pair>>> detectArucoTags(std::vector tags = {}); diff --git a/arucoDetector.cpp b/arucoDetector.cpp index 81c04af..0878de1 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,34 @@ 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\n"); 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); + + std::cout << robotPose->position << std::endl; if (code == -2) { @@ -86,14 +104,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, robotPose); + 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 +157,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/tcp/MyClient.cpp b/tcp/MyClient.cpp new file mode 100644 index 0000000..c938778 --- /dev/null +++ b/tcp/MyClient.cpp @@ -0,0 +1,45 @@ +#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) + " "; + } + + this->sendMessage(res.c_str()); + } else if (startWith(message, "ping aruco")) + { + 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..5ccae38 --- /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: + + virtual ~MyClient(); + + 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.cpp b/utils/utils.cpp index 5439a73..fb1666b 100644 --- a/utils/utils.cpp +++ b/utils/utils.cpp @@ -1,6 +1,6 @@ #include "utils.h" -double distanceBetweenRobotAndTag(const Type::RobotPose robotPose, const cv::Mat& a) +double distanceBetweenRobotAndTag(Type::RobotPose* robotPose, const cv::Mat& a) { - return sqrt(pow(robotPose.position.x + a.at(0, 0), 2) + pow(robotPose.position.y + a.at(1, 0), 2) + pow(robotPose.position.z + a.at(2, 0), 2)); + return sqrt(pow(robotPose->position.x + a.at(0, 0), 2) + pow(robotPose->position.y + a.at(1, 0), 2) + pow(robotPose->position.z + a.at(2, 0), 2)); }