update with socket

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

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_<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,
-        -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();

aruco/ArucoDetector.h

@@ -7,7 +7,7 @@
 class ArucoDetector {
     std::vector<ArucoTag> 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<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> detectArucoTags(std::vector<ArucoTag> tags = {});

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,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<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 +157,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);

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<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:
+
+    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<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>& arucoTags);
+};

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<double>(0, 0), 2) + pow(robotPose.position.y + a.at<double>(1, 0), 2) + pow(robotPose.position.z + a.at<double>(2, 0), 2));
+    return sqrt(pow(robotPose->position.x + a.at<double>(0, 0), 2) + pow(robotPose->position.y + a.at<double>(1, 0), 2) + pow(robotPose->position.z + a.at<double>(2, 0), 2));
 }