modelec/detection_pot
1
0
Fork 0
mirror of https://github.com/modelec/detection_pot.git synced 2026-01-18 16:47:33 +01:00
Code Issues Packages Projects Releases Wiki Activity

add support of TCP client

Browse Source
This commit is contained in:
ackimixs
2024-03-24 10:43:22 +01:00
parent 50b67eb2bf
commit 6a4f4e036b
9 changed files with 236 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
CMakeLists.txt
View File

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

29
aruco/ArucoDetector.cpp
View File

@@ -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_<double>(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<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)

15
aruco/ArucoDetector.h
View File

@@ -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::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);
};

68
arucoDetector.cpp
View File

@@ -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);
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<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();
}

4
calibration.cpp
View File

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

27
photo.cpp Normal file
View File

@@ -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;
}

48
tcp/MyClient.cpp Normal file
View File

@@ -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;
}

21
tcp/MyClient.h Normal file
View File

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

36
utils/utils.h
View File

@@ -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;
}