add support of TCP client

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} )
+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} )

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->detector = cv::aruco::ArucoDetector(getPredefinedDictionary(cv::aruco::DICT_4X4_50), cv::aruco::DetectorParameters());
-    // this->dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
+    this->dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
-    // this->parameters = cv::aruco::DetectorParameters();
+    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_<double>(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,
+        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);
@@ -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<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
     std::vector<std::vector<cv::Point2f>> 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<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> 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<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
 
     std::sort(result.second.begin(), result.second.end(), [this](const std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>& a, const std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>& 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)

aruco/ArucoDetector.h

@@ -6,7 +6,7 @@
 class ArucoDetector {
     std::vector<ArucoTag> 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<cv::aruco::Dictionary> dictionary;
+    // cv::Ptr<cv::aruco::Dictionary> dictionary;
 
     // 4.8
-    // cv::aruco::Dictionary dictionary;
+    cv::aruco::Dictionary dictionary;
-    // cv::aruco::DetectorParameters parameters;
+    cv::aruco::DetectorParameters parameters;
-    // cv::aruco::ArucoDetector detector;
+    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(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();
 
@@ -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);
-
 };

arucoDetector.cpp

@@ -1,5 +1,7 @@
 #include "aruco/ArucoDetector.h"
+#include "tcp/MyClient.h"
 
+#include <QCoreApplication>
 #include <iostream>
 #include <thread>
 #include <atomic>
@@ -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);
+    auto whiteFlower = ArucoTag(36, "White_flower", 20, FLOWER);
-    whiteFlower.setFlowerObjectRepresentation();
+    //whiteFlower.setFlowerObjectRepresentation();
-    auto purpleFlower = ArucoTag(13, "Purple flower", 20, FLOWER);
+    auto purpleFlower = ArucoTag(13, "Purple_flower", 20, FLOWER);
-    purpleFlower.setFlowerObjectRepresentation();
+    //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<double>(0, 0);// + (distanceToPot * sin(rotationMatrix.at<double>(1, 0)));
+                const double distanceZFlower = matrix.first.at<double>(2, 0);// + (distanceToPot * cos(rotationMatrix.at<double>(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<double>(0, 0) << " z: " << matrix.first.at<double>(2, 0) << " " << std::endl;
+                const auto yaw = matrix.second.at<double>(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();
 }

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);

photo.cpp

@@ -0,0 +1,27 @@
+#include <thread>
+#include <opencv2/opencv.hpp>
+
+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<nbPhoto;i++){
+        std::cout<<i<<std::endl;
+        cv::Mat image;
+        cap >> image;
+        if(!image.empty()){
+            cv::imwrite("../calibration_images/"+std::to_string(i)+".jpg", image);
+
+            sleep(100);
+        }
+    }
+    cap.release();
+
+    return 0;
+}

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<float>(0, 0)) + " ";
+            res += std::to_string(pos.first.at<float>(1, 0)) + " ";
+            res += std::to_string(pos.first.at<float>(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<std::string> 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<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>& arucoTags)
+{
+    this->arucoTags = arucoTags;
+}

tcp/MyClient.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <TCPSocket/TCPClient.hpp>
+
+#include "../utils/utils.h"
+#include "../aruco/ArucoTag.h"
+
+class MyClient : public TCPClient {
+Type::RobotPose* robotPose;
+std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>> 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<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>& arucoTags);
+};

utils/utils.h

@@ -24,4 +24,38 @@ enum Team
     YELLOW
 };
 
-double distanceBetweenRobotAndTag(Type::RobotPose robotPose, const cv::Mat& a);
+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<std::string> split(const std::string& str, const std::string& delimiter)
+{
+    std::vector<std::string> 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;
+}