From 05d99d1c1b8e230df695da59d17db7677b0889c8 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 20 Mar 2024 20:26:32 +0100 Subject: [PATCH] patch robot pos declaration --- aruco/ArucoDetector.cpp | 8 ++++---- aruco/ArucoDetector.h | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/aruco/ArucoDetector.cpp b/aruco/ArucoDetector.cpp index 3b33d7b..8ecffa0 100644 --- a/aruco/ArucoDetector.cpp +++ b/aruco/ArucoDetector.cpp @@ -182,7 +182,7 @@ std::pair>>> 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(2, 0);// + (distanceToPot * cos(rotationMatrix.at(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(0, 0) << " z: " << translationMatrix.at(2, 0) << " " << std::endl; const auto yaw = rotationMatrix.at(2, 0); - const auto rotationBaseTable = (-yaw) + robotPose.theta; + const auto rotationBaseTable = (-yaw) + robotPose->theta; std::cout << " Rotation: " << rotationBaseTable * (180 / CV_PI) << std::endl; diff --git a/aruco/ArucoDetector.h b/aruco/ArucoDetector.h index 06bd364..4f5d00b 100644 --- a/aruco/ArucoDetector.h +++ b/aruco/ArucoDetector.h @@ -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); };