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