modelec/detection_pot
1
0
Fork 0
mirror of https://github.com/modelec/detection_pot.git synced 2026-01-19 00:57:28 +01:00
Code Issues Packages Projects Releases Wiki Activity

patch robot pos declaration

Browse Source
This commit is contained in:
ackimixs
2024-03-20 20:26:32 +01:00
parent 86e46274eb
commit 05d99d1c1b
2 changed files with 6 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
aruco/ArucoDetector.cpp
View File

@@ -182,7 +182,7 @@ std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
return result;
}
void ArucoDetector::flowerDetector(const ArucoTag& tag, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix, const Type::RobotPose& robotPose)
void ArucoDetector::flowerDetector(const ArucoTag& tag, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix, Type::RobotPose* robotPose)
{
constexpr double distanceToPot = 21;
@@ -190,15 +190,15 @@ void ArucoDetector::flowerDetector(const ArucoTag& tag, const cv::Mat& translati
const double distanceZFlower = translationMatrix.at<double>(2, 0);// + (distanceToPot * cos(rotationMatrix.at<double>(1, 0)));
const double distanceFlower = distanceBetweenRobotAndTag(robotPose, translationMatrix);
std::cout << tag.name << " Pos : x: " << distanceXFlower << " z: " << distanceZFlower << " " << "distance: " << distanceFlower << std::endl;
//std::cout << tag.name << " Pos : x: " << distanceXFlower << " z: " << distanceZFlower << " " << "distance: " << distanceFlower << std::endl;
}
void ArucoDetector::solarPanelDetector(const ArucoTag& tag, cv::Mat translationMatrix, const cv::Mat& rotationMatrix, const Type::RobotPose& robotPose)
void ArucoDetector::solarPanelDetector(const ArucoTag& tag, cv::Mat translationMatrix, const cv::Mat& rotationMatrix, Type::RobotPose* robotPose)
{
std::cout << tag.name << " Pos : x: " << translationMatrix.at<double>(0, 0) << " z: " << translationMatrix.at<double>(2, 0) << " " << std::endl;
const auto yaw = rotationMatrix.at<double>(2, 0);
const auto rotationBaseTable = (-yaw) + robotPose.theta;
const auto rotationBaseTable = (-yaw) + robotPose->theta;
std::cout << " Rotation: " << rotationBaseTable * (180 / CV_PI) << std::endl;

4
aruco/ArucoDetector.h
View File

@@ -42,8 +42,8 @@ public:
void addArucoTag(const ArucoTag& tag);
static void flowerDetector(const ArucoTag& type, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix, const Type::RobotPose& robotPose);
static void flowerDetector(const ArucoTag& type, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix, Type::RobotPose* robotPose);
static void solarPanelDetector(const ArucoTag& type, cv::Mat translationMatrix, const cv::Mat& rotationMatrix, const Type::RobotPose& robotPose);
static void solarPanelDetector(const ArucoTag& type, cv::Mat translationMatrix, const cv::Mat& rotationMatrix, Type::RobotPose* robotPose);
};