From 1a274edf44c169d9a9effdc745743d5e84bd9957 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Tue, 26 Sep 2023 17:44:48 -0400 Subject: [PATCH 001/197] add starter files for second cam tag detection node --- .../tag_detector/long_range_tag_detector.cpp | 156 ++++++++++++++++++ .../tag_detector/long_range_tag_detector.hpp | 45 +++++ .../long_range_tag_detector.processing.cpp | 21 +++ 3 files changed, 222 insertions(+) create mode 100644 src/perception/tag_detector/long_range_tag_detector.cpp create mode 100644 src/perception/tag_detector/long_range_tag_detector.hpp create mode 100644 src/perception/tag_detector/long_range_tag_detector.processing.cpp diff --git a/src/perception/tag_detector/long_range_tag_detector.cpp b/src/perception/tag_detector/long_range_tag_detector.cpp new file mode 100644 index 000000000..86ab376b4 --- /dev/null +++ b/src/perception/tag_detector/long_range_tag_detector.cpp @@ -0,0 +1,156 @@ +#include "tag_detector.hpp" + +namespace mrover { + + void TagDetectorNodelet::onInit() { + mNh = getMTNodeHandle(); + mPnh = getMTPrivateNodeHandle(); + mDetectorParams = new cv::aruco::DetectorParameters(); + auto defaultDetectorParams = cv::aruco::DetectorParameters::create(); + int dictionaryNumber; + + mPnh.param("publish_images", mPublishImages, true); + using DictEnumType = std::underlying_type_t; + mPnh.param("dictionary", dictionaryNumber, static_cast(cv::aruco::DICT_4X4_50)); + mPnh.param("min_hit_count_before_publish", mMinHitCountBeforePublish, 5); + mPnh.param("max_hit_count", mMaxHitCount, 5); + mPnh.param("tag_increment_weight", mTagIncrementWeight, 2); + mPnh.param("tag_decrement_weight", mTagDecrementWeight, 1); + + mIt.emplace(mNh); + mImgPub = mIt->advertise("long_range_tag_detection", 1); + mDictionary = cv::aruco::getPredefinedDictionary(dictionaryNumber); + + mPcSub = mNh.subscribe("zoom_cam/image", 1, &LongRangeTagDetectorNodelet::imageCallback, this); + mServiceEnableDetections = mNh.advertiseService("enable_detections", &TagDetectorNodelet::enableDetectionsCallback, this); + + // Lambda handles passing class pointer (implicit first parameter) to configCallback + mCallbackType = [this](mrover::DetectorParamsConfig& config, uint32_t level) { configCallback(config, level); }; + mConfigServer.setCallback(mCallbackType); + + mPnh.param("adaptiveThreshConstant", + mDetectorParams->adaptiveThreshConstant, defaultDetectorParams->adaptiveThreshConstant); + mPnh.param("adaptiveThreshWinSizeMax", + mDetectorParams->adaptiveThreshWinSizeMax, defaultDetectorParams->adaptiveThreshWinSizeMax); + mPnh.param("adaptiveThreshWinSizeMin", + mDetectorParams->adaptiveThreshWinSizeMin, defaultDetectorParams->adaptiveThreshWinSizeMin); + mPnh.param("adaptiveThreshWinSizeStep", + mDetectorParams->adaptiveThreshWinSizeStep, defaultDetectorParams->adaptiveThreshWinSizeStep); + mPnh.param("cornerRefinementMaxIterations", + mDetectorParams->cornerRefinementMaxIterations, + defaultDetectorParams->cornerRefinementMaxIterations); + mPnh.param("cornerRefinementMinAccuracy", + mDetectorParams->cornerRefinementMinAccuracy, + defaultDetectorParams->cornerRefinementMinAccuracy); + mPnh.param("cornerRefinementWinSize", + mDetectorParams->cornerRefinementWinSize, defaultDetectorParams->cornerRefinementWinSize); + + bool doCornerRefinement = true; + mPnh.param("doCornerRefinement", doCornerRefinement, true); + if (doCornerRefinement) { + bool cornerRefinementSubPix = true; + mPnh.param("cornerRefinementSubPix", cornerRefinementSubPix, true); + if (cornerRefinementSubPix) { + mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; + } else { + mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR; + } + } else { + mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; + } + + mPnh.param("errorCorrectionRate", + mDetectorParams->errorCorrectionRate, defaultDetectorParams->errorCorrectionRate); + mPnh.param("minCornerDistanceRate", + mDetectorParams->minCornerDistanceRate, defaultDetectorParams->minCornerDistanceRate); + mPnh.param("markerBorderBits", + mDetectorParams->markerBorderBits, defaultDetectorParams->markerBorderBits); + mPnh.param("maxErroneousBitsInBorderRate", + mDetectorParams->maxErroneousBitsInBorderRate, + defaultDetectorParams->maxErroneousBitsInBorderRate); + mPnh.param("minDistanceToBorder", + mDetectorParams->minDistanceToBorder, defaultDetectorParams->minDistanceToBorder); + mPnh.param("minMarkerDistanceRate", + mDetectorParams->minMarkerDistanceRate, defaultDetectorParams->minMarkerDistanceRate); + mPnh.param("minMarkerPerimeterRate", + mDetectorParams->minMarkerPerimeterRate, defaultDetectorParams->minMarkerPerimeterRate); + mPnh.param("maxMarkerPerimeterRate", + mDetectorParams->maxMarkerPerimeterRate, defaultDetectorParams->maxMarkerPerimeterRate); + mPnh.param("minOtsuStdDev", mDetectorParams->minOtsuStdDev, defaultDetectorParams->minOtsuStdDev); + mPnh.param("perspectiveRemoveIgnoredMarginPerCell", + mDetectorParams->perspectiveRemoveIgnoredMarginPerCell, + defaultDetectorParams->perspectiveRemoveIgnoredMarginPerCell); + mPnh.param("perspectiveRemovePixelPerCell", + mDetectorParams->perspectiveRemovePixelPerCell, + defaultDetectorParams->perspectiveRemovePixelPerCell); + mPnh.param("polygonalApproxAccuracyRate", + mDetectorParams->polygonalApproxAccuracyRate, + defaultDetectorParams->polygonalApproxAccuracyRate); + + NODELET_INFO("Tag detection ready, use odom frame: %s, min hit count: %d, max hit count: %d, hit increment weight: %d, hit decrement weight: %d", mUseOdom ? "true" : "false", mMinHitCountBeforePublish, mMaxHitCount, mTagIncrementWeight, mTagDecrementWeight); + } + + void TagDetectorNodelet::configCallback(mrover::DetectorParamsConfig& config, uint32_t level) { + // Don't load initial config, since it will overwrite the rosparam settings + if (level == std::numeric_limits::max()) return; + + mDetectorParams->adaptiveThreshConstant = config.adaptiveThreshConstant; + mDetectorParams->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin; + mDetectorParams->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax; + mDetectorParams->adaptiveThreshWinSizeStep = config.adaptiveThreshWinSizeStep; + mDetectorParams->cornerRefinementMaxIterations = config.cornerRefinementMaxIterations; + mDetectorParams->cornerRefinementMinAccuracy = config.cornerRefinementMinAccuracy; + mDetectorParams->cornerRefinementWinSize = config.cornerRefinementWinSize; + if (config.doCornerRefinement) { + if (config.cornerRefinementSubpix) { + mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; + } else { + mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR; + } + } else { + mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; + } + mDetectorParams->errorCorrectionRate = config.errorCorrectionRate; + mDetectorParams->minCornerDistanceRate = config.minCornerDistanceRate; + mDetectorParams->markerBorderBits = config.markerBorderBits; + mDetectorParams->maxErroneousBitsInBorderRate = config.maxErroneousBitsInBorderRate; + mDetectorParams->minDistanceToBorder = config.minDistanceToBorder; + mDetectorParams->minMarkerDistanceRate = config.minMarkerDistanceRate; + mDetectorParams->minMarkerPerimeterRate = config.minMarkerPerimeterRate; + mDetectorParams->maxMarkerPerimeterRate = config.maxMarkerPerimeterRate; + mDetectorParams->minOtsuStdDev = config.minOtsuStdDev; + mDetectorParams->perspectiveRemoveIgnoredMarginPerCell = config.perspectiveRemoveIgnoredMarginPerCell; + mDetectorParams->perspectiveRemovePixelPerCell = config.perspectiveRemovePixelPerCell; + mDetectorParams->polygonalApproxAccuracyRate = config.polygonalApproxAccuracyRate; + } + + bool TagDetectorNodelet::enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { + mEnableDetections = req.data; + if (mEnableDetections) { + res.message = "Enabled tag detections."; + NODELET_INFO("Enabled tag detections."); + } else { + res.message = "Disabled tag detections."; + NODELET_INFO("Disabled tag detections."); + } + + res.success = true; + return true; + } + +} // namespace mrover + +int main(int argc, char** argv) { + ros::init(argc, argv, "tag_detector"); + + // Start the ZED Nodelet + nodelet::Loader nodelet; + nodelet.load(ros::this_node::getName(), "mrover/TagDetectorNodelet", ros::names::getRemappings(), {}); + + ros::spin(); + + return EXIT_SUCCESS; +} + +#include +PLUGINLIB_EXPORT_CLASS(mrover::TagDetectorNodelet, nodelet::Nodelet) diff --git a/src/perception/tag_detector/long_range_tag_detector.hpp b/src/perception/tag_detector/long_range_tag_detector.hpp new file mode 100644 index 000000000..bfeb2706e --- /dev/null +++ b/src/perception/tag_detector/long_range_tag_detector.hpp @@ -0,0 +1,45 @@ +namespace mrover { + + class LongRangeTagDetectorNodelet : public nodelet::Nodelet { + ros::NodeHandle mNh, mPnh; + + std::optional mIt; + image_transport::Publisher mImgPub; + std::unordered_map mThreshPubs; // Map from threshold scale to publisher + ros::ServiceServer mServiceEnableDetections; + + image_transport::Subscriber mImgSub; + + bool mEnableDetections = true; + bool mUseOdom{}; + std::string mOdomFrameId, mMapFrameId, mCameraFrameId; + bool mPublishImages{}; // If set, we publish the images with the tags drawn on top + int mMinHitCountBeforePublish{}; + int mMaxHitCount{}; + int mTagIncrementWeight{}; + int mTagDecrementWeight{}; + + cv::Ptr mDetectorParams; + cv::Ptr mDictionary; + + cv::Mat mImg; + cv::Mat mGrayImg; + sensor_msgs::Image mImgMsg; + sensor_msgs::Image mThreshMsg; + uint32_t mSeqNum{}; + std::optional mPrevDetectedCount; // Log spam prevention + std::vector> mImmediateCorners; + std::vector mImmediateIds; + std::unordered_map mTags; + dynamic_reconfigure::Server mConfigServer; + dynamic_reconfigure::Server::CallbackType mCallbackType; + + LoopProfiler mProfiler{"Tag Detector"}; + + void onInit() override; + + void publishThresholdedImage(); + + std::optional getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v); + } +} \ No newline at end of file diff --git a/src/perception/tag_detector/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_tag_detector.processing.cpp new file mode 100644 index 000000000..14b60ef2c --- /dev/null +++ b/src/perception/tag_detector/long_range_tag_detector.processing.cpp @@ -0,0 +1,21 @@ +#include "long_range_tag_detector.hpp" + +#include "../point.hpp" + +namespace mrover { + + /** + * Detects tags in an image, draws the detected markers onto the image, and publishes them to /long_range_tag_detection + * + * @param msg image message + */ + void TagDetectorNodelet::imageCallback(sensor_msgs::Image const& msg) { + // 1. Detect tags + // 2. Update the hit counts of the tags in the mTags map + // 3. We only want to publish the tags if the topic has subscribers + if (mPublishImages && mImgPub.getNumSubscribers()) { + // Draw the tags on the image using OpenCV + } + } + +} // namespace mrover \ No newline at end of file From ce56c5326ed880cde673c7947e74d320b4c85e55 Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Tue, 26 Sep 2023 16:08:58 -0400 Subject: [PATCH 002/197] Wrote detection and hit updating 4 long range tags --- .../tag_detector/long_range_tag_detector.hpp | 68 ++++++++++++++- .../long_range_tag_detector.processing.cpp | 85 ++++++++++++++++++- 2 files changed, 148 insertions(+), 5 deletions(-) diff --git a/src/perception/tag_detector/long_range_tag_detector.hpp b/src/perception/tag_detector/long_range_tag_detector.hpp index bfeb2706e..643f11801 100644 --- a/src/perception/tag_detector/long_range_tag_detector.hpp +++ b/src/perception/tag_detector/long_range_tag_detector.hpp @@ -1,5 +1,13 @@ +#include namespace mrover { + typedef struct LongRangeTagType { + bool updated = false; + int id = -1; + int hitCount = 0; + cv::Point2f imageCenter{}; + } LongRangeTag; + class LongRangeTagDetectorNodelet : public nodelet::Nodelet { ros::NodeHandle mNh, mPnh; @@ -16,21 +24,33 @@ namespace mrover { bool mPublishImages{}; // If set, we publish the images with the tags drawn on top int mMinHitCountBeforePublish{}; int mMaxHitCount{}; + int mBaseHitCount{}; //Value the tag is initialized with after first detection. (Not incremented) int mTagIncrementWeight{}; int mTagDecrementWeight{}; + int mTagRemoveWeight{}; //weight value before the tag is removed + + //ARUCO TAG DETECTION VARIABLES cv::Ptr mDetectorParams; cv::Ptr mDictionary; + //IMAGE MESSAGE VARIABLES + cv::Mat mImg; cv::Mat mGrayImg; sensor_msgs::Image mImgMsg; sensor_msgs::Image mThreshMsg; + + uint32_t mSeqNum{}; std::optional mPrevDetectedCount; // Log spam prevention + + //Raw Tag Data from CV::ARUCO std::vector> mImmediateCorners; std::vector mImmediateIds; - std::unordered_map mTags; + + //Map from tagID to Tag + std::unordered_map mTags; dynamic_reconfigure::Server mConfigServer; dynamic_reconfigure::Server::CallbackType mCallbackType; @@ -40,6 +60,46 @@ namespace mrover { void publishThresholdedImage(); - std::optional getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v); - } -} \ No newline at end of file + std::optional getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v); + + + /** + * Detects tags in an image, draws the detected markers onto the image, and publishes them to /long_range_tag_detection + * 1. Updates mImg to store the underlying image matrix + * @param msg image message + */ + void imageCallback(sensor_msgs::Image const& msg); + + /** + * Stores msg to mImgMsg + * Creates image matrix from message, stores to mImg + * + * @param msg Current Image Message + */ + void updateImageMatrices(sensor_msgs::Image const& msg); + + /** + * Runs the cv::aruco tag detection on the cv::mat + * Modifies mImmedateCorners and mImmediateIds + * Uses the mDetectorParams and mDictionary member variables + */ + void runTagDetection(); + + /** + * For all the tags stored in mImmediateIds and mImmediateCorners: + * Check to see if they already exist in mTags + * If they do, increment their + */ + void updateHitCounts(); + + LongRangeTag createLrt(int tagId, std::vector& tagCorners); + + /** + * Assumes that mImg exists + * @return a cv::Point2f that contains the centerx and centery + * centerx and centery are -0.5, 0.5, and reflect a normalized offset from the image center + * Average all x values + */ + cv::Point2f getImageCenter(std::vector& tagCorners); + }; +}; // namespace mrover \ No newline at end of file diff --git a/src/perception/tag_detector/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_tag_detector.processing.cpp index 14b60ef2c..cf1b5d232 100644 --- a/src/perception/tag_detector/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_tag_detector.processing.cpp @@ -1,6 +1,7 @@ #include "long_range_tag_detector.hpp" #include "../point.hpp" +#include namespace mrover { @@ -9,13 +10,95 @@ namespace mrover { * * @param msg image message */ - void TagDetectorNodelet::imageCallback(sensor_msgs::Image const& msg) { + void LongRangeTagDetectorNodelet::imageCallback(sensor_msgs::Image const& msg) { + if (!mEnableDetections) return; + + //Store image contents to member variables + updateImageMatrices(msg); + // 1. Detect tags + runTagDetection(); + // 2. Update the hit counts of the tags in the mTags map + updateHitCounts(); + // 3. We only want to publish the tags if the topic has subscribers if (mPublishImages && mImgPub.getNumSubscribers()) { // Draw the tags on the image using OpenCV + // LongRangeTagDetectorNodelet::publishDrawnImages() } + + //PUBLISH TAGS + } + + //HELPER FUNCTIONS + + void LongRangeTagDetectorNodelet::updateImageMatrices(sensor_msgs::Image const& msg) { + //Store image message to mImgMsg member variable + mImgMsg = msg; + + cv::Mat cvImage{static_cast(msg.height), static_cast(msg.width), CV_8UC3, const_cast(msg.data.data())}; + + //Store to mImg member variable - unsure if necessary but need it to persist + cvImage.copyTo(mImg); + + // TODO: Set the grayImage if neccesary + } + + void LongRangeTagDetectorNodelet::runTagDetection() { + cv::aruco::detectMarkers(mImg, mDictionary, mImmediateCorners, mImmediateIds, mDetectorParams); + } + + void LongRangeTagDetectorNodelet::updateHitCounts() { + //loop through all identified IDs + for (size_t i = 0; i < mImmediateIds.size(); i++) { + int currentId = mImmediateIds[i]; + + //Create new struct for each tag + LongRangeTag lrt = createLrt(currentId, mImmediateCorners[i]); + + //Check if the tag was already detected and update hitCount to reflect + if (mTags.contains(currentId)) { + //Key exist sin mTags + lrt.hitCount = std::min(mTags[currentId].hitCount + mTagIncrementWeight, mMaxHitCount); + } + + mTags[currentId] = lrt; + } + + //Now decrement all the hitcounts for tags that were not updated + // Set updated status to false + + for (auto& mTag: mTags) { + LongRangeTag& curtag = mTag.second; + + if (curtag.updated) { + curtag.updated = false; + } else { + //Decrement weight of undetected tags + curtag.hitCount -= mTagDecrementWeight; + + //if the value has fallen belown the minimum, remove it + if (curtag.hitCount <= mTagRemoveWeight) { + mTags.erase(mTag.first); + } + } + } + + + //decrement non updated & set updated status to false + } + + LongRangeTag LongRangeTagDetectorNodelet::createLrt(int tagId, std::vector& tagCorners) { + LongRangeTag lrt; + + lrt.hitCount = mBaseHitCount; //Start at base hit count value + lrt.id = tagId; + lrt.updated = true; + + lrt.imageCenter = getImageCenter(tagCorners); + + return lrt; } } // namespace mrover \ No newline at end of file From bba2188d571e13e6ec18c9f338ecc75e907dd218 Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Tue, 26 Sep 2023 17:19:12 -0400 Subject: [PATCH 003/197] added normed center offset functionality --- .../tag_detector/long_range_tag_detector.hpp | 29 +++++++- .../long_range_tag_detector.processing.cpp | 73 +++++++++++++++---- 2 files changed, 87 insertions(+), 15 deletions(-) diff --git a/src/perception/tag_detector/long_range_tag_detector.hpp b/src/perception/tag_detector/long_range_tag_detector.hpp index 643f11801..42c3a8aee 100644 --- a/src/perception/tag_detector/long_range_tag_detector.hpp +++ b/src/perception/tag_detector/long_range_tag_detector.hpp @@ -92,14 +92,41 @@ namespace mrover { */ void updateHitCounts(); + /** + * @see updateNewlyIdentifiedTags() + * @param tagId - the tagId of the current tag + * @param tagCorners - Reference to the mTagCorners vector of Point2fs for the current id of the lrt being created + * @return a new LongRangeTag + */ LongRangeTag createLrt(int tagId, std::vector& tagCorners); /** + * @see getNormedTagCenter * Assumes that mImg exists * @return a cv::Point2f that contains the centerx and centery * centerx and centery are -0.5, 0.5, and reflect a normalized offset from the image center * Average all x values */ - cv::Point2f getImageCenter(std::vector& tagCorners); + cv::Point2f static getTagCenterPixels(std::vector& tagCorners); + + /** + * Assumes upper left corner is 0, 0 + * Want to return a negative pixel offset if width less than image_width / 2 + * similarily, negative if height > image_height / 2 + * @param tagCorners reference to tag corners, passed to @see getTagCenterPixesl + * @return Point2f of pixel offset from center + */ + cv::Point2f getTagCenterOffsetPixels(std::vector& tagCorners) const; + + cv::Point2f getNormedTagCenterOffset(std::vector& tagCorners) const; + + /** + * @see updateHitCounts() + * Helper function used in updateHitCounts + * Creates a new LongRangeTag for the identified tags and handles + * logic of adding it to the map + * @param tagIndex the index i of the target tag in the mImmediate vectors + */ + void updateNewlyIdentifiedTags(size_t tagIndex); }; }; // namespace mrover \ No newline at end of file diff --git a/src/perception/tag_detector/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_tag_detector.processing.cpp index cf1b5d232..ae57007f7 100644 --- a/src/perception/tag_detector/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_tag_detector.processing.cpp @@ -1,6 +1,7 @@ #include "long_range_tag_detector.hpp" #include "../point.hpp" +#include #include namespace mrover { @@ -49,22 +50,12 @@ namespace mrover { cv::aruco::detectMarkers(mImg, mDictionary, mImmediateCorners, mImmediateIds, mDetectorParams); } + void LongRangeTagDetectorNodelet::updateHitCounts() { //loop through all identified IDs - for (size_t i = 0; i < mImmediateIds.size(); i++) { - int currentId = mImmediateIds[i]; - - //Create new struct for each tag - LongRangeTag lrt = createLrt(currentId, mImmediateCorners[i]); + for (size_t i = 0; i < mImmediateIds.size(); i++) + updateNewlyIdentifiedTags(i); - //Check if the tag was already detected and update hitCount to reflect - if (mTags.contains(currentId)) { - //Key exist sin mTags - lrt.hitCount = std::min(mTags[currentId].hitCount + mTagIncrementWeight, mMaxHitCount); - } - - mTags[currentId] = lrt; - } //Now decrement all the hitcounts for tags that were not updated // Set updated status to false @@ -96,9 +87,63 @@ namespace mrover { lrt.id = tagId; lrt.updated = true; - lrt.imageCenter = getImageCenter(tagCorners); + lrt.imageCenter = getNormedTagCenterOffset(tagCorners); return lrt; } + void LongRangeTagDetectorNodelet::updateNewlyIdentifiedTags(size_t tagIndex) { + int currentId = mImmediateIds[tagIndex]; + + //Create new struct for each tag + LongRangeTag lrt = createLrt(currentId, mImmediateCorners[tagIndex]); + + //Check if the tag was already detected and update hitCount to reflect + if (mTags.contains(currentId)) { + //Key exist sin mTags + lrt.hitCount = std::min(mTags[currentId].hitCount + mTagIncrementWeight, mMaxHitCount); + } + + mTags[currentId] = lrt; + } + + cv::Point2f LongRangeTagDetectorNodelet::getTagCenterPixels(std::vector& tagCorners) { + cv::Point2f centerPoint; + float centerX = 0; + float centerY = 0; + + for (size_t i = 0; i < 4; i++) { + centerX += tagCorners[i].x; + centerY += tagCorners[i].y; + } + + centerX /= 4.0; + centerY /= 4.0; + + centerPoint.x = centerX; + centerPoint.y = centerY; + + return centerPoint; + } + + cv::Point2f LongRangeTagDetectorNodelet::getTagCenterOffsetPixels(std::vector& tagCorners) const { + cv::Point2f centerPoint = getTagCenterPixels(tagCorners); + + centerPoint.x -= float(mImgMsg.width); + + //-1 is necessary because of 0,0 being in the top left + centerPoint.y = float(-1.0) * (centerPoint.y - float(mImgMsg.height)); + + return centerPoint; + } + + cv::Point2f LongRangeTagDetectorNodelet::getNormedTagCenterOffset(std::vector& tagCorners) const { + cv::Point2f offsetCenterPoint = getTagCenterOffsetPixels(tagCorners); + + offsetCenterPoint.x /= float(mImgMsg.width); + offsetCenterPoint.y /= float(mImgMsg.height); + + return offsetCenterPoint; + } + } // namespace mrover \ No newline at end of file From 1ce57d1aedf5267f11995eab0211ff49df948efa Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Tue, 26 Sep 2023 20:07:37 -0400 Subject: [PATCH 004/197] updated Perception README.md --- src/perception/tag_detector/README.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/perception/tag_detector/README.md b/src/perception/tag_detector/README.md index f8a24788b..a80de7d8f 100644 --- a/src/perception/tag_detector/README.md +++ b/src/perception/tag_detector/README.md @@ -2,5 +2,12 @@ ### Code Layout + +## Short Range Detection - [tag_detector.cpp](./tag_detector.cpp) Mainly ROS node setup (topics, parameters, etc.) - [tag_detector.processing.cpp](./tag_detector.processing.cpp) Processes inputs (camera feed and pointcloud) to estimate locations of the ArUco fiducials + + +## Long Range Detection +- [long_range_tag_detector.cpp](./long_range_tag_detector.cpp) Mainly ROS node setup (topics, parameters, etc.) +- [long_range_tag_detector.processing.cpp](./long_range_tag_detector.processing.cpp) Does all the input processing (ArUco recognition) and publishes the information. From f14d6fbc6e6f4e66f1ad660426de8da2a5b4c29a Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Tue, 26 Sep 2023 20:13:28 -0400 Subject: [PATCH 005/197] Created message types --- msg/LongRangeTag.msg | 7 +++++++ msg/LongRangeTags.msg | 1 + 2 files changed, 8 insertions(+) create mode 100644 msg/LongRangeTag.msg create mode 100644 msg/LongRangeTags.msg diff --git a/msg/LongRangeTag.msg b/msg/LongRangeTag.msg new file mode 100644 index 000000000..6a8ea2cb2 --- /dev/null +++ b/msg/LongRangeTag.msg @@ -0,0 +1,7 @@ +# Message published by the LongRangeTagDetector nodelet. +# -0.5 <= xOffset, yOffset <= 0.5 +# Normed to image size, and have signs reflecting typical cartesian coordinates +# Where (0, 0) is at the image center +uint8 id +float32 xOffset +float32 yOffset \ No newline at end of file diff --git a/msg/LongRangeTags.msg b/msg/LongRangeTags.msg new file mode 100644 index 000000000..2f5c82dff --- /dev/null +++ b/msg/LongRangeTags.msg @@ -0,0 +1 @@ +LongRangeTags[] longRangeTags \ No newline at end of file From 1ee5d9fed4e1dd5ac00c5d7617b82469870b7814 Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Tue, 26 Sep 2023 20:36:52 -0400 Subject: [PATCH 006/197] fixed tags issue and added to CmakeLists --- CMakeLists.txt | 2 ++ msg/LongRangeTags.msg | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3943d9f44..1ba21c323 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,6 +49,8 @@ set(MROVER_MESSAGE_FILES Waypoint.msg WaypointType.msg GPSPointList.msg + LongRangeTag.msg + LongRangeTags.msg ) # Service files diff --git a/msg/LongRangeTags.msg b/msg/LongRangeTags.msg index 2f5c82dff..4a48ffb9b 100644 --- a/msg/LongRangeTags.msg +++ b/msg/LongRangeTags.msg @@ -1 +1 @@ -LongRangeTags[] longRangeTags \ No newline at end of file +LongRangeTag[] longRangeTags \ No newline at end of file From de3fd1ebf1e324d10f352e5d6b0d4c372a74ee53 Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Tue, 26 Sep 2023 20:43:35 -0400 Subject: [PATCH 007/197] added tag publishing logic --- .../tag_detector/long_range_tag_detector.hpp | 68 ++++++++++++------- .../long_range_tag_detector.processing.cpp | 40 +++++++++-- 2 files changed, 77 insertions(+), 31 deletions(-) diff --git a/src/perception/tag_detector/long_range_tag_detector.hpp b/src/perception/tag_detector/long_range_tag_detector.hpp index 42c3a8aee..c600e25a6 100644 --- a/src/perception/tag_detector/long_range_tag_detector.hpp +++ b/src/perception/tag_detector/long_range_tag_detector.hpp @@ -1,4 +1,9 @@ +#include "mrover/LongRangeTag.h" +#include "mrover/LongRangeTags.h" + #include +#include + namespace mrover { typedef struct LongRangeTagType { @@ -6,22 +11,23 @@ namespace mrover { int id = -1; int hitCount = 0; cv::Point2f imageCenter{}; - } LongRangeTag; + } LongRangeTagStruct; class LongRangeTagDetectorNodelet : public nodelet::Nodelet { ros::NodeHandle mNh, mPnh; - std::optional mIt; + //Image Subscriber + image_transport::Subscriber mImgSub; image_transport::Publisher mImgPub; - std::unordered_map mThreshPubs; // Map from threshold scale to publisher - ros::ServiceServer mServiceEnableDetections; - image_transport::Subscriber mImgSub; + //Publishes LongRangeTags messages + ros::Publisher mLongRangeTagsPub; + //Publishing Flags bool mEnableDetections = true; - bool mUseOdom{}; - std::string mOdomFrameId, mMapFrameId, mCameraFrameId; bool mPublishImages{}; // If set, we publish the images with the tags drawn on top + + //Constatns set in ROS params for num hits needed to publish int mMinHitCountBeforePublish{}; int mMaxHitCount{}; int mBaseHitCount{}; //Value the tag is initialized with after first detection. (Not incremented) @@ -34,35 +40,34 @@ namespace mrover { cv::Ptr mDetectorParams; cv::Ptr mDictionary; - //IMAGE MESSAGE VARIABLES - + //IMAGE VARIABLES cv::Mat mImg; - cv::Mat mGrayImg; sensor_msgs::Image mImgMsg; - sensor_msgs::Image mThreshMsg; - - - uint32_t mSeqNum{}; - std::optional mPrevDetectedCount; // Log spam prevention //Raw Tag Data from CV::ARUCO std::vector> mImmediateCorners; std::vector mImmediateIds; //Map from tagID to Tag - std::unordered_map mTags; + std::unordered_map mTags; + + //TODO + //All these variables are unused by long_range_tag_detector.processing.cpp + //A bunch of things I think I should be able to delete + //But I am not sure about + uint32_t mSeqNum{}; + std::optional mPrevDetectedCount; // Log spam prevention dynamic_reconfigure::Server mConfigServer; dynamic_reconfigure::Server::CallbackType mCallbackType; - - LoopProfiler mProfiler{"Tag Detector"}; + LoopProfiler mProfiler{"Long RangeTag Detector"}; + std::optional mIt; + std::unordered_map mThreshPubs; // Map from threshold scale to publisher + ros::ServiceServer mServiceEnableDetections; + bool mUseOdom{}; + std::string mOdomFrameId, mMapFrameId, mCameraFrameId; void onInit() override; - void publishThresholdedImage(); - - std::optional getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v); - - /** * Detects tags in an image, draws the detected markers onto the image, and publishes them to /long_range_tag_detection * 1. Updates mImg to store the underlying image matrix @@ -98,7 +103,7 @@ namespace mrover { * @param tagCorners - Reference to the mTagCorners vector of Point2fs for the current id of the lrt being created * @return a new LongRangeTag */ - LongRangeTag createLrt(int tagId, std::vector& tagCorners); + LongRangeTagStruct createLrt(int tagId, std::vector& tagCorners); /** * @see getNormedTagCenter @@ -128,5 +133,18 @@ namespace mrover { * @param tagIndex the index i of the target tag in the mImmediate vectors */ void updateNewlyIdentifiedTags(size_t tagIndex); + + + /** + * Publish the tags which have been detected for more than + * mMinHitCountBeforePublish + */ + void publishThresholdTags(); + + /** + * publishes the thresholded tags onto an image using OpenCV + * only if mPublishImages and the topic has a subscriber + */ + void publishTagsOnImage(); }; -}; // namespace mrover \ No newline at end of file +}; // namespace mrover diff --git a/src/perception/tag_detector/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_tag_detector.processing.cpp index ae57007f7..993ecd13d 100644 --- a/src/perception/tag_detector/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_tag_detector.processing.cpp @@ -1,6 +1,8 @@ #include "long_range_tag_detector.hpp" -#include "../point.hpp" +#include "../point.hpp" //Might not actually need?' +#include "mrover/LongRangeTags.h" + #include #include @@ -26,9 +28,11 @@ namespace mrover { // 3. We only want to publish the tags if the topic has subscribers if (mPublishImages && mImgPub.getNumSubscribers()) { // Draw the tags on the image using OpenCV - // LongRangeTagDetectorNodelet::publishDrawnImages() + publishTagsOnImage(); } + publishThresholdTags(); + //PUBLISH TAGS } @@ -61,7 +65,7 @@ namespace mrover { // Set updated status to false for (auto& mTag: mTags) { - LongRangeTag& curtag = mTag.second; + LongRangeTagStruct& curtag = mTag.second; if (curtag.updated) { curtag.updated = false; @@ -80,8 +84,8 @@ namespace mrover { //decrement non updated & set updated status to false } - LongRangeTag LongRangeTagDetectorNodelet::createLrt(int tagId, std::vector& tagCorners) { - LongRangeTag lrt; + LongRangeTagStruct LongRangeTagDetectorNodelet::createLrt(int tagId, std::vector& tagCorners) { + LongRangeTagStruct lrt; lrt.hitCount = mBaseHitCount; //Start at base hit count value lrt.id = tagId; @@ -96,7 +100,7 @@ namespace mrover { int currentId = mImmediateIds[tagIndex]; //Create new struct for each tag - LongRangeTag lrt = createLrt(currentId, mImmediateCorners[tagIndex]); + LongRangeTagStruct lrt = createLrt(currentId, mImmediateCorners[tagIndex]); //Check if the tag was already detected and update hitCount to reflect if (mTags.contains(currentId)) { @@ -146,4 +150,28 @@ namespace mrover { return offsetCenterPoint; } + void LongRangeTagDetectorNodelet::publishThresholdTags() { + //Loop through all the tags + LongRangeTags tagsMessage; // + + for (auto& tag: mTags) { + if (tag.second.hitCount >= mMinHitCountBeforePublish) { + //LongRangeTag message + LongRangeTag newTagMessage; + + //Fill in fields + newTagMessage.id = tag.second.id; + newTagMessage.xOffset = tag.second.imageCenter.x; + newTagMessage.yOffset = tag.second.imageCenter.y; + + //Add to back of tagsMessage + tagsMessage.longRangeTags.push_back(newTagMessage); + } + } + + //tagsMessage should be a vector of LongRangeTag messages + //Need something like an mTagsPublisher + mLongRangeTagsPub.publish(tagsMessage); + } + } // namespace mrover \ No newline at end of file From 1dc1f1d0dc05e8b309a6d67f01bb0e3bef8b42d4 Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Tue, 26 Sep 2023 20:44:49 -0400 Subject: [PATCH 008/197] replaced C style casting with static casts --- .../tag_detector/long_range_tag_detector.processing.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/perception/tag_detector/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_tag_detector.processing.cpp index 993ecd13d..76b8fe7bc 100644 --- a/src/perception/tag_detector/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_tag_detector.processing.cpp @@ -133,10 +133,10 @@ namespace mrover { cv::Point2f LongRangeTagDetectorNodelet::getTagCenterOffsetPixels(std::vector& tagCorners) const { cv::Point2f centerPoint = getTagCenterPixels(tagCorners); - centerPoint.x -= float(mImgMsg.width); + centerPoint.x -= static_cast(mImgMsg.width); //-1 is necessary because of 0,0 being in the top left - centerPoint.y = float(-1.0) * (centerPoint.y - float(mImgMsg.height)); + centerPoint.y = static_cast(-1.0) * (centerPoint.y - static_cast(mImgMsg.height)); return centerPoint; } @@ -144,8 +144,8 @@ namespace mrover { cv::Point2f LongRangeTagDetectorNodelet::getNormedTagCenterOffset(std::vector& tagCorners) const { cv::Point2f offsetCenterPoint = getTagCenterOffsetPixels(tagCorners); - offsetCenterPoint.x /= float(mImgMsg.width); - offsetCenterPoint.y /= float(mImgMsg.height); + offsetCenterPoint.x /= static_cast(mImgMsg.width); + offsetCenterPoint.y /= static_cast(mImgMsg.height); return offsetCenterPoint; } From 977e2526eccb1e10e3d641fa6829da304954dd37 Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Tue, 26 Sep 2023 20:48:43 -0400 Subject: [PATCH 009/197] added marked image publishing --- .../long_range_tag_detector.processing.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/perception/tag_detector/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_tag_detector.processing.cpp index 76b8fe7bc..1120f5bc0 100644 --- a/src/perception/tag_detector/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_tag_detector.processing.cpp @@ -3,8 +3,12 @@ #include "../point.hpp" //Might not actually need?' #include "mrover/LongRangeTags.h" +#include +#include #include +#include #include +#include namespace mrover { @@ -31,6 +35,7 @@ namespace mrover { publishTagsOnImage(); } + //Publish all tags that meet threshold publishThresholdTags(); //PUBLISH TAGS @@ -174,4 +179,16 @@ namespace mrover { mLongRangeTagsPub.publish(tagsMessage); } + void LongRangeTagDetectorNodelet::publishTagsOnImage() { + cv::Mat markedImage; + mImg.copyTo(markedImage); + + cv::aruco::drawDetectedMarkers(markedImage, mImmediateCorners); + + sensor_msgs::Image imgMsgOut = mImgMsg; + imgMsgOut.data = markedImage; + + mImgPub.publish(imgMsgOut); + } + } // namespace mrover \ No newline at end of file From 35c9bf2571542e9e93b8ca46c0536ba2775054e8 Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Sat, 30 Sep 2023 11:01:03 -0400 Subject: [PATCH 010/197] Removed abbreviation Removed typedef --- .../tag_detector/long_range_tag_detector.hpp | 4 ++-- .../long_range_tag_detector.processing.cpp | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/perception/tag_detector/long_range_tag_detector.hpp b/src/perception/tag_detector/long_range_tag_detector.hpp index c600e25a6..b5dddfa06 100644 --- a/src/perception/tag_detector/long_range_tag_detector.hpp +++ b/src/perception/tag_detector/long_range_tag_detector.hpp @@ -6,12 +6,12 @@ namespace mrover { - typedef struct LongRangeTagType { + struct LongRangeTagStruct { bool updated = false; int id = -1; int hitCount = 0; cv::Point2f imageCenter{}; - } LongRangeTagStruct; + }; class LongRangeTagDetectorNodelet : public nodelet::Nodelet { ros::NodeHandle mNh, mPnh; diff --git a/src/perception/tag_detector/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_tag_detector.processing.cpp index 1120f5bc0..63975f713 100644 --- a/src/perception/tag_detector/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_tag_detector.processing.cpp @@ -70,16 +70,16 @@ namespace mrover { // Set updated status to false for (auto& mTag: mTags) { - LongRangeTagStruct& curtag = mTag.second; + LongRangeTagStruct& currentTag = mTag.second; - if (curtag.updated) { - curtag.updated = false; + if (currentTag.updated) { + currentTag.updated = false; } else { //Decrement weight of undetected tags - curtag.hitCount -= mTagDecrementWeight; + currentTag.hitCount -= mTagDecrementWeight; //if the value has fallen belown the minimum, remove it - if (curtag.hitCount <= mTagRemoveWeight) { + if (currentTag.hitCount <= mTagRemoveWeight) { mTags.erase(mTag.first); } } From 51c664baf2dc4d0a377e66ad1235c95fd9a1fe73 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Sun, 1 Oct 2023 12:33:10 -0400 Subject: [PATCH 011/197] add wrapper node --- .../long_range_cam/long_range_cam.cpp | 167 ++++++++++++++++++ .../long_range_cam/long_range_cam.hpp | 60 +++++++ .../tag_detector/long_range_tag_detector.cpp | 14 +- .../tag_detector/long_range_tag_detector.hpp | 100 +---------- 4 files changed, 242 insertions(+), 99 deletions(-) create mode 100644 src/perception/long_range_cam/long_range_cam.cpp create mode 100644 src/perception/long_range_cam/long_range_cam.hpp diff --git a/src/perception/long_range_cam/long_range_cam.cpp b/src/perception/long_range_cam/long_range_cam.cpp new file mode 100644 index 000000000..1a13c5cb1 --- /dev/null +++ b/src/perception/long_range_cam/long_range_cam.cpp @@ -0,0 +1,167 @@ +#include "long_range_cam.hpp" + +namespace mrover { + + using namespace std::chrono_literals; + + /** + * @brief Load config, open the camera, and start our threads + */ + void LongRangeCamNodelet::onInit() { + try { + // See: http://wiki.ros.org/roscpp/Overview/NodeHandles for public vs. private node handle + // MT means multithreaded + mNh = getMTNodeHandle(); + mPnh = getMTPrivateNodeHandle(); + mCamInfoPub = mNh.advertise("camera/right/camera_info", 1); + image_transport::ImageTransport it{mNh}; + mRightImgPub = it.advertise("long_range_cam/image", 1); + + int imageWidth{}; + int imageHeight{}; + // TODO: edit these so they correspond to the long range cam's resolution + mPnh.param("image_width", imageWidth, 1280); + mPnh.param("image_height", imageHeight, 720); + + if (imageWidth < 0 || imageHeight < 0) { + throw std::invalid_argument("Invalid image dimensions"); + } + if (mGrabTargetFps < 0) { + throw std::invalid_argument("Invalid grab target framerate"); + } + + mImageResolution = sl::Resolution(imageWidth, imageHeight); + mPointResolution = sl::Resolution(imageWidth, imageHeight); + + NODELET_INFO("Resolution: %s image: %zux%zu points: %zux%zu", + grabResolutionString.c_str(), mImageResolution.width, mImageResolution.height); + + sl::InitParameters initParameters; + if (mSvoPath) { + initParameters.input.setFromSVOFile(mSvoPath); + } else { + initParameters.input.setFromCameraID(-1, sl::BUS_TYPE::USB); + } + initParameters.depth_stabilization = mUseDepthStabilization; + initParameters.camera_resolution = stringToZedEnum(grabResolutionString); + initParameters.depth_mode = stringToZedEnum(depthModeString); + initParameters.coordinate_units = sl::UNIT::METER; + initParameters.sdk_verbose = true; // Log useful information + initParameters.camera_fps = mGrabTargetFps; + initParameters.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; // Match ROS + initParameters.depth_maximum_distance = mDepthMaximumDistance; + + if (mZed.open(initParameters) != sl::ERROR_CODE::SUCCESS) { + throw std::runtime_error("ZED failed to open"); + } + + cudaDeviceProp prop{}; + cudaGetDeviceProperties(&prop, 0); + ROS_INFO("MP count: %d, Max threads/MP: %d, Max blocks/MP: %d, max threads/block: %d", + prop.multiProcessorCount, prop.maxThreadsPerMultiProcessor, prop.maxBlocksPerMultiProcessor, prop.maxThreadsPerBlock); + + mGrabThread = std::thread(&ZedNodelet::grabUpdate, this); + + } catch (std::exception const& e) { + NODELET_FATAL("Exception while starting: %s", e.what()); + ros::shutdown(); + } + } + + /** + * @brief Grabs measures from the ZED. + * + * This update loop needs to happen as fast as possible. + * grab() on the ZED updates positional tracking (visual odometry) which works best at high update rates. + * As such we retrieve the image and point cloud on the GPU to send to the other thread for processing. + * This only happens if the other thread is ready to avoid blocking, hence the try lock. + */ + void ZedNodelet::grabUpdate() { + try { + NODELET_INFO("Starting grab thread"); + while (ros::ok()) { + mGrabThreadProfiler.beginLoop(); + + sl::RuntimeParameters runtimeParameters; + runtimeParameters.confidence_threshold = mDepthConfidence; + runtimeParameters.texture_confidence_threshold = mTextureConfidence; + + if (mZed.grab(runtimeParameters) != sl::ERROR_CODE::SUCCESS) + throw std::runtime_error("ZED failed to grab"); + mGrabThreadProfiler.measureEvent("Grab"); + + // Retrieval has to happen on the same thread as grab so that the image and point cloud are synced + if (mRightImgPub.getNumSubscribers()) + if (mZed.retrieveImage(mGrabMeasures.rightImage, sl::VIEW::RIGHT, sl::MEM::GPU, mImageResolution) != sl::ERROR_CODE::SUCCESS) + throw std::runtime_error("ZED failed to retrieve right image"); + // Only left set is used for processing + if (mZed.retrieveImage(mGrabMeasures.leftImage, sl::VIEW::LEFT, sl::MEM::GPU, mImageResolution) != sl::ERROR_CODE::SUCCESS) + throw std::runtime_error("ZED failed to retrieve left image"); + if (mZed.retrieveMeasure(mGrabMeasures.leftPoints, sl::MEASURE::XYZ, sl::MEM::GPU, mPointResolution) != sl::ERROR_CODE::SUCCESS) + throw std::runtime_error("ZED failed to retrieve point cloud"); + + assert(mGrabMeasures.leftImage.timestamp == mGrabMeasures.leftPoints.timestamp); + + + mGrabMeasures.time = mSvoPath ? ros::Time::now() : slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); + mGrabThreadProfiler.measureEvent("Retrieve"); + + // If the processing thread is busy skip + // We want this thread to run as fast as possible for grab and positional tracking + if (mSwapMutex.try_lock()) { + std::swap(mGrabMeasures, mPcMeasures); + mIsSwapReady = true; + mSwapMutex.unlock(); + mSwapCv.notify_one(); + } + mGrabThreadProfiler.measureEvent("Try swap"); + + mGrabUpdateTick++; + } + + mZed.close(); + NODELET_INFO("Grab thread finished"); + + } catch (std::exception const& e) { + NODELET_FATAL("Exception while running grab thread: %s", e.what()); + mZed.close(); + ros::shutdown(); + std::exit(EXIT_FAILURE); + } + } + + LongRangeCamNodelet::~LongRangeCamNodelet() { + NODELET_INFO("Long range cam node shutting down"); + mPointCloudThread.join(); + mGrabThread.join(); + } + + ZedNodelet::Measures::Measures(ZedNodelet::Measures&& other) noexcept { + *this = std::move(other); + } + + ZedNodelet::Measures& ZedNodelet::Measures::operator=(ZedNodelet::Measures&& other) noexcept { + sl::Mat::swap(other.leftImage, leftImage); + sl::Mat::swap(other.rightImage, rightImage); + sl::Mat::swap(other.leftPoints, leftPoints); + std::swap(time, other.time); + return *this; + } +} // namespace mrover + +int main(int argc, char** argv) { + ros::init(argc, argv, "zed_wrapper"); + + // Start the ZED Nodelet + nodelet::Loader nodelet; + nodelet.load(ros::this_node::getName(), "mrover/ZedNodelet", ros::names::getRemappings(), {}); + + ros::spin(); + + return EXIT_SUCCESS; +} + +#ifdef MROVER_IS_NODELET +#include +PLUGINLIB_EXPORT_CLASS(mrover::ZedNodelet, nodelet::Nodelet) +#endif diff --git a/src/perception/long_range_cam/long_range_cam.hpp b/src/perception/long_range_cam/long_range_cam.hpp new file mode 100644 index 000000000..e53e140f4 --- /dev/null +++ b/src/perception/long_range_cam/long_range_cam.hpp @@ -0,0 +1,60 @@ +#pragma once + +namespace mrover { + + class LongRangeCamNodelet : public nodelet::Nodelet { + + private: + explicit LongRangeCamNodelet; + virtual ~LongRangeCamNodelet; + LongRangeCamNodelet + ros::NodeHandle mNh, mPnh; + + ros::Publisher mCamInfoPub; + image_transport::Publisher mCamImgPub; + + PointCloudGpu mPointCloudGpu; + + sl::Resolution mImageResolution; + sl::String mSvoPath; + int mGrabTargetFps{}; + int mDepthConfidence{}; + int mTextureConfidence{}; + bool mUseBuiltinPosTracking{}; + bool mUseAreaMemory{}; + bool mUsePoseSmoothing{}; + bool mUseLoopProfiler{}; + bool mUseDepthStabilization{}; + float mDepthMaximumDistance{}; + + Measures mGrabMeasures, mPcMeasures; + + std::thread mGrabThread; + std::mutex mSwapMutex; + std::condition_variable mSwapCv; + std::atomic_bool mIsSwapReady = false; + + LoopProfiler mGrabThreadProfiler{"Long Range Cam Grab"}; + + size_t mGrabUpdateTick = 0, mPointCloudUpdateTick = 0; + + void onInit() override; + + public: + LongRangeCamNodelet() = default; + + ~LongRangeCamNodelet() override; + + void grabUpdate(); + }; + + ros::Time slTime2Ros(sl::Timestamp t); + + void fillCameraInfoMessages(sl::CalibrationParameters& calibration, sl::Resolution const& resolution, + sensor_msgs::CameraInfoPtr const& leftInfoMsg, sensor_msgs::CameraInfoPtr const& rightInfoMsg); + + void fillImageMessage(sl::Mat& bgra, sensor_msgs::ImagePtr const& msg); + + void checkCudaError(cudaError_t error); + +} // namespace mrover diff --git a/src/perception/tag_detector/long_range_tag_detector.cpp b/src/perception/tag_detector/long_range_tag_detector.cpp index 86ab376b4..cec4a48b6 100644 --- a/src/perception/tag_detector/long_range_tag_detector.cpp +++ b/src/perception/tag_detector/long_range_tag_detector.cpp @@ -2,7 +2,7 @@ namespace mrover { - void TagDetectorNodelet::onInit() { + void LongRangeTagDetectorNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); mDetectorParams = new cv::aruco::DetectorParameters(); @@ -21,7 +21,7 @@ namespace mrover { mImgPub = mIt->advertise("long_range_tag_detection", 1); mDictionary = cv::aruco::getPredefinedDictionary(dictionaryNumber); - mPcSub = mNh.subscribe("zoom_cam/image", 1, &LongRangeTagDetectorNodelet::imageCallback, this); + mPcSub = mNh.subscribe("long_range_cam/image", 1, &LongRangeTagDetectorNodelet::imageCallback, this); mServiceEnableDetections = mNh.advertiseService("enable_detections", &TagDetectorNodelet::enableDetectionsCallback, this); // Lambda handles passing class pointer (implicit first parameter) to configCallback @@ -124,7 +124,7 @@ namespace mrover { mDetectorParams->polygonalApproxAccuracyRate = config.polygonalApproxAccuracyRate; } - bool TagDetectorNodelet::enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { + bool LongRangeTagDetectorNodelet::enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { mEnableDetections = req.data; if (mEnableDetections) { res.message = "Enabled tag detections."; @@ -141,11 +141,11 @@ namespace mrover { } // namespace mrover int main(int argc, char** argv) { - ros::init(argc, argv, "tag_detector"); + ros::init(argc, argv, "long_range_tag_detector"); - // Start the ZED Nodelet + // Start the long range cam Nodelet nodelet::Loader nodelet; - nodelet.load(ros::this_node::getName(), "mrover/TagDetectorNodelet", ros::names::getRemappings(), {}); + nodelet.load(ros::this_node::getName(), "mrover/LongRangeTagDetectorNodelet", ros::names::getRemappings(), {}); ros::spin(); @@ -153,4 +153,4 @@ int main(int argc, char** argv) { } #include -PLUGINLIB_EXPORT_CLASS(mrover::TagDetectorNodelet, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(mrover::LongRangeTagDetectorNodelet, nodelet::Nodelet) diff --git a/src/perception/tag_detector/long_range_tag_detector.hpp b/src/perception/tag_detector/long_range_tag_detector.hpp index 42c3a8aee..1805c155e 100644 --- a/src/perception/tag_detector/long_range_tag_detector.hpp +++ b/src/perception/tag_detector/long_range_tag_detector.hpp @@ -1,13 +1,5 @@ -#include namespace mrover { - typedef struct LongRangeTagType { - bool updated = false; - int id = -1; - int hitCount = 0; - cv::Point2f imageCenter{}; - } LongRangeTag; - class LongRangeTagDetectorNodelet : public nodelet::Nodelet { ros::NodeHandle mNh, mPnh; @@ -17,6 +9,9 @@ namespace mrover { ros::ServiceServer mServiceEnableDetections; image_transport::Subscriber mImgSub; + tf2_ros::Buffer mTfBuffer; + tf2_ros::TransformListener mTfListener{mTfBuffer}; + tf2_ros::TransformBroadcaster mTfBroadcaster; bool mEnableDetections = true; bool mUseOdom{}; @@ -24,109 +19,30 @@ namespace mrover { bool mPublishImages{}; // If set, we publish the images with the tags drawn on top int mMinHitCountBeforePublish{}; int mMaxHitCount{}; - int mBaseHitCount{}; //Value the tag is initialized with after first detection. (Not incremented) int mTagIncrementWeight{}; int mTagDecrementWeight{}; - int mTagRemoveWeight{}; //weight value before the tag is removed - - //ARUCO TAG DETECTION VARIABLES cv::Ptr mDetectorParams; cv::Ptr mDictionary; - //IMAGE MESSAGE VARIABLES - cv::Mat mImg; cv::Mat mGrayImg; sensor_msgs::Image mImgMsg; sensor_msgs::Image mThreshMsg; - - uint32_t mSeqNum{}; std::optional mPrevDetectedCount; // Log spam prevention - - //Raw Tag Data from CV::ARUCO std::vector> mImmediateCorners; std::vector mImmediateIds; - - //Map from tagID to Tag - std::unordered_map mTags; + std::unordered_map mTags; dynamic_reconfigure::Server mConfigServer; dynamic_reconfigure::Server::CallbackType mCallbackType; - LoopProfiler mProfiler{"Tag Detector"}; + LoopProfiler mProfiler{"Long Range Tag Detector"}; void onInit() override; void publishThresholdedImage(); - std::optional getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v); - - - /** - * Detects tags in an image, draws the detected markers onto the image, and publishes them to /long_range_tag_detection - * 1. Updates mImg to store the underlying image matrix - * @param msg image message - */ - void imageCallback(sensor_msgs::Image const& msg); - - /** - * Stores msg to mImgMsg - * Creates image matrix from message, stores to mImg - * - * @param msg Current Image Message - */ - void updateImageMatrices(sensor_msgs::Image const& msg); - - /** - * Runs the cv::aruco tag detection on the cv::mat - * Modifies mImmedateCorners and mImmediateIds - * Uses the mDetectorParams and mDictionary member variables - */ - void runTagDetection(); - - /** - * For all the tags stored in mImmediateIds and mImmediateCorners: - * Check to see if they already exist in mTags - * If they do, increment their - */ - void updateHitCounts(); - - /** - * @see updateNewlyIdentifiedTags() - * @param tagId - the tagId of the current tag - * @param tagCorners - Reference to the mTagCorners vector of Point2fs for the current id of the lrt being created - * @return a new LongRangeTag - */ - LongRangeTag createLrt(int tagId, std::vector& tagCorners); - - /** - * @see getNormedTagCenter - * Assumes that mImg exists - * @return a cv::Point2f that contains the centerx and centery - * centerx and centery are -0.5, 0.5, and reflect a normalized offset from the image center - * Average all x values - */ - cv::Point2f static getTagCenterPixels(std::vector& tagCorners); - - /** - * Assumes upper left corner is 0, 0 - * Want to return a negative pixel offset if width less than image_width / 2 - * similarily, negative if height > image_height / 2 - * @param tagCorners reference to tag corners, passed to @see getTagCenterPixesl - * @return Point2f of pixel offset from center - */ - cv::Point2f getTagCenterOffsetPixels(std::vector& tagCorners) const; - - cv::Point2f getNormedTagCenterOffset(std::vector& tagCorners) const; - - /** - * @see updateHitCounts() - * Helper function used in updateHitCounts - * Creates a new LongRangeTag for the identified tags and handles - * logic of adding it to the map - * @param tagIndex the index i of the target tag in the mImmediate vectors - */ - void updateNewlyIdentifiedTags(size_t tagIndex); - }; -}; // namespace mrover \ No newline at end of file + std::optional getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v); + } +} \ No newline at end of file From f05e396b86a70f2730b71d97440b14c887e4d64f Mon Sep 17 00:00:00 2001 From: ajaysum Date: Sun, 1 Oct 2023 10:09:32 -0700 Subject: [PATCH 012/197] x --- src/perception/long_range_cam/long_range_cam.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/perception/long_range_cam/long_range_cam.hpp b/src/perception/long_range_cam/long_range_cam.hpp index e53e140f4..4ded2cf4a 100644 --- a/src/perception/long_range_cam/long_range_cam.hpp +++ b/src/perception/long_range_cam/long_range_cam.hpp @@ -26,7 +26,10 @@ namespace mrover { bool mUseLoopProfiler{}; bool mUseDepthStabilization{}; float mDepthMaximumDistance{}; + + + Measures mGrabMeasures, mPcMeasures; std::thread mGrabThread; From 947417dd05bbe2f797d84279b8397c343721650f Mon Sep 17 00:00:00 2001 From: qhdwight Date: Tue, 26 Sep 2023 15:03:07 -0400 Subject: [PATCH 013/197] save --- .gitmodules | 3 + CMakeLists.txt | 305 ++++++++---------- deps/opencv | 1 + package.xml | 5 - src/esw/cameras/cameras.cpp | 5 + src/esw/streaming/streaming.cu | 24 ++ src/esw/streaming/streaming.hpp | 10 + src/perception/tag_detector/pch.hpp | 2 +- src/perception/tag_detector/tag_detector.cpp | 105 +++--- src/perception/tag_detector/tag_detector.hpp | 5 +- .../tag_detector/tag_detector.processing.cpp | 4 +- .../tag_detector/tag_detector.threshold.cpp | 6 +- .../autonomy/AutonomyStarterProject.cmake | 2 +- starter_project/autonomy/src/perception.cpp | 17 +- starter_project/autonomy/src/perception.hpp | 13 +- 15 files changed, 249 insertions(+), 258 deletions(-) create mode 100644 .gitmodules create mode 160000 deps/opencv create mode 100644 src/esw/cameras/cameras.cpp create mode 100644 src/esw/streaming/streaming.cu create mode 100644 src/esw/streaming/streaming.hpp diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..b179da5f2 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "deps/opencv"] + path = deps/opencv + url = https://github.com/opencv/opencv.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 3943d9f44..e8de2de68 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,18 +1,18 @@ cmake_minimum_required(VERSION 3.16) project(mrover VERSION 2024.0.0 LANGUAGES CXX) +# Generate compile_commands.json for clangd +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_CXX_STANDARD 23) set(CMAKE_CXX_STANDARD_REQUIRED ON) if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang") add_link_options(-fuse-ld=lld-16) endif () -set(MROVER_CPP_COMPILE_OPTIONS -Wall -Wextra -Werror -pedantic) -# Generate compile_commands.json for clangd -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +#set(MROVER_CPP_COMPILE_OPTIONS -Wall -Wextra -Werror -pedantic) # ROS packages list -set(MROVER_PACKAGES +set(MROVER_ROS_DEPENDENCIES rospy roscpp std_msgs @@ -20,6 +20,12 @@ set(MROVER_PACKAGES dynamic_reconfigure rostest sensor_msgs + tf2 + tf2_ros + tf2_geometry_msgs + image_transport + teleop_twist_joy + gazebo_ros ) # Message files @@ -68,9 +74,7 @@ set(MROVER_SERVICE_FILES ResetCameras.srv ) -# Generate messages list -set(MROVER_ROS_MESSAGES - sensor_msgs +set(MROVER_MESSAGE_DEPENDENCIES std_msgs sensor_msgs ) @@ -80,11 +84,12 @@ set(MROVER_PARAMETERS config/DetectorParams.cfg ) -# catkin packages list -set(MROVER_CATKIN_PACKAGES - roscpp rospy std_msgs message_runtime +set(MROVER_CMAKE_INCLUDES + starter_project/autonomy/AutonomyStarterProject.cmake ) +## Macros + macro(rosify_cpp_target_macro target) target_link_libraries(${target} PRIVATE ${catkin_LIBRARIES}) target_include_directories(${target} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS} "src/util") @@ -127,166 +132,78 @@ macro(add_gazebo_plugin_macro name sources includes) set_target_properties(${name} PROPERTIES CXX_STANDARD 17) endmacro() -# launch install macro -macro(install_launch_macro) - install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - ) -endmacro() - -macro(add_tests_macro) - # Add C++ unit tests - catkin_add_gtest(example-cpp-test test/example/cpp_test.cpp) - - # Python unit tests - catkin_add_nosetests(test/navigation/drive_test.py) - catkin_add_nosetests(test/teleop/teleop_test.py) - catkin_add_nosetests(test/util/SE3_test.py) - catkin_add_nosetests(test/util/SO3_test.py) - - # Integration tests (python and c++) - find_package(rostest REQUIRED) - add_rostest(test/example/basic_integration_test.test) - add_rostest(test/integration/integration.test) - add_rostest(test/util/SE3_tf_test.test) -endmacro() - -# Subdirectories before message declarations -set(MROVER_CMAKE_INCLUDES "") - - -# -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -# Specify build details by appending to lists -# and implementing the some extra macros -# Add new devices as elseif blocks -# Select device with --cmake-flags -D DEVICE= -# -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- - -if (${DEVICE} MATCHES "raspi4") - # -=-=-=-=- - # Lists - # -=-=-=-=- - - # Add any raspi4 specific packages here - # list(APPEND _PACKAGES ) - - # -=-=-=-=- - # Macros - # -=-=-=-=- - - macro(include_directories_macro) - include_directories( - ${catkin_INCLUDE_DIRS} - ) - endmacro() - - # define an add and link macro - # Put items here to build - macro(add_and_link_macro) - add_cpp_node_macro(brushed_motors "src/esw/brushed_motors/*") - endmacro() -else () - # -=-=-=-=- - # Lists - # -=-=-=-=- - - # Add any laptop specific packages here - list(APPEND MROVER_PACKAGES - tf2_geometry_msgs - tf2_ros - tf2 - tf2_web_republisher - visualization_msgs - image_transport - cv_bridge - rosbridge_server - teleop_twist_joy - gazebo_ros - ) - - # append subdirectories - list(APPEND MROVER_CMAKE_INCLUDES - starter_project/autonomy/AutonomyStarterProject.cmake - ) - - # -=-=-=-=- - # Macros - # -=-=-=-=- - - # These packages need to be found individually - macro(add_packages_macro) - find_package(OpenCV REQUIRED COMPONENTS core aruco) - find_package(gazebo REQUIRED) - find_package(Eigen3 REQUIRED) - find_package(ZED 2 QUIET) - if (ZED_FOUND) - # Anything newer than C++17 combined with libstdc++13 is not supported just yet by NVCC (the CUDA compiler) - set(CMAKE_CUDA_STANDARD 17) - set(CMAKE_CUDA_STANDARD_REQUIRED ON) - set(CMAKE_CUDA_SEPARABLE_COMPILATION ON) - # Jetson Xavier NX is Volta 7.2 architecture - # Perception Laptop (A4000, Quadro version of RTX 3080) is Ampere 8.6 architecture - set(CMAKE_CUDA_ARCHITECTURES 72 86) - enable_language(CUDA) - endif () - endmacro() - - # define an add and link macro - # Put items here to build - macro(add_and_link_macro) - add_cpp_nodelet_macro(tag_detector_nodelet "src/perception/tag_detector/*.cpp" "src/perception/tag_detector") - target_precompile_headers(tag_detector_nodelet PRIVATE src/perception/tag_detector/pch.hpp) - target_link_libraries(tag_detector_nodelet PRIVATE ${OpenCV_LIBRARIES} lie) - - add_cpp_node_macro(brushed_motors "src/esw/brushed_motors/*.cpp") - - add_cpp_library_macro(lie "src/util/lie/*.cpp" "src/util/lie") - - add_gazebo_plugin_macro(differential_drive_plugin_6w "src/gazebo/differential_drive_6w.cpp" "src") - - add_gazebo_plugin_macro(kinect_plugin "src/gazebo/gazebo_ros_openni_kinect.cpp" "src/gazebo") - target_link_libraries(kinect_plugin PRIVATE gazebo_ros_camera_utils DepthCameraPlugin Eigen3::Eigen) - set_target_properties(kinect_plugin PROPERTIES CXX_CLANG_TIDY "") - - if (ZED_FOUND) - # add_cpp_node_macro(zed_node "src/perception/zed_wrapper/*.c*") - # target_link_options(zed_node PRIVATE "LINKER:--copy-dt-needed-entries") - # target_include_directories(zed_node SYSTEM PRIVATE ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) - # target_link_libraries(zed_node PRIVATE ${ZED_LIBRARIES} ${OpenCV_LIBRARIES} ${SPECIAL_OS_LIBS} lie tag_detector_nodelet) - - add_cpp_nodelet_macro(zed_nodelet "src/perception/zed_wrapper/*.c*" , "src/perception/zed_wrapper") - target_precompile_headers(zed_nodelet PRIVATE src/perception/zed_wrapper/pch.hpp) - target_include_directories(zed_nodelet SYSTEM PRIVATE ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) - target_link_libraries(zed_nodelet PRIVATE ${ZED_LIBRARIES} ${OpenCV_LIBRARIES} ${SPECIAL_OS_LIBS} lie) - target_compile_definitions(zed_nodelet PRIVATE MROVER_IS_NODELET) - target_compile_options(zed_nodelet PRIVATE $<$:--expt-extended-lambda>) - endif () - endmacro() +## Dependencies + +find_package(ZED 2 QUIET) +find_package(gazebo REQUIRED) +find_package(Eigen3 REQUIRED) +if (ZED_FOUND) + # Anything newer than C++17 combined with libstdc++13 is not supported just yet by NVCC (the CUDA compiler) + set(CMAKE_CUDA_STANDARD 17) + set(CMAKE_CUDA_STANDARD_REQUIRED ON) + set(CMAKE_CUDA_SEPARABLE_COMPILATION ON) + # Jetson Xavier NX is Volta 7.2 architecture + # Perception Laptop (A4000, Quadro version of RTX 3080) is Ampere 8.6 architecture + set(CMAKE_CUDA_ARCHITECTURES 72 86) + enable_language(CUDA) endif () +set(BUILD_LIST "core,imgproc,objdetect") +set(BUILD_JAVA OFF) +set(BUILD_TESTS OFF) +set(BUILD_EXAMPLES OFF) +set(BUILD_PERF_TESTS OFF) +set(OPENCV_ENABLE_NONFREE ON) +# set(OPENCV_EXTRA_MODULES_PATH ${CMAKE_CURRENT_LIST_DIR}/deps/opencv_contrib/modules) +# set(WITH_CUDA ON) +set(WITH_EIGEN ON) +set(WITH_LAPACK ON) +set(WITH_LAPACK ON) +set(WITH_LAPACK ON) +set(CUDA_ARCH_BIN 8.6) +add_subdirectory(deps/opencv) + +#include(FetchContent) +#FetchContent_Declare( +# OpenCV +# SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/deps/opencv +# SYSTEM +# OVERRIDE_FIND_PACKAGE +#) +#FetchContent_MakeAvailable(OpenCV) +#if (NOT OpenCV_POPULATED) +# FetchContent_Populate(opencv) +# set(BUILD_LIST "core,objdetect") +# set(BUILD_JAVA OFF) +# set(BUILD_TESTS OFF) +# set(BUILD_EXAMPLES OFF) +# set(BUILD_PERF_TESTS OFF) +# set(OPENCV_ENABLE_NONFREE ON) +# set(OPENCV_EXTRA_MODULES_PATH ${CMAKE_CURRENT_LIST_DIR}/deps/opencv_contrib/modules) +# set(WITH_CUDA ON) +# set(WITH_EIGEN ON) +# set(WITH_LAPACK ON) +# set(CUDA_ARCH_BIN 8.6) +# add_subdirectory(${OpenCV_SOURCE_DIR} ${OpenCV_BINARY_DIR}) +#endif () +target_include_directories(opencv_core SYSTEM INTERFACE $) +target_include_directories(opencv_core SYSTEM INTERFACE $) +target_include_directories(opencv_imgproc SYSTEM INTERFACE $) +target_include_directories(opencv_objdetect SYSTEM INTERFACE $) -# 3. Find Packages find_package( catkin REQUIRED COMPONENTS - ${MROVER_PACKAGES} + ${MROVER_ROS_DEPENDENCIES} ) -if (COMMAND add_packages_macro) - add_packages_macro() -endif () - - -# 4. Python module support catkin_python_setup() - -# 4.5. CMake includes before message declarations -foreach (CMAKE_INCLUDE ${MROVER_CMAKE_INCLUDES}) - include(${CMAKE_INCLUDE}) +foreach (MROVER_CMAKE_INCLUDE ${MROVER_CMAKE_INCLUDES}) + include(${MROVER_CMAKE_INCLUDE}) endforeach () +## Messages -# 5. Message Generators (add_xxx) add_message_files( FILES ${MROVER_MESSAGE_FILES} @@ -297,38 +214,72 @@ add_service_files( ${MROVER_SERVICE_FILES} ) - -# 6. Invoke messages (generate_messages) generate_messages( DEPENDENCIES - ${MROVER_ROS_MESSAGES} + ${MROVER_MESSAGE_DEPENDENCIES} ) generate_dynamic_reconfigure_options( ${MROVER_PARAMETERS} ) +catkin_package() -# 7. Specify package build info export (catkin_package) -catkin_package( - CATKIN_DEPENDS - ${MROVER_CATKIN_PACKAGES} -) +## Targets +add_cpp_library_macro(lie src/util/lie/*.c* src/util/lie) -if (COMMAND add_and_link_macro) - add_and_link_macro() -endif () +add_cpp_nodelet_macro(tag_detector_nodelet src/perception/tag_detector/*.c* src/perception/tag_detector) +target_precompile_headers(tag_detector_nodelet PRIVATE src/perception/tag_detector/pch.hpp) +target_link_libraries(tag_detector_nodelet PRIVATE opencv_core opencv_objdetect opencv_imgproc lie) +add_cpp_node_macro(brushed_motors src/esw/brushed_motors/*.c*) -# 9. Tests to build -if (COMMAND add_tests_macro) - add_tests_macro() -endif () +if (ZED_FOUND) + add_cpp_library_macro(streaming src/esw/streaming/*.c* src/esw/streaming) + target_link_libraries(streaming PUBLIC /usr/lib/x86_64-linux-gnu/libnvidia-encode.so) + target_include_directories(streaming PUBLIC deps) + + add_cpp_node_macro(cameras src/esw/cameras/*.c*) + target_link_libraries(cameras PRIVATE streaming) + # add_cpp_node_macro(zed_node "src/perception/zed_wrapper/*.c*") + # target_link_options(zed_node PRIVATE "LINKER:--copy-dt-needed-entries") + # target_include_directories(zed_node SYSTEM PRIVATE ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) + # target_link_libraries(zed_node PRIVATE ${ZED_LIBRARIES} ${OpenCV_LIBRARIES} ${SPECIAL_OS_LIBS} lie tag_detector_nodelet) -# 10. Install rules -install_launch_macro() -if (COMMAND additional_install_macro) - additional_install_macro() + add_cpp_nodelet_macro(zed_nodelet src/perception/zed_wrapper/*.c* , src/perception/zed_wrapper) + target_precompile_headers(zed_nodelet PRIVATE src/perception/zed_wrapper/pch.hpp) + target_include_directories(zed_nodelet SYSTEM PRIVATE ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) + target_link_libraries(zed_nodelet PRIVATE ${ZED_LIBRARIES} ${SPECIAL_OS_LIBS} lie) + target_compile_definitions(zed_nodelet PRIVATE MROVER_IS_NODELET ALLOW_BUILD_DEBUG) endif () + +add_gazebo_plugin_macro(differential_drive_plugin_6w src/gazebo/differential_drive_6w.cpp src) + +add_gazebo_plugin_macro(kinect_plugin src/gazebo/gazebo_ros_openni_kinect.cpp src/gazebo) +target_link_libraries(kinect_plugin PRIVATE gazebo_ros_camera_utils DepthCameraPlugin Eigen3::Eigen) +set_target_properties(kinect_plugin PROPERTIES CXX_CLANG_TIDY "") + +## Testing + +# Add C++ unit tests +catkin_add_gtest(example-cpp-test test/example/cpp_test.cpp) + +# Python unit tests +catkin_add_nosetests(test/navigation/drive_test.py) +catkin_add_nosetests(test/teleop/teleop_test.py) +catkin_add_nosetests(test/util/SE3_test.py) +catkin_add_nosetests(test/util/SO3_test.py) + +# Integration tests (python and c++) +find_package(rostest REQUIRED) +add_rostest(test/example/basic_integration_test.test) +add_rostest(test/integration/integration.test) +add_rostest(test/util/SE3_tf_test.test) + +## Install + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) \ No newline at end of file diff --git a/deps/opencv b/deps/opencv new file mode 160000 index 000000000..f9a59f259 --- /dev/null +++ b/deps/opencv @@ -0,0 +1 @@ +Subproject commit f9a59f2592993d3dcc080e495f4f5e02dd8ec7ef diff --git a/package.xml b/package.xml index e87ee3c2c..ab077d7fe 100644 --- a/package.xml +++ b/package.xml @@ -56,8 +56,6 @@ xacro joint_state_publisher robot_state_publisher - rosbridge_server - tf2_web_republisher rviz @@ -68,9 +66,6 @@ tf2_ros tf2_geometry_msgs image_transport - theora_image_transport - compressed_image_transport - compressed_depth_image_transport sensor_msgs dynamic_reconfigure diff --git a/src/esw/cameras/cameras.cpp b/src/esw/cameras/cameras.cpp new file mode 100644 index 000000000..7429961d6 --- /dev/null +++ b/src/esw/cameras/cameras.cpp @@ -0,0 +1,5 @@ +#include + +int main() { + Streamer streamer{}; +} \ No newline at end of file diff --git a/src/esw/streaming/streaming.cu b/src/esw/streaming/streaming.cu new file mode 100644 index 000000000..f473405f4 --- /dev/null +++ b/src/esw/streaming/streaming.cu @@ -0,0 +1,24 @@ +#include + +#include "streaming.hpp" + +#include +#include + +void NvCheck(NVENCSTATUS status) { + if (status != NV_ENC_SUCCESS) { + throw std::runtime_error("NvEnc failed"); + } +} + +Streamer::Streamer() { + NvCheck(NvEncodeAPICreateInstance(&m_nvenc)); + NV_ENC_OPEN_ENCODE_SESSION_EX_PARAMS params{ + .version = NV_ENC_OPEN_ENCODE_SESSION_EX_PARAMS_VER, + .deviceType = NV_ENC_DEVICE_TYPE_CUDA, + .device = nullptr, + .apiVersion = NVENCAPI_VERSION, + }; + void* encoder = nullptr; + NvCheck(m_nvenc.nvEncOpenEncodeSessionEx(¶ms, &encoder)); +} diff --git a/src/esw/streaming/streaming.hpp b/src/esw/streaming/streaming.hpp new file mode 100644 index 000000000..e08559a38 --- /dev/null +++ b/src/esw/streaming/streaming.hpp @@ -0,0 +1,10 @@ +#pragma once + +#include + +struct Streamer { + + NV_ENCODE_API_FUNCTION_LIST m_nvenc{NV_ENCODE_API_FUNCTION_LIST_VER}; + + Streamer(); +}; \ No newline at end of file diff --git a/src/perception/tag_detector/pch.hpp b/src/perception/tag_detector/pch.hpp index 538fcd197..73fb2dd2b 100644 --- a/src/perception/tag_detector/pch.hpp +++ b/src/perception/tag_detector/pch.hpp @@ -14,7 +14,7 @@ #include -#include +#include #include #include diff --git a/src/perception/tag_detector/tag_detector.cpp b/src/perception/tag_detector/tag_detector.cpp index 7daea870b..8ba467da9 100644 --- a/src/perception/tag_detector/tag_detector.cpp +++ b/src/perception/tag_detector/tag_detector.cpp @@ -5,8 +5,7 @@ namespace mrover { void TagDetectorNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mDetectorParams = new cv::aruco::DetectorParameters(); - auto defaultDetectorParams = cv::aruco::DetectorParameters::create(); + cv::aruco::DetectorParameters defaultDetectorParams; int dictionaryNumber; mNh.param("use_odom_frame", mUseOdom, false); @@ -15,7 +14,7 @@ namespace mrover { mNh.param("camera_frame", mCameraFrameId, "zed2i_left_camera_frame"); mPnh.param("publish_images", mPublishImages, true); - using DictEnumType = std::underlying_type_t; + using DictEnumType = std::underlying_type_t; mPnh.param("dictionary", dictionaryNumber, static_cast(cv::aruco::DICT_4X4_50)); mPnh.param("min_hit_count_before_publish", mMinHitCountBeforePublish, 5); mPnh.param("max_hit_count", mMaxHitCount, 5); @@ -34,21 +33,21 @@ namespace mrover { mConfigServer.setCallback(mCallbackType); mPnh.param("adaptiveThreshConstant", - mDetectorParams->adaptiveThreshConstant, defaultDetectorParams->adaptiveThreshConstant); + mDetectorParams.adaptiveThreshConstant, defaultDetectorParams.adaptiveThreshConstant); mPnh.param("adaptiveThreshWinSizeMax", - mDetectorParams->adaptiveThreshWinSizeMax, defaultDetectorParams->adaptiveThreshWinSizeMax); + mDetectorParams.adaptiveThreshWinSizeMax, defaultDetectorParams.adaptiveThreshWinSizeMax); mPnh.param("adaptiveThreshWinSizeMin", - mDetectorParams->adaptiveThreshWinSizeMin, defaultDetectorParams->adaptiveThreshWinSizeMin); + mDetectorParams.adaptiveThreshWinSizeMin, defaultDetectorParams.adaptiveThreshWinSizeMin); mPnh.param("adaptiveThreshWinSizeStep", - mDetectorParams->adaptiveThreshWinSizeStep, defaultDetectorParams->adaptiveThreshWinSizeStep); + mDetectorParams.adaptiveThreshWinSizeStep, defaultDetectorParams.adaptiveThreshWinSizeStep); mPnh.param("cornerRefinementMaxIterations", - mDetectorParams->cornerRefinementMaxIterations, - defaultDetectorParams->cornerRefinementMaxIterations); + mDetectorParams.cornerRefinementMaxIterations, + defaultDetectorParams.cornerRefinementMaxIterations); mPnh.param("cornerRefinementMinAccuracy", - mDetectorParams->cornerRefinementMinAccuracy, - defaultDetectorParams->cornerRefinementMinAccuracy); + mDetectorParams.cornerRefinementMinAccuracy, + defaultDetectorParams.cornerRefinementMinAccuracy); mPnh.param("cornerRefinementWinSize", - mDetectorParams->cornerRefinementWinSize, defaultDetectorParams->cornerRefinementWinSize); + mDetectorParams.cornerRefinementWinSize, defaultDetectorParams.cornerRefinementWinSize); bool doCornerRefinement = true; mPnh.param("doCornerRefinement", doCornerRefinement, true); @@ -56,41 +55,41 @@ namespace mrover { bool cornerRefinementSubPix = true; mPnh.param("cornerRefinementSubPix", cornerRefinementSubPix, true); if (cornerRefinementSubPix) { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; + mDetectorParams.cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; } else { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR; + mDetectorParams.cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR; } } else { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; + mDetectorParams.cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; } mPnh.param("errorCorrectionRate", - mDetectorParams->errorCorrectionRate, defaultDetectorParams->errorCorrectionRate); + mDetectorParams.errorCorrectionRate, defaultDetectorParams.errorCorrectionRate); mPnh.param("minCornerDistanceRate", - mDetectorParams->minCornerDistanceRate, defaultDetectorParams->minCornerDistanceRate); + mDetectorParams.minCornerDistanceRate, defaultDetectorParams.minCornerDistanceRate); mPnh.param("markerBorderBits", - mDetectorParams->markerBorderBits, defaultDetectorParams->markerBorderBits); + mDetectorParams.markerBorderBits, defaultDetectorParams.markerBorderBits); mPnh.param("maxErroneousBitsInBorderRate", - mDetectorParams->maxErroneousBitsInBorderRate, - defaultDetectorParams->maxErroneousBitsInBorderRate); + mDetectorParams.maxErroneousBitsInBorderRate, + defaultDetectorParams.maxErroneousBitsInBorderRate); mPnh.param("minDistanceToBorder", - mDetectorParams->minDistanceToBorder, defaultDetectorParams->minDistanceToBorder); + mDetectorParams.minDistanceToBorder, defaultDetectorParams.minDistanceToBorder); mPnh.param("minMarkerDistanceRate", - mDetectorParams->minMarkerDistanceRate, defaultDetectorParams->minMarkerDistanceRate); + mDetectorParams.minMarkerDistanceRate, defaultDetectorParams.minMarkerDistanceRate); mPnh.param("minMarkerPerimeterRate", - mDetectorParams->minMarkerPerimeterRate, defaultDetectorParams->minMarkerPerimeterRate); + mDetectorParams.minMarkerPerimeterRate, defaultDetectorParams.minMarkerPerimeterRate); mPnh.param("maxMarkerPerimeterRate", - mDetectorParams->maxMarkerPerimeterRate, defaultDetectorParams->maxMarkerPerimeterRate); - mPnh.param("minOtsuStdDev", mDetectorParams->minOtsuStdDev, defaultDetectorParams->minOtsuStdDev); + mDetectorParams.maxMarkerPerimeterRate, defaultDetectorParams.maxMarkerPerimeterRate); + mPnh.param("minOtsuStdDev", mDetectorParams.minOtsuStdDev, defaultDetectorParams.minOtsuStdDev); mPnh.param("perspectiveRemoveIgnoredMarginPerCell", - mDetectorParams->perspectiveRemoveIgnoredMarginPerCell, - defaultDetectorParams->perspectiveRemoveIgnoredMarginPerCell); + mDetectorParams.perspectiveRemoveIgnoredMarginPerCell, + defaultDetectorParams.perspectiveRemoveIgnoredMarginPerCell); mPnh.param("perspectiveRemovePixelPerCell", - mDetectorParams->perspectiveRemovePixelPerCell, - defaultDetectorParams->perspectiveRemovePixelPerCell); + mDetectorParams.perspectiveRemovePixelPerCell, + defaultDetectorParams.perspectiveRemovePixelPerCell); mPnh.param("polygonalApproxAccuracyRate", - mDetectorParams->polygonalApproxAccuracyRate, - defaultDetectorParams->polygonalApproxAccuracyRate); + mDetectorParams.polygonalApproxAccuracyRate, + defaultDetectorParams.polygonalApproxAccuracyRate); NODELET_INFO("Tag detection ready, use odom frame: %s, min hit count: %d, max hit count: %d, hit increment weight: %d, hit decrement weight: %d", mUseOdom ? "true" : "false", mMinHitCountBeforePublish, mMaxHitCount, mTagIncrementWeight, mTagDecrementWeight); } @@ -99,34 +98,34 @@ namespace mrover { // Don't load initial config, since it will overwrite the rosparam settings if (level == std::numeric_limits::max()) return; - mDetectorParams->adaptiveThreshConstant = config.adaptiveThreshConstant; - mDetectorParams->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin; - mDetectorParams->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax; - mDetectorParams->adaptiveThreshWinSizeStep = config.adaptiveThreshWinSizeStep; - mDetectorParams->cornerRefinementMaxIterations = config.cornerRefinementMaxIterations; - mDetectorParams->cornerRefinementMinAccuracy = config.cornerRefinementMinAccuracy; - mDetectorParams->cornerRefinementWinSize = config.cornerRefinementWinSize; + mDetectorParams.adaptiveThreshConstant = config.adaptiveThreshConstant; + mDetectorParams.adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin; + mDetectorParams.adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax; + mDetectorParams.adaptiveThreshWinSizeStep = config.adaptiveThreshWinSizeStep; + mDetectorParams.cornerRefinementMaxIterations = config.cornerRefinementMaxIterations; + mDetectorParams.cornerRefinementMinAccuracy = config.cornerRefinementMinAccuracy; + mDetectorParams.cornerRefinementWinSize = config.cornerRefinementWinSize; if (config.doCornerRefinement) { if (config.cornerRefinementSubpix) { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; + mDetectorParams.cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; } else { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR; + mDetectorParams.cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR; } } else { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; + mDetectorParams.cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; } - mDetectorParams->errorCorrectionRate = config.errorCorrectionRate; - mDetectorParams->minCornerDistanceRate = config.minCornerDistanceRate; - mDetectorParams->markerBorderBits = config.markerBorderBits; - mDetectorParams->maxErroneousBitsInBorderRate = config.maxErroneousBitsInBorderRate; - mDetectorParams->minDistanceToBorder = config.minDistanceToBorder; - mDetectorParams->minMarkerDistanceRate = config.minMarkerDistanceRate; - mDetectorParams->minMarkerPerimeterRate = config.minMarkerPerimeterRate; - mDetectorParams->maxMarkerPerimeterRate = config.maxMarkerPerimeterRate; - mDetectorParams->minOtsuStdDev = config.minOtsuStdDev; - mDetectorParams->perspectiveRemoveIgnoredMarginPerCell = config.perspectiveRemoveIgnoredMarginPerCell; - mDetectorParams->perspectiveRemovePixelPerCell = config.perspectiveRemovePixelPerCell; - mDetectorParams->polygonalApproxAccuracyRate = config.polygonalApproxAccuracyRate; + mDetectorParams.errorCorrectionRate = config.errorCorrectionRate; + mDetectorParams.minCornerDistanceRate = config.minCornerDistanceRate; + mDetectorParams.markerBorderBits = config.markerBorderBits; + mDetectorParams.maxErroneousBitsInBorderRate = config.maxErroneousBitsInBorderRate; + mDetectorParams.minDistanceToBorder = config.minDistanceToBorder; + mDetectorParams.minMarkerDistanceRate = config.minMarkerDistanceRate; + mDetectorParams.minMarkerPerimeterRate = config.minMarkerPerimeterRate; + mDetectorParams.maxMarkerPerimeterRate = config.maxMarkerPerimeterRate; + mDetectorParams.minOtsuStdDev = config.minOtsuStdDev; + mDetectorParams.perspectiveRemoveIgnoredMarginPerCell = config.perspectiveRemoveIgnoredMarginPerCell; + mDetectorParams.perspectiveRemovePixelPerCell = config.perspectiveRemovePixelPerCell; + mDetectorParams.polygonalApproxAccuracyRate = config.polygonalApproxAccuracyRate; } bool TagDetectorNodelet::enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { diff --git a/src/perception/tag_detector/tag_detector.hpp b/src/perception/tag_detector/tag_detector.hpp index 616405033..a86747aee 100644 --- a/src/perception/tag_detector/tag_detector.hpp +++ b/src/perception/tag_detector/tag_detector.hpp @@ -33,8 +33,9 @@ namespace mrover { int mTagIncrementWeight{}; int mTagDecrementWeight{}; - cv::Ptr mDetectorParams; - cv::Ptr mDictionary; + cv::aruco::ArucoDetector mDetector; + cv::aruco::DetectorParameters mDetectorParams; + cv::aruco::Dictionary mDictionary; cv::Mat mImg; cv::Mat mGrayImg; diff --git a/src/perception/tag_detector/tag_detector.processing.cpp b/src/perception/tag_detector/tag_detector.processing.cpp index b1033d61d..2fae72b73 100644 --- a/src/perception/tag_detector/tag_detector.processing.cpp +++ b/src/perception/tag_detector/tag_detector.processing.cpp @@ -67,7 +67,9 @@ namespace mrover { // Detect the tag vertices in screen space and their respective ids // {mImmediateCorneres, mImmediateIds} are the outputs from OpenCV - cv::aruco::detectMarkers(mImg, mDictionary, mImmediateCorners, mImmediateIds, mDetectorParams); + mDetector.setDictionary(mDictionary); + mDetector.setDetectorParameters(mDetectorParams); + mDetector.detectMarkers(mImg, mImmediateCorners, mImmediateIds); NODELET_DEBUG("OpenCV detect size: %zu", mImmediateIds.size()); mProfiler.measureEvent("OpenCV Detect"); diff --git a/src/perception/tag_detector/tag_detector.threshold.cpp b/src/perception/tag_detector/tag_detector.threshold.cpp index b368df16c..3127805ce 100644 --- a/src/perception/tag_detector/tag_detector.threshold.cpp +++ b/src/perception/tag_detector/tag_detector.threshold.cpp @@ -19,7 +19,7 @@ namespace mrover { cvtColor(mImg, mGrayImg, cv::COLOR_BGR2GRAY); // number of window sizes (scales) to apply adaptive thresholding - int scaleCount = (mDetectorParams->adaptiveThreshWinSizeMax - mDetectorParams->adaptiveThreshWinSizeMin) / mDetectorParams->adaptiveThreshWinSizeStep + 1; + int scaleCount = (mDetectorParams.adaptiveThreshWinSizeMax - mDetectorParams.adaptiveThreshWinSizeMin) / mDetectorParams.adaptiveThreshWinSizeStep + 1; // for each value in the interval of thresholding window sizes for (int scale = 0; scale < scaleCount; ++scale) { @@ -32,8 +32,8 @@ namespace mrover { if (publisher.getNumSubscribers() == 0) continue; - int windowSize = mDetectorParams->adaptiveThreshWinSizeMin + scale * mDetectorParams->adaptiveThreshWinSizeStep; - threshold(mGrayImg, mGrayImg, windowSize, mDetectorParams->adaptiveThreshConstant); + int windowSize = mDetectorParams.adaptiveThreshWinSizeMin + scale * mDetectorParams.adaptiveThreshWinSizeStep; + threshold(mGrayImg, mGrayImg, windowSize, mDetectorParams.adaptiveThreshConstant); mThreshMsg.header.seq = mSeqNum; mThreshMsg.header.stamp = ros::Time::now(); diff --git a/starter_project/autonomy/AutonomyStarterProject.cmake b/starter_project/autonomy/AutonomyStarterProject.cmake index 4b84c75af..76c8c1b08 100644 --- a/starter_project/autonomy/AutonomyStarterProject.cmake +++ b/starter_project/autonomy/AutonomyStarterProject.cmake @@ -18,7 +18,7 @@ add_executable(starter_project_perception ${STARTER_PROJECT_PERCEPTION_SOURCES}) # Ensure that our project builds after message generation add_dependencies(starter_project_perception ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # Link needed libraries -target_link_libraries(starter_project_perception ${catkin_LIBRARIES} ${OpenCV_LIBS}) +target_link_libraries(starter_project_perception ${catkin_LIBRARIES} opencv_core opencv_objdetect) # Include needed directories target_include_directories(starter_project_perception PUBLIC ${catkin_INCLUDE_DIRS}) diff --git a/starter_project/autonomy/src/perception.cpp b/starter_project/autonomy/src/perception.cpp index 2185a5fe2..b652565a5 100644 --- a/starter_project/autonomy/src/perception.cpp +++ b/starter_project/autonomy/src/perception.cpp @@ -28,26 +28,25 @@ namespace mrover { // TODO: uncomment me! // mTagPublisher = mNodeHandle.advertise("tag", 1); - mTagDetectorParams = cv::aruco::DetectorParameters::create(); - mTagDictionary = cv::aruco::getPredefinedDictionary(0); + mTagDictionary = cv::aruco::getPredefinedDictionary(0) ; } - void Perception::imageCallback(sensor_msgs::ImageConstPtr const& image) { + void Perception::imageCallback(sensor_msgs::ImageConstPtr const& imageMessage) { // Create a cv::Mat from the ROS image message // Note this does not copy the image data, it is basically a pointer // Be careful if you extend its lifetime beyond this function - cv::Mat cvImage{static_cast(image->height), static_cast(image->width), - CV_8UC3, const_cast(image->data.data())}; + cv::Mat image{static_cast(imageMessage->height), static_cast(imageMessage->width), + CV_8UC3, const_cast(imageMessage->data.data())}; // Detect tags in the image pixels - findTagsInImage(cvImage, mTags); + findTagsInImage(image, mTags); // Select the tag that is closest to the middle of the screen - StarterProjectTag tag = selectTag(mTags); + StarterProjectTag tag = selectTag(image, mTags); // Publish the message to our topic so navigation or others can receive it publishTag(tag); } void Perception::findTagsInImage(cv::Mat const& image, std::vector& tags) { // NOLINT(*-convert-member-functions-to-static) - // hint: take a look at OpenCV's documentation for the detectMarkers function + // hint: take a look at OpenCV's documentation for the detectMarkers function that can be called on mTagDetector // hint: you have mTagDictionary, mTagCorners, mTagIds, and mTagDetectorParams member variables already defined! // hint: write and use the "getCenterFromTagCorners" and "getClosenessMetricFromTagCorners" functions @@ -56,7 +55,7 @@ namespace mrover { // TODO: implement me! } - StarterProjectTag Perception::selectTag(std::vector const& tags) { // NOLINT(*-convert-member-functions-to-static) + StarterProjectTag Perception::selectTag(cv::Mat const& image, std::vector const& tags) { // NOLINT(*-convert-member-functions-to-static) // TODO: implement me! return {}; } diff --git a/starter_project/autonomy/src/perception.hpp b/starter_project/autonomy/src/perception.hpp index d690e2486..23e3c1f89 100644 --- a/starter_project/autonomy/src/perception.hpp +++ b/starter_project/autonomy/src/perception.hpp @@ -8,8 +8,8 @@ #include // OpenCV Headers, cv namespace -#include #include +#include // ROS Headers, ros namespace #include @@ -38,8 +38,9 @@ namespace mrover { ros::NodeHandle mNodeHandle; image_transport::ImageTransport mImageTransport; image_transport::Subscriber mImageSubscriber; - cv::Ptr mTagDetectorParams; - cv::Ptr mTagDictionary; + cv::aruco::ArucoDetector mTagDetector; + cv::aruco::DetectorParameters mTagDetectorParams; + cv::aruco::Dictionary mTagDictionary; std::vector> mTagCorners; std::vector mTagIds; std::vector mTags; @@ -52,9 +53,9 @@ namespace mrover { * Called when we receive a new image message from the camera. * Specifically this is one frame. * - * @param image + * @param imageMessage */ - void imageCallback(sensor_msgs::ImageConstPtr const& image); + void imageCallback(sensor_msgs::ImageConstPtr const& imageMessage); /** * Given an image, detect ArUco tags, and fill a vector full of output messages. @@ -94,7 +95,7 @@ namespace mrover { * @param tags Vector of tags * @return Center tag */ - [[nodiscard]] StarterProjectTag selectTag(std::vector const& tags); + [[nodiscard]] StarterProjectTag selectTag(cv::Mat const& image, std::vector const& tags); }; } // namespace mrover From 5ab8d47e34c32bbfa6c55335b3fde6a9eb8cb049 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 1 Oct 2023 21:03:13 -0400 Subject: [PATCH 014/197] Clean starting point --- .gitmodules | 3 --- deps/opencv | 1 - src/esw/cameras/cameras.cpp | 5 ----- src/esw/streaming/streaming.cu | 24 ------------------------ src/esw/streaming/streaming.hpp | 10 ---------- 5 files changed, 43 deletions(-) delete mode 160000 deps/opencv delete mode 100644 src/esw/cameras/cameras.cpp delete mode 100644 src/esw/streaming/streaming.cu delete mode 100644 src/esw/streaming/streaming.hpp diff --git a/.gitmodules b/.gitmodules index b179da5f2..e69de29bb 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +0,0 @@ -[submodule "deps/opencv"] - path = deps/opencv - url = https://github.com/opencv/opencv.git diff --git a/deps/opencv b/deps/opencv deleted file mode 160000 index f9a59f259..000000000 --- a/deps/opencv +++ /dev/null @@ -1 +0,0 @@ -Subproject commit f9a59f2592993d3dcc080e495f4f5e02dd8ec7ef diff --git a/src/esw/cameras/cameras.cpp b/src/esw/cameras/cameras.cpp deleted file mode 100644 index 7429961d6..000000000 --- a/src/esw/cameras/cameras.cpp +++ /dev/null @@ -1,5 +0,0 @@ -#include - -int main() { - Streamer streamer{}; -} \ No newline at end of file diff --git a/src/esw/streaming/streaming.cu b/src/esw/streaming/streaming.cu deleted file mode 100644 index f473405f4..000000000 --- a/src/esw/streaming/streaming.cu +++ /dev/null @@ -1,24 +0,0 @@ -#include - -#include "streaming.hpp" - -#include -#include - -void NvCheck(NVENCSTATUS status) { - if (status != NV_ENC_SUCCESS) { - throw std::runtime_error("NvEnc failed"); - } -} - -Streamer::Streamer() { - NvCheck(NvEncodeAPICreateInstance(&m_nvenc)); - NV_ENC_OPEN_ENCODE_SESSION_EX_PARAMS params{ - .version = NV_ENC_OPEN_ENCODE_SESSION_EX_PARAMS_VER, - .deviceType = NV_ENC_DEVICE_TYPE_CUDA, - .device = nullptr, - .apiVersion = NVENCAPI_VERSION, - }; - void* encoder = nullptr; - NvCheck(m_nvenc.nvEncOpenEncodeSessionEx(¶ms, &encoder)); -} diff --git a/src/esw/streaming/streaming.hpp b/src/esw/streaming/streaming.hpp deleted file mode 100644 index e08559a38..000000000 --- a/src/esw/streaming/streaming.hpp +++ /dev/null @@ -1,10 +0,0 @@ -#pragma once - -#include - -struct Streamer { - - NV_ENCODE_API_FUNCTION_LIST m_nvenc{NV_ENCODE_API_FUNCTION_LIST_VER}; - - Streamer(); -}; \ No newline at end of file From fa185e4b7232d9fe0140133387ecd774b06ea034 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 1 Oct 2023 21:05:39 -0400 Subject: [PATCH 015/197] Cleanup cmake --- CMakeLists.txt | 56 ++++---------------------------------------------- 1 file changed, 4 insertions(+), 52 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e8de2de68..255ab4871 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,6 +134,7 @@ endmacro() ## Dependencies +find_package(OpenCV 4.8.1 EXACT REQUIRED) find_package(ZED 2 QUIET) find_package(gazebo REQUIRED) find_package(Eigen3 REQUIRED) @@ -142,55 +143,13 @@ if (ZED_FOUND) set(CMAKE_CUDA_STANDARD 17) set(CMAKE_CUDA_STANDARD_REQUIRED ON) set(CMAKE_CUDA_SEPARABLE_COMPILATION ON) + set(CMAKE_CUDA_FLAGS "--diag-suppress 108,68") # Jetson Xavier NX is Volta 7.2 architecture # Perception Laptop (A4000, Quadro version of RTX 3080) is Ampere 8.6 architecture set(CMAKE_CUDA_ARCHITECTURES 72 86) enable_language(CUDA) endif () -set(BUILD_LIST "core,imgproc,objdetect") -set(BUILD_JAVA OFF) -set(BUILD_TESTS OFF) -set(BUILD_EXAMPLES OFF) -set(BUILD_PERF_TESTS OFF) -set(OPENCV_ENABLE_NONFREE ON) -# set(OPENCV_EXTRA_MODULES_PATH ${CMAKE_CURRENT_LIST_DIR}/deps/opencv_contrib/modules) -# set(WITH_CUDA ON) -set(WITH_EIGEN ON) -set(WITH_LAPACK ON) -set(WITH_LAPACK ON) -set(WITH_LAPACK ON) -set(CUDA_ARCH_BIN 8.6) -add_subdirectory(deps/opencv) - -#include(FetchContent) -#FetchContent_Declare( -# OpenCV -# SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/deps/opencv -# SYSTEM -# OVERRIDE_FIND_PACKAGE -#) -#FetchContent_MakeAvailable(OpenCV) -#if (NOT OpenCV_POPULATED) -# FetchContent_Populate(opencv) -# set(BUILD_LIST "core,objdetect") -# set(BUILD_JAVA OFF) -# set(BUILD_TESTS OFF) -# set(BUILD_EXAMPLES OFF) -# set(BUILD_PERF_TESTS OFF) -# set(OPENCV_ENABLE_NONFREE ON) -# set(OPENCV_EXTRA_MODULES_PATH ${CMAKE_CURRENT_LIST_DIR}/deps/opencv_contrib/modules) -# set(WITH_CUDA ON) -# set(WITH_EIGEN ON) -# set(WITH_LAPACK ON) -# set(CUDA_ARCH_BIN 8.6) -# add_subdirectory(${OpenCV_SOURCE_DIR} ${OpenCV_BINARY_DIR}) -#endif () -target_include_directories(opencv_core SYSTEM INTERFACE $) -target_include_directories(opencv_core SYSTEM INTERFACE $) -target_include_directories(opencv_imgproc SYSTEM INTERFACE $) -target_include_directories(opencv_objdetect SYSTEM INTERFACE $) - find_package( catkin REQUIRED COMPONENTS ${MROVER_ROS_DEPENDENCIES} @@ -236,13 +195,6 @@ target_link_libraries(tag_detector_nodelet PRIVATE opencv_core opencv_objdetect add_cpp_node_macro(brushed_motors src/esw/brushed_motors/*.c*) if (ZED_FOUND) - add_cpp_library_macro(streaming src/esw/streaming/*.c* src/esw/streaming) - target_link_libraries(streaming PUBLIC /usr/lib/x86_64-linux-gnu/libnvidia-encode.so) - target_include_directories(streaming PUBLIC deps) - - add_cpp_node_macro(cameras src/esw/cameras/*.c*) - target_link_libraries(cameras PRIVATE streaming) - # add_cpp_node_macro(zed_node "src/perception/zed_wrapper/*.c*") # target_link_options(zed_node PRIVATE "LINKER:--copy-dt-needed-entries") # target_include_directories(zed_node SYSTEM PRIVATE ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) @@ -252,7 +204,7 @@ if (ZED_FOUND) target_precompile_headers(zed_nodelet PRIVATE src/perception/zed_wrapper/pch.hpp) target_include_directories(zed_nodelet SYSTEM PRIVATE ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) target_link_libraries(zed_nodelet PRIVATE ${ZED_LIBRARIES} ${SPECIAL_OS_LIBS} lie) - target_compile_definitions(zed_nodelet PRIVATE MROVER_IS_NODELET ALLOW_BUILD_DEBUG) + target_compile_definitions(zed_nodelet PRIVATE MROVER_IS_NODELET ALLOW_BUILD_DEBUG __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__) endif () add_gazebo_plugin_macro(differential_drive_plugin_6w src/gazebo/differential_drive_6w.cpp src) @@ -282,4 +234,4 @@ add_rostest(test/util/SE3_tf_test.test) install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) \ No newline at end of file +) From 5b538477f07ad6dcbea3f86ce163bc118f5067a2 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 1 Oct 2023 21:13:40 -0400 Subject: [PATCH 016/197] Add starter files --- CMakeLists.txt | 4 +++ package.xml | 3 +- plugins/object_detector_plugin.xml | 6 ++++ .../object_detector/object_detector.cpp | 25 +++++++++++++++ .../object_detector/object_detector.hpp | 23 +++++++++++++ .../object_detector.processing.cpp | 11 +++++++ src/perception/object_detector/pch.hpp | 32 +++++++++++++++++++ src/perception/tag_detector/tag_detector.cpp | 12 +++---- src/perception/tag_detector/tag_detector.hpp | 6 ++-- .../tag_detector/tag_detector.processing.cpp | 4 +-- .../tag_detector/tag_detector.threshold.cpp | 2 +- 11 files changed, 115 insertions(+), 13 deletions(-) create mode 100644 plugins/object_detector_plugin.xml create mode 100644 src/perception/object_detector/object_detector.cpp create mode 100644 src/perception/object_detector/object_detector.hpp create mode 100644 src/perception/object_detector/object_detector.processing.cpp create mode 100644 src/perception/object_detector/pch.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 255ab4871..0721aff35 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -200,6 +200,10 @@ if (ZED_FOUND) # target_include_directories(zed_node SYSTEM PRIVATE ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) # target_link_libraries(zed_node PRIVATE ${ZED_LIBRARIES} ${OpenCV_LIBRARIES} ${SPECIAL_OS_LIBS} lie tag_detector_nodelet) + add_cpp_nodelet_macro(object_detector_nodelet src/perception/object_detector/*.c* src/object_detector_nodelet/tag_detector) + target_precompile_headers(object_detector_nodelet PRIVATE src/perception/object_detector/pch.hpp) + target_link_libraries(object_detector_nodelet PRIVATE opencv_core opencv_dnn opencv_imgproc lie) + add_cpp_nodelet_macro(zed_nodelet src/perception/zed_wrapper/*.c* , src/perception/zed_wrapper) target_precompile_headers(zed_nodelet PRIVATE src/perception/zed_wrapper/pch.hpp) target_include_directories(zed_nodelet SYSTEM PRIVATE ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) diff --git a/package.xml b/package.xml index ab077d7fe..a2701d6c3 100644 --- a/package.xml +++ b/package.xml @@ -92,7 +92,8 @@ - + + diff --git a/plugins/object_detector_plugin.xml b/plugins/object_detector_plugin.xml new file mode 100644 index 000000000..36ddebcde --- /dev/null +++ b/plugins/object_detector_plugin.xml @@ -0,0 +1,6 @@ + + + + diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp new file mode 100644 index 000000000..5d2de01df --- /dev/null +++ b/src/perception/object_detector/object_detector.cpp @@ -0,0 +1,25 @@ +#include "object_detector.hpp" + +namespace mrover { + + void ObjectDetectorNodelet::onInit() { + mNh = getMTNodeHandle(); + mPnh = getMTPrivateNodeHandle(); + } + +} // namespace mrover + +int main(int argc, char** argv) { + ros::init(argc, argv, "object_detector"); + + // Start the ZED Nodelet + nodelet::Loader nodelet; + nodelet.load(ros::this_node::getName(), "mrover/ObjectDetectorNodelet", ros::names::getRemappings(), {}); + + ros::spin(); + + return EXIT_SUCCESS; +} + +#include +PLUGINLIB_EXPORT_CLASS(mrover::ObjectDetectorNodelet, nodelet::Nodelet) diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp new file mode 100644 index 000000000..f11e6d1df --- /dev/null +++ b/src/perception/object_detector/object_detector.hpp @@ -0,0 +1,23 @@ +#include "pch.hpp" + +namespace mrover { + + class ObjectDetectorNodelet : public nodelet::Nodelet { + private: + ros::NodeHandle mNh, mPnh; + + LoopProfiler mProfiler{"Object Detector"}; + + void onInit() override; + + public: + ObjectDetectorNodelet() = default; + + ~ObjectDetectorNodelet() override = default; + + void imageCallback(sensor_msgs::ImageConstPtr const& msg); + + bool enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); + }; + +} // namespace mrover diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp new file mode 100644 index 000000000..5727ada67 --- /dev/null +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -0,0 +1,11 @@ +#include "object_detector.hpp" + +namespace mrover { + + void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { + assert(msg); + assert(msg->height > 0); + assert(msg->width > 0); + } + +} // namespace mrover diff --git a/src/perception/object_detector/pch.hpp b/src/perception/object_detector/pch.hpp new file mode 100644 index 000000000..2c6453d0b --- /dev/null +++ b/src/perception/object_detector/pch.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include diff --git a/src/perception/tag_detector/tag_detector.cpp b/src/perception/tag_detector/tag_detector.cpp index 8ba467da9..bbe8c6fc6 100644 --- a/src/perception/tag_detector/tag_detector.cpp +++ b/src/perception/tag_detector/tag_detector.cpp @@ -2,7 +2,7 @@ namespace mrover { - void TagDetectorNodelet::onInit() { + void ObjectDetectorNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); cv::aruco::DetectorParameters defaultDetectorParams; @@ -25,8 +25,8 @@ namespace mrover { mImgPub = mIt->advertise("tag_detection", 1); mDictionary = cv::aruco::getPredefinedDictionary(dictionaryNumber); - mPcSub = mNh.subscribe("camera/left/points", 1, &TagDetectorNodelet::pointCloudCallback, this); - mServiceEnableDetections = mNh.advertiseService("enable_detections", &TagDetectorNodelet::enableDetectionsCallback, this); + mPcSub = mNh.subscribe("camera/left/points", 1, &ObjectDetectorNodelet::pointCloudCallback, this); + mServiceEnableDetections = mNh.advertiseService("enable_detections", &ObjectDetectorNodelet::enableDetectionsCallback, this); // Lambda handles passing class pointer (implicit first parameter) to configCallback mCallbackType = [this](mrover::DetectorParamsConfig& config, uint32_t level) { configCallback(config, level); }; @@ -94,7 +94,7 @@ namespace mrover { NODELET_INFO("Tag detection ready, use odom frame: %s, min hit count: %d, max hit count: %d, hit increment weight: %d, hit decrement weight: %d", mUseOdom ? "true" : "false", mMinHitCountBeforePublish, mMaxHitCount, mTagIncrementWeight, mTagDecrementWeight); } - void TagDetectorNodelet::configCallback(mrover::DetectorParamsConfig& config, uint32_t level) { + void ObjectDetectorNodelet::configCallback(mrover::DetectorParamsConfig& config, uint32_t level) { // Don't load initial config, since it will overwrite the rosparam settings if (level == std::numeric_limits::max()) return; @@ -128,7 +128,7 @@ namespace mrover { mDetectorParams.polygonalApproxAccuracyRate = config.polygonalApproxAccuracyRate; } - bool TagDetectorNodelet::enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { + bool ObjectDetectorNodelet::enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { mEnableDetections = req.data; if (mEnableDetections) { res.message = "Enabled tag detections."; @@ -157,4 +157,4 @@ int main(int argc, char** argv) { } #include -PLUGINLIB_EXPORT_CLASS(mrover::TagDetectorNodelet, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(mrover::ObjectDetectorNodelet, nodelet::Nodelet) diff --git a/src/perception/tag_detector/tag_detector.hpp b/src/perception/tag_detector/tag_detector.hpp index a86747aee..506f97ab1 100644 --- a/src/perception/tag_detector/tag_detector.hpp +++ b/src/perception/tag_detector/tag_detector.hpp @@ -9,7 +9,7 @@ namespace mrover { std::optional tagInCam; }; - class TagDetectorNodelet : public nodelet::Nodelet { + class ObjectDetectorNodelet : public nodelet::Nodelet { private: ros::NodeHandle mNh, mPnh; @@ -58,9 +58,9 @@ namespace mrover { std::optional getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v); public: - TagDetectorNodelet() = default; + ObjectDetectorNodelet() = default; - ~TagDetectorNodelet() override = default; + ~ObjectDetectorNodelet() override = default; void pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg); diff --git a/src/perception/tag_detector/tag_detector.processing.cpp b/src/perception/tag_detector/tag_detector.processing.cpp index 2fae72b73..4b29b3e9b 100644 --- a/src/perception/tag_detector/tag_detector.processing.cpp +++ b/src/perception/tag_detector/tag_detector.processing.cpp @@ -10,7 +10,7 @@ namespace mrover { * @param u X Pixel Position * @param v Y Pixel Position */ - std::optional TagDetectorNodelet::getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) { + std::optional ObjectDetectorNodelet::getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) { assert(cloudPtr); if (u >= cloudPtr->width || v >= cloudPtr->height) { @@ -34,7 +34,7 @@ namespace mrover { * * @param msg Point cloud message */ - void TagDetectorNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) { + void ObjectDetectorNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) { assert(msg); assert(msg->height > 0); assert(msg->width > 0); diff --git a/src/perception/tag_detector/tag_detector.threshold.cpp b/src/perception/tag_detector/tag_detector.threshold.cpp index 3127805ce..47b112d04 100644 --- a/src/perception/tag_detector/tag_detector.threshold.cpp +++ b/src/perception/tag_detector/tag_detector.threshold.cpp @@ -15,7 +15,7 @@ namespace mrover { * * @param msg */ - void TagDetectorNodelet::publishThresholdedImage() { + void ObjectDetectorNodelet::publishThresholdedImage() { cvtColor(mImg, mGrayImg, cv::COLOR_BGR2GRAY); // number of window sizes (scales) to apply adaptive thresholding From 3442e343dd69cbcd9e7035e5306242969f03d930 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 1 Oct 2023 23:48:45 -0400 Subject: [PATCH 017/197] Disambiguate tag detector and obj detector config, add more starter stuff, modify zed launch file --- CMakeLists.txt | 18 ++++--- config/ObjectDetectorParams.cfg | 10 ++++ ...tectorParams.cfg => TagDetectorParams.cfg} | 2 +- launch/zed_test.launch | 50 +++++++++++-------- package.xml | 22 +++----- .../object_detector/object_detector.cpp | 5 ++ .../object_detector/object_detector.hpp | 17 ++++++- src/perception/object_detector/pch.hpp | 2 + src/perception/tag_detector/pch.hpp | 2 +- src/perception/tag_detector/tag_detector.cpp | 14 +++--- src/perception/tag_detector/tag_detector.hpp | 27 ++++++---- .../tag_detector/tag_detector.processing.cpp | 4 +- .../tag_detector/tag_detector.threshold.cpp | 2 +- 13 files changed, 108 insertions(+), 67 deletions(-) create mode 100755 config/ObjectDetectorParams.cfg rename config/{DetectorParams.cfg => TagDetectorParams.cfg} (98%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0721aff35..3bd10b577 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,7 +81,8 @@ set(MROVER_MESSAGE_DEPENDENCIES # Dynamic reconfigure parameter file list set(MROVER_PARAMETERS - config/DetectorParams.cfg + config/TagDetectorParams.cfg + config/ObjectDetectorParams.cfg ) set(MROVER_CMAKE_INCLUDES @@ -92,7 +93,7 @@ set(MROVER_CMAKE_INCLUDES macro(rosify_cpp_target_macro target) target_link_libraries(${target} PRIVATE ${catkin_LIBRARIES}) - target_include_directories(${target} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS} "src/util") + target_include_directories(${target} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS} src/util) add_dependencies(${target} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_compile_options(${target} PRIVATE $<$:${MROVER_CPP_COMPILE_OPTIONS}>) @@ -195,14 +196,15 @@ target_link_libraries(tag_detector_nodelet PRIVATE opencv_core opencv_objdetect add_cpp_node_macro(brushed_motors src/esw/brushed_motors/*.c*) if (ZED_FOUND) - # add_cpp_node_macro(zed_node "src/perception/zed_wrapper/*.c*") - # target_link_options(zed_node PRIVATE "LINKER:--copy-dt-needed-entries") - # target_include_directories(zed_node SYSTEM PRIVATE ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) - # target_link_libraries(zed_node PRIVATE ${ZED_LIBRARIES} ${OpenCV_LIBRARIES} ${SPECIAL_OS_LIBS} lie tag_detector_nodelet) - - add_cpp_nodelet_macro(object_detector_nodelet src/perception/object_detector/*.c* src/object_detector_nodelet/tag_detector) + add_cpp_nodelet_macro(object_detector_nodelet src/perception/object_detector/*.c* src/perception/object_detector) target_precompile_headers(object_detector_nodelet PRIVATE src/perception/object_detector/pch.hpp) target_link_libraries(object_detector_nodelet PRIVATE opencv_core opencv_dnn opencv_imgproc lie) + target_compile_definitions(object_detector_nodelet PRIVATE MROVER_IS_NODELET) + + # TODO: remove eventually, just for testing + add_cpp_node_macro(object_detector_node src/perception/object_detector/*.c* src/perception/object_detector) + target_precompile_headers(object_detector_node PRIVATE src/perception/object_detector/pch.hpp) + target_link_libraries(object_detector_node PRIVATE opencv_core opencv_dnn opencv_imgproc lie /opt/ros/noetic/lib/libnodeletlib.so) add_cpp_nodelet_macro(zed_nodelet src/perception/zed_wrapper/*.c* , src/perception/zed_wrapper) target_precompile_headers(zed_nodelet PRIVATE src/perception/zed_wrapper/pch.hpp) diff --git a/config/ObjectDetectorParams.cfg b/config/ObjectDetectorParams.cfg new file mode 100755 index 000000000..73952b2e4 --- /dev/null +++ b/config/ObjectDetectorParams.cfg @@ -0,0 +1,10 @@ +#!/usr/bin/env python3 +PACKAGE = "mrover" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +# TODO(percep/obj-detectr): add parameters + +exit(gen.generate(PACKAGE, "object_detector", "ObjectDetectorParams")) diff --git a/config/DetectorParams.cfg b/config/TagDetectorParams.cfg similarity index 98% rename from config/DetectorParams.cfg rename to config/TagDetectorParams.cfg index d33504f83..6c9692af7 100755 --- a/config/DetectorParams.cfg +++ b/config/TagDetectorParams.cfg @@ -89,4 +89,4 @@ gen.add("polygonalApproxAccuracyRate", double_t, 0, "Minimum accuracy during the polygonal approximation process to determine which contours are squares", 0.08, 0, 1) -exit(gen.generate(PACKAGE, "tag_detector", "DetectorParams")) +exit(gen.generate(PACKAGE, "tag_detector", "TagDetectorParams")) diff --git a/launch/zed_test.launch b/launch/zed_test.launch index c2cfcf976..81c98f59d 100644 --- a/launch/zed_test.launch +++ b/launch/zed_test.launch @@ -1,33 +1,39 @@ - - + + - - - + + + + - - - - + + + + - + - + - + - - - - + + + + - - + + + + + diff --git a/package.xml b/package.xml index a2701d6c3..89bf45585 100644 --- a/package.xml +++ b/package.xml @@ -18,7 +18,6 @@ - @@ -28,13 +27,13 @@ - message_generation + - message_runtime + @@ -42,18 +41,13 @@ catkin - roscpp - rospy - std_msgs - nodelet - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - nodelet + message_generation + roscpp + rospy + std_msgs + nodelet xacro + message_runtime joint_state_publisher robot_state_publisher diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 5d2de01df..cf9325cab 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -5,6 +5,11 @@ namespace mrover { void ObjectDetectorNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); + + // TODO(percep/obj-detectr): paramaterize this + mNet = cv::dnn::readNetFromTorch("/home/quintin/Downloads/yolov8n.pt"); + + mImgSub = mNh.subscribe("image", 1, &ObjectDetectorNodelet::imageCallback, this); } } // namespace mrover diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index f11e6d1df..726e23656 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -6,6 +6,21 @@ namespace mrover { private: ros::NodeHandle mNh, mPnh; + // Publishers + + ros::Subscriber mImgSub; + + // Subscribers + + dynamic_reconfigure::Server mConfigServer; + dynamic_reconfigure::Server::CallbackType mCallbackType; + + // Internal state + + cv::dnn::Net mNet; + + // Debug + LoopProfiler mProfiler{"Object Detector"}; void onInit() override; @@ -16,8 +31,6 @@ namespace mrover { ~ObjectDetectorNodelet() override = default; void imageCallback(sensor_msgs::ImageConstPtr const& msg); - - bool enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); }; } // namespace mrover diff --git a/src/perception/object_detector/pch.hpp b/src/perception/object_detector/pch.hpp index 2c6453d0b..a34fad005 100644 --- a/src/perception/object_detector/pch.hpp +++ b/src/perception/object_detector/pch.hpp @@ -28,5 +28,7 @@ #include #include +#include + #include #include diff --git a/src/perception/tag_detector/pch.hpp b/src/perception/tag_detector/pch.hpp index 73fb2dd2b..8e7ec6515 100644 --- a/src/perception/tag_detector/pch.hpp +++ b/src/perception/tag_detector/pch.hpp @@ -30,7 +30,7 @@ #include #include -#include +#include #include #include diff --git a/src/perception/tag_detector/tag_detector.cpp b/src/perception/tag_detector/tag_detector.cpp index bbe8c6fc6..afed83beb 100644 --- a/src/perception/tag_detector/tag_detector.cpp +++ b/src/perception/tag_detector/tag_detector.cpp @@ -2,7 +2,7 @@ namespace mrover { - void ObjectDetectorNodelet::onInit() { + void TagDetectorNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); cv::aruco::DetectorParameters defaultDetectorParams; @@ -25,11 +25,11 @@ namespace mrover { mImgPub = mIt->advertise("tag_detection", 1); mDictionary = cv::aruco::getPredefinedDictionary(dictionaryNumber); - mPcSub = mNh.subscribe("camera/left/points", 1, &ObjectDetectorNodelet::pointCloudCallback, this); - mServiceEnableDetections = mNh.advertiseService("enable_detections", &ObjectDetectorNodelet::enableDetectionsCallback, this); + mPcSub = mNh.subscribe("camera/left/points", 1, &TagDetectorNodelet::pointCloudCallback, this); + mServiceEnableDetections = mNh.advertiseService("enable_detections", &TagDetectorNodelet::enableDetectionsCallback, this); // Lambda handles passing class pointer (implicit first parameter) to configCallback - mCallbackType = [this](mrover::DetectorParamsConfig& config, uint32_t level) { configCallback(config, level); }; + mCallbackType = [this](mrover::TagDetectorParamsConfig& config, uint32_t level) { configCallback(config, level); }; mConfigServer.setCallback(mCallbackType); mPnh.param("adaptiveThreshConstant", @@ -94,7 +94,7 @@ namespace mrover { NODELET_INFO("Tag detection ready, use odom frame: %s, min hit count: %d, max hit count: %d, hit increment weight: %d, hit decrement weight: %d", mUseOdom ? "true" : "false", mMinHitCountBeforePublish, mMaxHitCount, mTagIncrementWeight, mTagDecrementWeight); } - void ObjectDetectorNodelet::configCallback(mrover::DetectorParamsConfig& config, uint32_t level) { + void TagDetectorNodelet::configCallback(mrover::TagDetectorParamsConfig& config, uint32_t level) { // Don't load initial config, since it will overwrite the rosparam settings if (level == std::numeric_limits::max()) return; @@ -128,7 +128,7 @@ namespace mrover { mDetectorParams.polygonalApproxAccuracyRate = config.polygonalApproxAccuracyRate; } - bool ObjectDetectorNodelet::enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { + bool TagDetectorNodelet::enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { mEnableDetections = req.data; if (mEnableDetections) { res.message = "Enabled tag detections."; @@ -157,4 +157,4 @@ int main(int argc, char** argv) { } #include -PLUGINLIB_EXPORT_CLASS(mrover::ObjectDetectorNodelet, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(mrover::TagDetectorNodelet, nodelet::Nodelet) diff --git a/src/perception/tag_detector/tag_detector.hpp b/src/perception/tag_detector/tag_detector.hpp index 506f97ab1..fee945e36 100644 --- a/src/perception/tag_detector/tag_detector.hpp +++ b/src/perception/tag_detector/tag_detector.hpp @@ -9,21 +9,29 @@ namespace mrover { std::optional tagInCam; }; - class ObjectDetectorNodelet : public nodelet::Nodelet { + class TagDetectorNodelet : public nodelet::Nodelet { private: ros::NodeHandle mNh, mPnh; + // Publishers + std::optional mIt; image_transport::Publisher mImgPub; std::unordered_map mThreshPubs; // Map from threshold scale to publisher ros::ServiceServer mServiceEnableDetections; + // Subscribers + ros::Subscriber mPcSub; - image_transport::Subscriber mImgSub; tf2_ros::Buffer mTfBuffer; tf2_ros::TransformListener mTfListener{mTfBuffer}; tf2_ros::TransformBroadcaster mTfBroadcaster; + dynamic_reconfigure::Server mConfigServer; + dynamic_reconfigure::Server::CallbackType mCallbackType; + + // Settings + bool mEnableDetections = true; bool mUseOdom{}; std::string mOdomFrameId, mMapFrameId, mCameraFrameId; @@ -32,11 +40,11 @@ namespace mrover { int mMaxHitCount{}; int mTagIncrementWeight{}; int mTagDecrementWeight{}; - - cv::aruco::ArucoDetector mDetector; cv::aruco::DetectorParameters mDetectorParams; cv::aruco::Dictionary mDictionary; + // Internal state + cv::Mat mImg; cv::Mat mGrayImg; sensor_msgs::Image mImgMsg; @@ -46,8 +54,9 @@ namespace mrover { std::vector> mImmediateCorners; std::vector mImmediateIds; std::unordered_map mTags; - dynamic_reconfigure::Server mConfigServer; - dynamic_reconfigure::Server::CallbackType mCallbackType; + cv::aruco::ArucoDetector mDetector; + + // Debug LoopProfiler mProfiler{"Tag Detector"}; @@ -58,13 +67,13 @@ namespace mrover { std::optional getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v); public: - ObjectDetectorNodelet() = default; + TagDetectorNodelet() = default; - ~ObjectDetectorNodelet() override = default; + ~TagDetectorNodelet() override = default; void pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg); - void configCallback(mrover::DetectorParamsConfig& config, uint32_t level); + void configCallback(mrover::TagDetectorParamsConfig& config, uint32_t level); bool enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); }; diff --git a/src/perception/tag_detector/tag_detector.processing.cpp b/src/perception/tag_detector/tag_detector.processing.cpp index 4b29b3e9b..2fae72b73 100644 --- a/src/perception/tag_detector/tag_detector.processing.cpp +++ b/src/perception/tag_detector/tag_detector.processing.cpp @@ -10,7 +10,7 @@ namespace mrover { * @param u X Pixel Position * @param v Y Pixel Position */ - std::optional ObjectDetectorNodelet::getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) { + std::optional TagDetectorNodelet::getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) { assert(cloudPtr); if (u >= cloudPtr->width || v >= cloudPtr->height) { @@ -34,7 +34,7 @@ namespace mrover { * * @param msg Point cloud message */ - void ObjectDetectorNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) { + void TagDetectorNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) { assert(msg); assert(msg->height > 0); assert(msg->width > 0); diff --git a/src/perception/tag_detector/tag_detector.threshold.cpp b/src/perception/tag_detector/tag_detector.threshold.cpp index 47b112d04..3127805ce 100644 --- a/src/perception/tag_detector/tag_detector.threshold.cpp +++ b/src/perception/tag_detector/tag_detector.threshold.cpp @@ -15,7 +15,7 @@ namespace mrover { * * @param msg */ - void ObjectDetectorNodelet::publishThresholdedImage() { + void TagDetectorNodelet::publishThresholdedImage() { cvtColor(mImg, mGrayImg, cv::COLOR_BGR2GRAY); // number of window sizes (scales) to apply adaptive thresholding From ff8bb2d4cffe3c18b72098af00297f9136de98cd Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 1 Oct 2023 23:57:57 -0400 Subject: [PATCH 018/197] Add comments --- config/ObjectDetectorParams.cfg | 2 +- src/perception/object_detector/object_detector.cpp | 3 ++- .../object_detector/object_detector.processing.cpp | 11 +++++++++++ .../tag_detector/tag_detector.processing.cpp | 4 ++-- 4 files changed, 16 insertions(+), 4 deletions(-) diff --git a/config/ObjectDetectorParams.cfg b/config/ObjectDetectorParams.cfg index 73952b2e4..1ef932be5 100755 --- a/config/ObjectDetectorParams.cfg +++ b/config/ObjectDetectorParams.cfg @@ -5,6 +5,6 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -# TODO(percep/obj-detectr): add parameters +# TODO(percep/obj-detector): add parameters exit(gen.generate(PACKAGE, "object_detector", "ObjectDetectorParams")) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index cf9325cab..d11b7dafe 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -6,7 +6,8 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - // TODO(percep/obj-detectr): paramaterize this + // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases + // TODO(percep/obj-detectr): make this configurable mNet = cv::dnn::readNetFromTorch("/home/quintin/Downloads/yolov8n.pt"); mImgSub = mNh.subscribe("image", 1, &ObjectDetectorNodelet::imageCallback, this); diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 5727ada67..ccea00775 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -6,6 +6,17 @@ namespace mrover { assert(msg); assert(msg->height > 0); assert(msg->width > 0); + + cv::Mat imageView{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; + + /* + * TODO(percep/obj-detector): + * 0. Google "OpenCV DNN example." View a couple of tutorials. Most will be in Python but the C++ API is similar. + * 1. Use cv::dnn::blobFromImage to convert the image to a blob + * 2. Use mNet.forward to run the network + * 3. Parse the output of the network to get the bounding boxes + * 4. Publish the bounding boxes + */ } } // namespace mrover diff --git a/src/perception/tag_detector/tag_detector.processing.cpp b/src/perception/tag_detector/tag_detector.processing.cpp index 2fae72b73..cecffe78f 100644 --- a/src/perception/tag_detector/tag_detector.processing.cpp +++ b/src/perception/tag_detector/tag_detector.processing.cpp @@ -49,7 +49,7 @@ namespace mrover { // So we need to copy the data into the correct format if (static_cast(msg->height) != mImg.rows || static_cast(msg->width) != mImg.cols) { NODELET_INFO("Image size changed from [%d %d] to [%u %u]", mImg.cols, mImg.rows, msg->width, msg->height); - mImg = cv::Mat{static_cast(msg->height), static_cast(msg->width), CV_8UC3, cv::Scalar{0, 0, 0}}; + mImg = cv::Mat::zeros(msg->height, msg->width, CV_8UC3); } auto* pixelPtr = reinterpret_cast(mImg.data); auto* pointPtr = reinterpret_cast(msg->data.data()); @@ -66,7 +66,7 @@ namespace mrover { mProfiler.measureEvent("Threshold"); // Detect the tag vertices in screen space and their respective ids - // {mImmediateCorneres, mImmediateIds} are the outputs from OpenCV + // {mImmediateCorners, mImmediateIds} are the outputs from OpenCV mDetector.setDictionary(mDictionary); mDetector.setDetectorParameters(mDetectorParams); mDetector.detectMarkers(mImg, mImmediateCorners, mImmediateIds); From 7629bcee86892a2c2aa6841f5b0be1738a948431 Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Tue, 3 Oct 2023 16:00:09 -0400 Subject: [PATCH 019/197] created long_range_cam pch --- src/perception/long_range_cam/pch.hpp | 31 +++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 src/perception/long_range_cam/pch.hpp diff --git a/src/perception/long_range_cam/pch.hpp b/src/perception/long_range_cam/pch.hpp new file mode 100644 index 000000000..b9dcda34c --- /dev/null +++ b/src/perception/long_range_cam/pch.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include From d5223775e954376df126f7e7e0946f9a79318ed2 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Tue, 3 Oct 2023 18:00:47 -0400 Subject: [PATCH 020/197] move tag detectors into their own folders and added long range cam videoCapture --- .../long_range_cam/long_range_cam.cpp | 159 +++++++++--------- .../long_range_cam/long_range_cam.hpp | 47 +++--- .../long_range_tag_detector.cpp | 0 .../long_range_tag_detector.hpp | 0 .../long_range_tag_detector.processing.cpp | 0 .../tag_detector/long_range/pch.hpp | 31 ++++ src/perception/tag_detector/{ => zed}/pch.hpp | 0 .../tag_detector/{ => zed}/tag_detector.cpp | 0 .../tag_detector/{ => zed}/tag_detector.hpp | 0 .../{ => zed}/tag_detector.processing.cpp | 0 10 files changed, 127 insertions(+), 110 deletions(-) rename src/perception/tag_detector/{ => long_range}/long_range_tag_detector.cpp (100%) rename src/perception/tag_detector/{ => long_range}/long_range_tag_detector.hpp (100%) rename src/perception/tag_detector/{ => long_range}/long_range_tag_detector.processing.cpp (100%) create mode 100644 src/perception/tag_detector/long_range/pch.hpp rename src/perception/tag_detector/{ => zed}/pch.hpp (100%) rename src/perception/tag_detector/{ => zed}/tag_detector.cpp (100%) rename src/perception/tag_detector/{ => zed}/tag_detector.hpp (100%) rename src/perception/tag_detector/{ => zed}/tag_detector.processing.cpp (100%) diff --git a/src/perception/long_range_cam/long_range_cam.cpp b/src/perception/long_range_cam/long_range_cam.cpp index 1a13c5cb1..bd2dce381 100644 --- a/src/perception/long_range_cam/long_range_cam.cpp +++ b/src/perception/long_range_cam/long_range_cam.cpp @@ -13,58 +13,73 @@ namespace mrover { // MT means multithreaded mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mCamInfoPub = mNh.advertise("camera/right/camera_info", 1); + mCamInfoPub = mNh.advertise("camera/camera_info", 1); image_transport::ImageTransport it{mNh}; - mRightImgPub = it.advertise("long_range_cam/image", 1); - - int imageWidth{}; - int imageHeight{}; - // TODO: edit these so they correspond to the long range cam's resolution - mPnh.param("image_width", imageWidth, 1280); - mPnh.param("image_height", imageHeight, 720); - - if (imageWidth < 0 || imageHeight < 0) { - throw std::invalid_argument("Invalid image dimensions"); + mCamImgPub = it.advertise("long_range_cam/image", 1); + + // TODO: Make these camera open values ROS params/not hardcoded + int deviceID = 0; + int apiID = cv::CAP_ANY; + mCapture.open(deviceID, apiID); + if (!mCapture.isOpened()) { + throw std::runtime_error("Long range cam failed to open"); } - if (mGrabTargetFps < 0) { - throw std::invalid_argument("Invalid grab target framerate"); - } - - mImageResolution = sl::Resolution(imageWidth, imageHeight); - mPointResolution = sl::Resolution(imageWidth, imageHeight); + + mReadThread = std::thread(&LongRangeCamNodelet::readUpdate, this); + mPubThread = std::thread(&LongRangeCamNodelet::imagePubUpdate, this); - NODELET_INFO("Resolution: %s image: %zux%zu points: %zux%zu", - grabResolutionString.c_str(), mImageResolution.width, mImageResolution.height); + } catch (std::exception const& e) { + NODELET_FATAL("Exception while starting: %s", e.what()); + ros::shutdown(); + } + } - sl::InitParameters initParameters; - if (mSvoPath) { - initParameters.input.setFromSVOFile(mSvoPath); - } else { - initParameters.input.setFromCameraID(-1, sl::BUS_TYPE::USB); - } - initParameters.depth_stabilization = mUseDepthStabilization; - initParameters.camera_resolution = stringToZedEnum(grabResolutionString); - initParameters.depth_mode = stringToZedEnum(depthModeString); - initParameters.coordinate_units = sl::UNIT::METER; - initParameters.sdk_verbose = true; // Log useful information - initParameters.camera_fps = mGrabTargetFps; - initParameters.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; // Match ROS - initParameters.depth_maximum_distance = mDepthMaximumDistance; - - if (mZed.open(initParameters) != sl::ERROR_CODE::SUCCESS) { - throw std::runtime_error("ZED failed to open"); - } + /** + * @brief Publishes images from long range cam. + * + * Takes in the image from the cam and publishes it. + */ + void LongRangeCamNodelet::imagePubUpdate() { + try { + NODELET_INFO("Starting image pub thead"); - cudaDeviceProp prop{}; - cudaGetDeviceProperties(&prop, 0); - ROS_INFO("MP count: %d, Max threads/MP: %d, Max blocks/MP: %d, max threads/block: %d", - prop.multiProcessorCount, prop.maxThreadsPerMultiProcessor, prop.maxBlocksPerMultiProcessor, prop.maxThreadsPerBlock); + while (ros::ok()) { + mPubThreadProfiler.beginLoop(); + + // TODO: probably bad that this allocation, best case optimized by tcache + // Needed because publish directly shares the pointer to other nodelets running in this process + // TODO: What does this do? + // Swap critical section + { + std::unique_lock lock{mSwapMutex}; + // Waiting on the condition variable will drop the lock and reacquire it when the condition is met + mSwapCv.wait(lock, [this] { return mIsSwapReady.load(); }); + mIsSwapReady = false; + mPubThreadProfiler.measureEvent("Wait"); + if (mLeftImgPub.getNumSubscribers()) { + auto imgMsg = boost::make_shared(); + fillImageMessage(mPubMeasures.mImage, imgMsg); + imgMsg->header.frame_id = "long_range_cam_frame"; + imgMsg->header.stamp = mPubMeasures.time; + imgMsg->header.seq = mImagePubUpdateTick; + mImgPub.publish(ImgMsg); + + auto camInfoMsg = boost::make_shared(); + fillCameraInfoMfessages(calibration, mImageResolution, leftCamInfoMsg, rightCamInfoMsg); + rightCamInfoMsg->header.frame_id = "long_range_cam_frame"; + rightCamInfoMsg->header.stamp = mPcMeasures.time; + rightCamInfoMsg->header.seq = mImagePubUpdateTick; + mLeftCamInfoPub.publish(camInfoMsg); + } + + mPubThreadProfiler.measureEvent("Publish Message"); + } - mGrabThread = std::thread(&ZedNodelet::grabUpdate, this); + mImagePubUpdateTick++; + } + NODELET_INFO("Tag thread finished"); - } catch (std::exception const& e) { - NODELET_FATAL("Exception while starting: %s", e.what()); - ros::shutdown(); + } } } @@ -76,40 +91,22 @@ namespace mrover { * As such we retrieve the image and point cloud on the GPU to send to the other thread for processing. * This only happens if the other thread is ready to avoid blocking, hence the try lock. */ - void ZedNodelet::grabUpdate() { + void LongRangeCam::readUpdate() { try { NODELET_INFO("Starting grab thread"); while (ros::ok()) { mGrabThreadProfiler.beginLoop(); + // TODO: Combining grab and retrieve together, is that ok? + if (!mCapture.read(mGrabMeasures, mImage)) + throw std::runtime_error("Long range cam failed to get image"); + mGrabThreadProfiler.measureEvent("Read"); - sl::RuntimeParameters runtimeParameters; - runtimeParameters.confidence_threshold = mDepthConfidence; - runtimeParameters.texture_confidence_threshold = mTextureConfidence; - - if (mZed.grab(runtimeParameters) != sl::ERROR_CODE::SUCCESS) - throw std::runtime_error("ZED failed to grab"); - mGrabThreadProfiler.measureEvent("Grab"); - - // Retrieval has to happen on the same thread as grab so that the image and point cloud are synced - if (mRightImgPub.getNumSubscribers()) - if (mZed.retrieveImage(mGrabMeasures.rightImage, sl::VIEW::RIGHT, sl::MEM::GPU, mImageResolution) != sl::ERROR_CODE::SUCCESS) - throw std::runtime_error("ZED failed to retrieve right image"); - // Only left set is used for processing - if (mZed.retrieveImage(mGrabMeasures.leftImage, sl::VIEW::LEFT, sl::MEM::GPU, mImageResolution) != sl::ERROR_CODE::SUCCESS) - throw std::runtime_error("ZED failed to retrieve left image"); - if (mZed.retrieveMeasure(mGrabMeasures.leftPoints, sl::MEASURE::XYZ, sl::MEM::GPU, mPointResolution) != sl::ERROR_CODE::SUCCESS) - throw std::runtime_error("ZED failed to retrieve point cloud"); - - assert(mGrabMeasures.leftImage.timestamp == mGrabMeasures.leftPoints.timestamp); - - - mGrabMeasures.time = mSvoPath ? ros::Time::now() : slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); - mGrabThreadProfiler.measureEvent("Retrieve"); + mGrabMeasures.time = ros::Time::now(); // If the processing thread is busy skip // We want this thread to run as fast as possible for grab and positional tracking if (mSwapMutex.try_lock()) { - std::swap(mGrabMeasures, mPcMeasures); + std::swap(mGrabMeasures, mPubMeasures); mIsSwapReady = true; mSwapMutex.unlock(); mSwapCv.notify_one(); @@ -118,13 +115,10 @@ namespace mrover { mGrabUpdateTick++; } - - mZed.close(); NODELET_INFO("Grab thread finished"); } catch (std::exception const& e) { NODELET_FATAL("Exception while running grab thread: %s", e.what()); - mZed.close(); ros::shutdown(); std::exit(EXIT_FAILURE); } @@ -132,29 +126,26 @@ namespace mrover { LongRangeCamNodelet::~LongRangeCamNodelet() { NODELET_INFO("Long range cam node shutting down"); - mPointCloudThread.join(); - mGrabThread.join(); + mReadThread.join(); } - ZedNodelet::Measures::Measures(ZedNodelet::Measures&& other) noexcept { + ZedNodelet::Measures::Measures(LongRangeCamNodelet::Measures&& other) noexcept { *this = std::move(other); } - ZedNodelet::Measures& ZedNodelet::Measures::operator=(ZedNodelet::Measures&& other) noexcept { - sl::Mat::swap(other.leftImage, leftImage); - sl::Mat::swap(other.rightImage, rightImage); - sl::Mat::swap(other.leftPoints, leftPoints); + LongRangeCamNodelet::Measures& LongRangeCamNodelet::Measures::operator=(LongRangeCamNodelet::Measures&& other) noexcept { + cv::OutpuArray::swap(other.mImage, mImage); std::swap(time, other.time); return *this; } } // namespace mrover int main(int argc, char** argv) { - ros::init(argc, argv, "zed_wrapper"); + ros::init(argc, argv, "long_range_cam_nodelet"); - // Start the ZED Nodelet + // Start the Long Range Cam Nodelet nodelet::Loader nodelet; - nodelet.load(ros::this_node::getName(), "mrover/ZedNodelet", ros::names::getRemappings(), {}); + nodelet.load(ros::this_node::getName(), "mrover/LongRangeCamNodelet", ros::names::getRemappings(), {}); ros::spin(); @@ -163,5 +154,5 @@ int main(int argc, char** argv) { #ifdef MROVER_IS_NODELET #include -PLUGINLIB_EXPORT_CLASS(mrover::ZedNodelet, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(mrover::LongRangeCamNodelet, nodelet::Nodelet) #endif diff --git a/src/perception/long_range_cam/long_range_cam.hpp b/src/perception/long_range_cam/long_range_cam.hpp index e53e140f4..8bd48c2ed 100644 --- a/src/perception/long_range_cam/long_range_cam.hpp +++ b/src/perception/long_range_cam/long_range_cam.hpp @@ -5,38 +5,36 @@ namespace mrover { class LongRangeCamNodelet : public nodelet::Nodelet { private: - explicit LongRangeCamNodelet; - virtual ~LongRangeCamNodelet; - LongRangeCamNodelet - ros::NodeHandle mNh, mPnh; + cv::VideoCapture mCapture; - ros::Publisher mCamInfoPub; - image_transport::Publisher mCamImgPub; + struct Measures { + ros::Time time; + cv::OutputArray mImage; - PointCloudGpu mPointCloudGpu; + Measures() = default; - sl::Resolution mImageResolution; - sl::String mSvoPath; - int mGrabTargetFps{}; - int mDepthConfidence{}; - int mTextureConfidence{}; - bool mUseBuiltinPosTracking{}; - bool mUseAreaMemory{}; - bool mUsePoseSmoothing{}; - bool mUseLoopProfiler{}; - bool mUseDepthStabilization{}; - float mDepthMaximumDistance{}; + Measures(Measures&) = delete; + Measures& operator=(Measures&) = delete; - Measures mGrabMeasures, mPcMeasures; + Measures(Measures&&) noexcept; + Measures& operator=(Measures&&) noexcept; + }; - std::thread mGrabThread; + ros::NodeHandle mNh, mPnh; + + ros::Publisher mCamInfoPub; + image_transport::Publisher mImgPub; + + Measures mGrabMeasures, mPubMeasures; + + std::thread mReadThread; std::mutex mSwapMutex; std::condition_variable mSwapCv; std::atomic_bool mIsSwapReady = false; LoopProfiler mGrabThreadProfiler{"Long Range Cam Grab"}; - size_t mGrabUpdateTick = 0, mPointCloudUpdateTick = 0; + size_t mGrabUpdateTick = 0; void onInit() override; @@ -45,16 +43,13 @@ namespace mrover { ~LongRangeCamNodelet() override; - void grabUpdate(); + void readUpdate(); + void imagePubUpdate(); }; - ros::Time slTime2Ros(sl::Timestamp t); - void fillCameraInfoMessages(sl::CalibrationParameters& calibration, sl::Resolution const& resolution, sensor_msgs::CameraInfoPtr const& leftInfoMsg, sensor_msgs::CameraInfoPtr const& rightInfoMsg); void fillImageMessage(sl::Mat& bgra, sensor_msgs::ImagePtr const& msg); - void checkCudaError(cudaError_t error); - } // namespace mrover diff --git a/src/perception/tag_detector/long_range_tag_detector.cpp b/src/perception/tag_detector/long_range/long_range_tag_detector.cpp similarity index 100% rename from src/perception/tag_detector/long_range_tag_detector.cpp rename to src/perception/tag_detector/long_range/long_range_tag_detector.cpp diff --git a/src/perception/tag_detector/long_range_tag_detector.hpp b/src/perception/tag_detector/long_range/long_range_tag_detector.hpp similarity index 100% rename from src/perception/tag_detector/long_range_tag_detector.hpp rename to src/perception/tag_detector/long_range/long_range_tag_detector.hpp diff --git a/src/perception/tag_detector/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range/long_range_tag_detector.processing.cpp similarity index 100% rename from src/perception/tag_detector/long_range_tag_detector.processing.cpp rename to src/perception/tag_detector/long_range/long_range_tag_detector.processing.cpp diff --git a/src/perception/tag_detector/long_range/pch.hpp b/src/perception/tag_detector/long_range/pch.hpp new file mode 100644 index 000000000..01f396d92 --- /dev/null +++ b/src/perception/tag_detector/long_range/pch.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include diff --git a/src/perception/tag_detector/pch.hpp b/src/perception/tag_detector/zed/pch.hpp similarity index 100% rename from src/perception/tag_detector/pch.hpp rename to src/perception/tag_detector/zed/pch.hpp diff --git a/src/perception/tag_detector/tag_detector.cpp b/src/perception/tag_detector/zed/tag_detector.cpp similarity index 100% rename from src/perception/tag_detector/tag_detector.cpp rename to src/perception/tag_detector/zed/tag_detector.cpp diff --git a/src/perception/tag_detector/tag_detector.hpp b/src/perception/tag_detector/zed/tag_detector.hpp similarity index 100% rename from src/perception/tag_detector/tag_detector.hpp rename to src/perception/tag_detector/zed/tag_detector.hpp diff --git a/src/perception/tag_detector/tag_detector.processing.cpp b/src/perception/tag_detector/zed/tag_detector.processing.cpp similarity index 100% rename from src/perception/tag_detector/tag_detector.processing.cpp rename to src/perception/tag_detector/zed/tag_detector.processing.cpp From 90ae8d3537354d47b11ebd68682cdcaffd7cf493 Mon Sep 17 00:00:00 2001 From: ajaysum Date: Thu, 5 Oct 2023 16:32:56 -0700 Subject: [PATCH 021/197] merged in fillImageMessage --- .../long_range_cam/long_range_cam.cpp | 186 +++++++++++------- 1 file changed, 110 insertions(+), 76 deletions(-) diff --git a/src/perception/long_range_cam/long_range_cam.cpp b/src/perception/long_range_cam/long_range_cam.cpp index bd2dce381..ecce0bc08 100644 --- a/src/perception/long_range_cam/long_range_cam.cpp +++ b/src/perception/long_range_cam/long_range_cam.cpp @@ -4,6 +4,31 @@ namespace mrover { using namespace std::chrono_literals; + + cv::VideoCapture cap { + 0, + { cv::CAP_V4L2, + cv::CAP_PROP_FRAME_WIDTH, size.width, + cv::CAP_PROP_FRAME_HEIGHT, size.height, + cv::CAP_PROP_FPS, 30, + + } + void fillImageMessage(sl::Mat& bgra, sensor_msgs::ImagePtr const& msg) { + + + } + }; + void cv::VideoCapture::fillImageMessage(sl::Mat& bgra, sensor_msgs::ImagePtr const& msg) { + assert(bgra.getChannels() == 4); + assert(msg); + msg->height = bgra.getHeight(); + msg->width = bgra.getWidth(); + msg->encoding = sensor_msgs::image_encodings::BGRA8; + msg->step = bgra.getStepBytes(); + msg->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; + bytes_to_copy = image.total() * image.elemSize() + std::memcpy(msg->data.data(), bgra.data, bytes_to_copy) + } /** * @brief Load config, open the camera, and start our threads */ @@ -13,73 +38,58 @@ namespace mrover { // MT means multithreaded mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mCamInfoPub = mNh.advertise("camera/camera_info", 1); + mCamInfoPub = mNh.advertise("camera/right/camera_info", 1); image_transport::ImageTransport it{mNh}; - mCamImgPub = it.advertise("long_range_cam/image", 1); - - // TODO: Make these camera open values ROS params/not hardcoded - int deviceID = 0; - int apiID = cv::CAP_ANY; - mCapture.open(deviceID, apiID); - if (!mCapture.isOpened()) { - throw std::runtime_error("Long range cam failed to open"); - } - - mReadThread = std::thread(&LongRangeCamNodelet::readUpdate, this); - mPubThread = std::thread(&LongRangeCamNodelet::imagePubUpdate, this); + mRightImgPub = it.advertise("long_range_cam/image", 1); - } catch (std::exception const& e) { - NODELET_FATAL("Exception while starting: %s", e.what()); - ros::shutdown(); - } - } + int imageWidth{}; + int imageHeight{}; + // TODO: edit these so they correspond to the long range cam's resolution + mPnh.param("image_width", imageWidth, 1280); + mPnh.param("image_height", imageHeight, 720); - /** - * @brief Publishes images from long range cam. - * - * Takes in the image from the cam and publishes it. - */ - void LongRangeCamNodelet::imagePubUpdate() { - try { - NODELET_INFO("Starting image pub thead"); + if (imageWidth < 0 || imageHeight < 0) { + throw std::invalid_argument("Invalid image dimensions"); + } + if (mGrabTargetFps < 0) { + throw std::invalid_argument("Invalid grab target framerate"); + } - while (ros::ok()) { - mPubThreadProfiler.beginLoop(); - - // TODO: probably bad that this allocation, best case optimized by tcache - // Needed because publish directly shares the pointer to other nodelets running in this process - // TODO: What does this do? - // Swap critical section - { - std::unique_lock lock{mSwapMutex}; - // Waiting on the condition variable will drop the lock and reacquire it when the condition is met - mSwapCv.wait(lock, [this] { return mIsSwapReady.load(); }); - mIsSwapReady = false; - mPubThreadProfiler.measureEvent("Wait"); - if (mLeftImgPub.getNumSubscribers()) { - auto imgMsg = boost::make_shared(); - fillImageMessage(mPubMeasures.mImage, imgMsg); - imgMsg->header.frame_id = "long_range_cam_frame"; - imgMsg->header.stamp = mPubMeasures.time; - imgMsg->header.seq = mImagePubUpdateTick; - mImgPub.publish(ImgMsg); - - auto camInfoMsg = boost::make_shared(); - fillCameraInfoMfessages(calibration, mImageResolution, leftCamInfoMsg, rightCamInfoMsg); - rightCamInfoMsg->header.frame_id = "long_range_cam_frame"; - rightCamInfoMsg->header.stamp = mPcMeasures.time; - rightCamInfoMsg->header.seq = mImagePubUpdateTick; - mLeftCamInfoPub.publish(camInfoMsg); - } - - mPubThreadProfiler.measureEvent("Publish Message"); - } + mImageResolution = sl::Resolution(imageWidth, imageHeight); + mPointResolution = sl::Resolution(imageWidth, imageHeight); - mImagePubUpdateTick++; - } - NODELET_INFO("Tag thread finished"); + NODELET_INFO("Resolution: %s image: %zux%zu points: %zux%zu", + grabResolutionString.c_str(), mImageResolution.width, mImageResolution.height); + sl::InitParameters initParameters; + if (mSvoPath) { + initParameters.input.setFromSVOFile(mSvoPath); + } else { + initParameters.input.setFromCameraID(-1, sl::BUS_TYPE::USB); } + initParameters.depth_stabilization = mUseDepthStabilization; + initParameters.camera_resolution = stringToZedEnum(grabResolutionString); + initParameters.depth_mode = stringToZedEnum(depthModeString); + initParameters.coordinate_units = sl::UNIT::METER; + initParameters.sdk_verbose = true; // Log useful information + initParameters.camera_fps = mGrabTargetFps; + initParameters.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; // Match ROS + initParameters.depth_maximum_distance = mDepthMaximumDistance; + + if (mZed.open(initParameters) != sl::ERROR_CODE::SUCCESS) { + throw std::runtime_error("ZED failed to open"); + } + + cudaDeviceProp prop{}; + cudaGetDeviceProperties(&prop, 0); + ROS_INFO("MP count: %d, Max threads/MP: %d, Max blocks/MP: %d, max threads/block: %d", + prop.multiProcessorCount, prop.maxThreadsPerMultiProcessor, prop.maxBlocksPerMultiProcessor, prop.maxThreadsPerBlock); + + mGrabThread = std::thread(&ZedNodelet::grabUpdate, this); + + } catch (std::exception const& e) { + NODELET_FATAL("Exception while starting: %s", e.what()); + ros::shutdown(); } } @@ -91,22 +101,40 @@ namespace mrover { * As such we retrieve the image and point cloud on the GPU to send to the other thread for processing. * This only happens if the other thread is ready to avoid blocking, hence the try lock. */ - void LongRangeCam::readUpdate() { + void ZedNodelet::grabUpdate() { try { NODELET_INFO("Starting grab thread"); while (ros::ok()) { - mGrabThreadProfiler.beginLoop(); - // TODO: Combining grab and retrieve together, is that ok? - if (!mCapture.read(mGrabMeasures, mImage)) - throw std::runtime_error("Long range cam failed to get image"); - mGrabThreadProfiler.measureEvent("Read"); + mGrabThreadProfiler.beginLoop(); + + sl::RuntimeParameters runtimeParameters; + runtimeParameters.confidence_threshold = mDepthConfidence; + runtimeParameters.texture_confidence_threshold = mTextureConfidence; + + if (mZed.grab(runtimeParameters) != sl::ERROR_CODE::SUCCESS) + throw std::runtime_error("ZED failed to grab"); + mGrabThreadProfiler.measureEvent("Grab"); - mGrabMeasures.time = ros::Time::now(); + // Retrieval has to happen on the same thread as grab so that the image and point cloud are synced + if (mRightImgPub.getNumSubscribers()) + if (mZed.retrieveImage(mGrabMeasures.rightImage, sl::VIEW::RIGHT, sl::MEM::GPU, mImageResolution) != sl::ERROR_CODE::SUCCESS) + throw std::runtime_error("ZED failed to retrieve right image"); + // Only left set is used for processing + if (mZed.retrieveImage(mGrabMeasures.leftImage, sl::VIEW::LEFT, sl::MEM::GPU, mImageResolution) != sl::ERROR_CODE::SUCCESS) + throw std::runtime_error("ZED failed to retrieve left image"); + if (mZed.retrieveMeasure(mGrabMeasures.leftPoints, sl::MEASURE::XYZ, sl::MEM::GPU, mPointResolution) != sl::ERROR_CODE::SUCCESS) + throw std::runtime_error("ZED failed to retrieve point cloud"); + + assert(mGrabMeasures.leftImage.timestamp == mGrabMeasures.leftPoints.timestamp); + + + mGrabMeasures.time = mSvoPath ? ros::Time::now() : slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); + mGrabThreadProfiler.measureEvent("Retrieve"); // If the processing thread is busy skip // We want this thread to run as fast as possible for grab and positional tracking if (mSwapMutex.try_lock()) { - std::swap(mGrabMeasures, mPubMeasures); + std::swap(mGrabMeasures, mPcMeasures); mIsSwapReady = true; mSwapMutex.unlock(); mSwapCv.notify_one(); @@ -115,10 +143,13 @@ namespace mrover { mGrabUpdateTick++; } + + mZed.close(); NODELET_INFO("Grab thread finished"); } catch (std::exception const& e) { NODELET_FATAL("Exception while running grab thread: %s", e.what()); + mZed.close(); ros::shutdown(); std::exit(EXIT_FAILURE); } @@ -126,26 +157,29 @@ namespace mrover { LongRangeCamNodelet::~LongRangeCamNodelet() { NODELET_INFO("Long range cam node shutting down"); - mReadThread.join(); + mPointCloudThread.join(); + mGrabThread.join(); } - ZedNodelet::Measures::Measures(LongRangeCamNodelet::Measures&& other) noexcept { + ZedNodelet::Measures::Measures(ZedNodelet::Measures&& other) noexcept { *this = std::move(other); } - LongRangeCamNodelet::Measures& LongRangeCamNodelet::Measures::operator=(LongRangeCamNodelet::Measures&& other) noexcept { - cv::OutpuArray::swap(other.mImage, mImage); + ZedNodelet::Measures& ZedNodelet::Measures::operator=(ZedNodelet::Measures&& other) noexcept { + sl::Mat::swap(other.leftImage, leftImage); + sl::Mat::swap(other.rightImage, rightImage); + sl::Mat::swap(other.leftPoints, leftPoints); std::swap(time, other.time); return *this; } } // namespace mrover int main(int argc, char** argv) { - ros::init(argc, argv, "long_range_cam_nodelet"); + ros::init(argc, argv, "zed_wrapper"); - // Start the Long Range Cam Nodelet + // Start the ZED Nodelet nodelet::Loader nodelet; - nodelet.load(ros::this_node::getName(), "mrover/LongRangeCamNodelet", ros::names::getRemappings(), {}); + nodelet.load(ros::this_node::getName(), "mrover/ZedNodelet", ros::names::getRemappings(), {}); ros::spin(); @@ -154,5 +188,5 @@ int main(int argc, char** argv) { #ifdef MROVER_IS_NODELET #include -PLUGINLIB_EXPORT_CLASS(mrover::LongRangeCamNodelet, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(mrover::ZedNodelet, nodelet::Nodelet) #endif From ef2e7653a2f5d6e2f32fbbe4813f44e595ff28f5 Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Tue, 10 Oct 2023 20:06:19 -0400 Subject: [PATCH 022/197] Began work on object detector --- src/perception/object_detector/object_detector.cpp | 3 ++- .../object_detector/object_detector.processing.cpp | 7 +++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index d11b7dafe..a10997338 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -6,9 +6,10 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); + //read ONNX file into this mNet, YOLOV8, second smallest one // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases // TODO(percep/obj-detectr): make this configurable - mNet = cv::dnn::readNetFromTorch("/home/quintin/Downloads/yolov8n.pt"); + mNet = cv::dnn::readNetFromTorch("/home/marshallvielmetti/Downloads/yolov8s.pt"); mImgSub = mNh.subscribe("image", 1, &ObjectDetectorNodelet::imageCallback, this); } diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index ccea00775..335c6e241 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -1,4 +1,5 @@ #include "object_detector.hpp" +#include namespace mrover { @@ -9,6 +10,12 @@ namespace mrover { cv::Mat imageView{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; + + cv::Mat imageBlob = cv::dnn::blobFromImage(imageView); + + mNet.forward(imageBlob); + + /* * TODO(percep/obj-detector): * 0. Google "OpenCV DNN example." View a couple of tutorials. Most will be in Python but the C++ API is similar. From e70dc0c7832c77547a4f389ff0ac11edc7a8ec09 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 10 Oct 2023 20:11:11 -0400 Subject: [PATCH 023/197] Add cmake --- CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1ba21c323..8a0be1154 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -240,6 +240,10 @@ else () target_precompile_headers(tag_detector_nodelet PRIVATE src/perception/tag_detector/pch.hpp) target_link_libraries(tag_detector_nodelet PRIVATE ${OpenCV_LIBRARIES} lie) + add_cpp_nodelet_macro(long_range_tag_detector_nodelet src/perception/long_range_cam/*.cpp src/perception/long_range_cam) + target_precompile_headers(long_range_tag_detector_nodelet PRIVATE src/perception/long_range_cam/pch.hpp) + target_link_libraries(long_range_tag_detector_nodelet PRIVATE ${OpenCV_LIBRARIES} lie) + add_cpp_node_macro(brushed_motors "src/esw/brushed_motors/*.cpp") add_cpp_library_macro(lie "src/util/lie/*.cpp" "src/util/lie") From 836961e85f15a1a1d102d0df1aa32edc45959c37 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 10 Oct 2023 20:17:56 -0400 Subject: [PATCH 024/197] Fix cmake --- CMakeLists.txt | 13 +++++++------ .../long_range_tag_detector.processing.cpp | 2 +- .../tag_detector/zed/tag_detector.processing.cpp | 2 +- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b9ece4d09..a2b5dc6d4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -235,15 +235,16 @@ else () # define an add and link macro # Put items here to build macro(add_and_link_macro) - add_cpp_nodelet_macro(tag_detector_nodelet src/perception/tag_detector/*.cpp src/perception/tag_detector) - target_precompile_headers(tag_detector_nodelet PRIVATE src/perception/tag_detector/pch.hpp) + # add_cpp_nodelet_macro(usb_camera_nodelet src/perception/long_range_cam/*.cpp src/perception/long_range_cam) + + add_cpp_nodelet_macro(tag_detector_nodelet src/perception/tag_detector/zed/*.cpp src/perception/tag_detector/zed) + target_precompile_headers(tag_detector_nodelet PRIVATE src/perception/tag_detector/zed/pch.hpp) target_link_libraries(tag_detector_nodelet PRIVATE opencv_core opencv_objdetect opencv_aruco opencv_imgproc lie) - add_cpp_nodelet_macro(long_range_tag_detector_nodelet src/perception/long_range_cam/*.cpp src/perception/long_range_cam) - target_precompile_headers(long_range_tag_detector_nodelet PRIVATE src/perception/long_range_cam/pch.hpp) - target_link_libraries(long_range_tag_detector_nodelet PRIVATE ${OpenCV_LIBRARIES} lie) + # add_cpp_nodelet_macro(long_range_tag_detector_nodelet src/perception/long_range_cam/*.cpp src/perception/long_range_cam) + # target_precompile_headers(long_range_tag_detector_nodelet PRIVATE src/perception/long_range_cam/pch.hpp) + # target_link_libraries(long_range_tag_detector_nodelet PRIVATE ${OpenCV_LIBRARIES} lie) - add_cpp_node_macro(brushed_motors "src/esw/brushed_motors/*.cpp") add_cpp_library_macro(lie src/util/lie/*.cpp src/util/lie) add_cpp_interface_library_macro(moteus deps/moteus/lib/cpp/mjbots) diff --git a/src/perception/tag_detector/long_range/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range/long_range_tag_detector.processing.cpp index 63975f713..e14136639 100644 --- a/src/perception/tag_detector/long_range/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range/long_range_tag_detector.processing.cpp @@ -1,6 +1,6 @@ #include "long_range_tag_detector.hpp" -#include "../point.hpp" //Might not actually need?' +#include "../../point.hpp" //Might not actually need?' #include "mrover/LongRangeTags.h" #include diff --git a/src/perception/tag_detector/zed/tag_detector.processing.cpp b/src/perception/tag_detector/zed/tag_detector.processing.cpp index 82866f8ea..63b7ab3ad 100644 --- a/src/perception/tag_detector/zed/tag_detector.processing.cpp +++ b/src/perception/tag_detector/zed/tag_detector.processing.cpp @@ -1,6 +1,6 @@ #include "tag_detector.hpp" -#include "../point.hpp" +#include "../../point.hpp" namespace mrover { From 20109b7c95aec7fb38669ef8932394745f88e16d Mon Sep 17 00:00:00 2001 From: jbrhm Date: Wed, 11 Oct 2023 11:46:37 -0400 Subject: [PATCH 025/197] Getting Matrix Results --- .../object_detector.processing.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 335c6e241..06694fceb 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -1,5 +1,9 @@ #include "object_detector.hpp" +#include +#include #include +#include +#include namespace mrover { @@ -10,11 +14,18 @@ namespace mrover { cv::Mat imageView{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; - cv::Mat imageBlob = cv::dnn::blobFromImage(imageView); + //Set the input data for the model + mNet.setInput(imageBlob); + + //Run the network and get the results + cv::Mat results = mNet.forward(); + + ROS_INFO("%d cols", results.cols); + ROS_INFO("%d rows", results.rows); - mNet.forward(imageBlob); + //Look at yolov8 documentation for the output matrix /* * TODO(percep/obj-detector): From 9996b7cfcc1733483de978b156d1bd76817df24a Mon Sep 17 00:00:00 2001 From: jbrhm Date: Thu, 12 Oct 2023 10:53:29 -0400 Subject: [PATCH 026/197] Data Gathered Just Need to Create and Publish Msg --- .../object_detector.processing.cpp | 113 +++++++++++++++++- 1 file changed, 110 insertions(+), 3 deletions(-) diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 06694fceb..9632cbd42 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -1,6 +1,9 @@ #include "object_detector.hpp" +#include #include +#include #include +#include #include #include #include @@ -12,6 +15,88 @@ namespace mrover { assert(msg->height > 0); assert(msg->width > 0); + std::vector classes = { + "person", + "bicycle", + "car", + "motorcycle", + "airplane", + "bus", + "train", + "truck", + "boat", + "traffic light", + "fire hydrant", + "stop sign", + "parking meter", + "bench", + "bird", + "cat", + "dog", + "horse", + "sheep", + "cow", + "elephant", + "bear", + "zebra", + "giraffe", + "backpack", + "umbrella", + "handbag", + "tie", + "suitcase", + "frisbee", + "skis", + "snowboard", + "sports ball", + "kite", + "baseball bat", + "baseball glove", + "skateboard", + "surfboard", + "tennis racket", + "bottle", + "wine glass", + "cup", + "fork", + "knife", + "spoon", + "bowl", + "banana", + "apple", + "sandwich", + "orange", + "broccoli", + "carrot", + "hot dog", + "pizza", + "donut", + "cake", + "chair", + "couch", + "potted plant", + "bed", + "dining table", + "toilet", + "tv", + "laptop", + "mouse", + "remote", + "keyboard", + "cell phone", + "microwave", + "oven", + "toaster", + "sink", + "refrigerator", + "book", + "clock", + "vase", + "scissors", + "teddy bear", + "hair drier", + "toothbrush"}; + cv::Mat imageView{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; cv::Mat imageBlob = cv::dnn::blobFromImage(imageView); @@ -19,10 +104,32 @@ namespace mrover { mNet.setInput(imageBlob); //Run the network and get the results - cv::Mat results = mNet.forward(); + std::vector outputsMats; + mNet.forward(outputsMats, mNet.getUnconnectedOutLayersNames()); + + //Result Variables + cv::Mat mat1 = outputsMats[0]; + float classConfidence = 0.0; + cv::Rect box; + + //Get the Data from the MAT + auto data = (float*) mat1.data; //Why does it want me to use auto here? + + //Get the confidence + classConfidence = data[4]; + + //Get the bounding box + box.x = (int) data[0]; + box.y = (int) data[1]; + box.width = (int) data[2]; + box.height = (int) data[3]; - ROS_INFO("%d cols", results.cols); - ROS_INFO("%d rows", results.rows); + //Get the ID of the object and its score + float* dataBegin = data + 5; //Start at the beginning of the scores + cv::Mat classScores(1, (int) classes.size(), CV_32FC1, dataBegin); //Make a new mat for minMaxLoc + cv::Point classID; //A place for the desired class id to go + double classScore; //The corresponding score + cv::minMaxLoc(classScore, nullptr, &classScore, nullptr, &classID); //Look at yolov8 documentation for the output matrix From 7272be8d2af7c81d9feca3cee253bcfe4a01a217 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Thu, 12 Oct 2023 16:22:34 -0400 Subject: [PATCH 027/197] Object msg - still needs publisher --- msg/Object.msg | 7 +++++++ .../object_detector/object_detector.processing.cpp | 11 +++++++++++ 2 files changed, 18 insertions(+) create mode 100644 msg/Object.msg diff --git a/msg/Object.msg b/msg/Object.msg new file mode 100644 index 000000000..6b9a4e70a --- /dev/null +++ b/msg/Object.msg @@ -0,0 +1,7 @@ +string object_type +float32 detection_confidence +float32 heading +float32 xBoxPixel +float32 yBoxPixel +float32 width +float32 height \ No newline at end of file diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 9632cbd42..145c9b484 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -1,4 +1,5 @@ #include "object_detector.hpp" +#include "mrover/Object.h" #include #include #include @@ -8,6 +9,7 @@ #include #include + namespace mrover { void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { @@ -131,6 +133,15 @@ namespace mrover { double classScore; //The corresponding score cv::minMaxLoc(classScore, nullptr, &classScore, nullptr, &classID); + Object msgData; + msgData.object_type = classes[classID.x]; + msgData.detection_confidence = classConfidence; + msgData.heading = ; //I'm not sure what heading is + msgData.xBoxPixel = box.x; + msgData.yBoxPixel = box.y; + msgData.width = box.width; + msgData.height = box.height; + //Look at yolov8 documentation for the output matrix From 133b6e221efd4b55ab11e2c895bbb61fc8b932c7 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Thu, 12 Oct 2023 20:22:09 -0400 Subject: [PATCH 028/197] Needs work with onnx --- CMakeLists.txt | 1 + msg/{Object.msg => DetectedObject.msg} | 0 src/perception/object_detector/inference.cpp | 169 ++++++++++++++++++ src/perception/object_detector/inference.h | 51 ++++++ .../object_detector/object_detector.cpp | 7 +- .../object_detector/object_detector.hpp | 3 + .../object_detector.processing.cpp | 147 +++------------ 7 files changed, 259 insertions(+), 119 deletions(-) rename msg/{Object.msg => DetectedObject.msg} (100%) create mode 100644 src/perception/object_detector/inference.cpp create mode 100644 src/perception/object_detector/inference.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 3bd10b577..1daf964a3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,6 +37,7 @@ set(MROVER_MESSAGE_FILES Carousel.msg Chassis.msg Course.msg + DetectedObject.msg Diagnostic.msg EnableAuton.msg GPSWaypoint.msg diff --git a/msg/Object.msg b/msg/DetectedObject.msg similarity index 100% rename from msg/Object.msg rename to msg/DetectedObject.msg diff --git a/src/perception/object_detector/inference.cpp b/src/perception/object_detector/inference.cpp new file mode 100644 index 000000000..d9f66d484 --- /dev/null +++ b/src/perception/object_detector/inference.cpp @@ -0,0 +1,169 @@ +#include "inference.h" + +Inference::Inference(const std::string& onnxModelPath, const cv::Size& modelInputShape, const std::string& classesTxtFile, const bool& runWithCuda) { + modelPath = onnxModelPath; + modelShape = modelInputShape; + classesPath = classesTxtFile; + cudaEnabled = runWithCuda; + + loadOnnxNetwork(); +} + +std::vector Inference::runInference(const cv::Mat& input) { + + cv::Mat modelInput = input; + if (letterBoxForSquare && modelShape.width == modelShape.height) + modelInput = formatToSquare(modelInput); + + cv::Mat blob; + cv::dnn::blobFromImage(modelInput, blob, 1.0 / 255.0, modelShape, cv::Scalar(), true, false); + net.setInput(blob); + + std::vector outputs; + net.forward(outputs, net.getUnconnectedOutLayersNames()); + + int rows = outputs[0].size[1]; + int dimensions = outputs[0].size[2]; + + bool yolov8 = false; + // yolov5 has an output of shape (batchSize, 25200, 85) (Num classes + box[x,y,w,h] + confidence[c]) + // yolov8 has an output of shape (batchSize, 84, 8400) (Num classes + box[x,y,w,h]) + if (dimensions > rows) // Check if the shape[2] is more than shape[1] (yolov8) + { + yolov8 = true; + rows = outputs[0].size[2]; + dimensions = outputs[0].size[1]; + + outputs[0] = outputs[0].reshape(1, dimensions); + cv::transpose(outputs[0], outputs[0]); + } + float* data = (float*) outputs[0].data; + + float x_factor = modelInput.cols / modelShape.width; + float y_factor = modelInput.rows / modelShape.height; + + std::vector class_ids; + std::vector confidences; + std::vector boxes; + + for (int i = 0; i < rows; ++i) { + if (yolov8) { + float* classes_scores = data + 4; + + cv::Mat scores(1, classes.size(), CV_32FC1, classes_scores); + cv::Point class_id; + double maxClassScore; + + minMaxLoc(scores, 0, &maxClassScore, 0, &class_id); + + if (maxClassScore > modelScoreThreshold) { + confidences.push_back(maxClassScore); + class_ids.push_back(class_id.x); + + float x = data[0]; + float y = data[1]; + float w = data[2]; + float h = data[3]; + + int left = int((x - 0.5 * w) * x_factor); + int top = int((y - 0.5 * h) * y_factor); + + int width = int(w * x_factor); + int height = int(h * y_factor); + + boxes.push_back(cv::Rect(left, top, width, height)); + } + } else // yolov5 + { + float confidence = data[4]; + + if (confidence >= modelConfidenceThreshold) { + float* classes_scores = data + 5; + + cv::Mat scores(1, classes.size(), CV_32FC1, classes_scores); + cv::Point class_id; + double max_class_score; + + minMaxLoc(scores, 0, &max_class_score, 0, &class_id); + + if (max_class_score > modelScoreThreshold) { + confidences.push_back(confidence); + class_ids.push_back(class_id.x); + + float x = data[0]; + float y = data[1]; + float w = data[2]; + float h = data[3]; + + int left = int((x - 0.5 * w) * x_factor); + int top = int((y - 0.5 * h) * y_factor); + + int width = int(w * x_factor); + int height = int(h * y_factor); + + boxes.push_back(cv::Rect(left, top, width, height)); + } + } + } + + data += dimensions; + } + + std::vector nms_result; + cv::dnn::NMSBoxes(boxes, confidences, modelScoreThreshold, modelNMSThreshold, nms_result); + + std::vector detections{}; + for (unsigned long i = 0; i < nms_result.size(); ++i) { + int idx = nms_result[i]; + + Detection result; + result.class_id = class_ids[idx]; + result.confidence = confidences[idx]; + + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution dis(100, 255); + result.color = cv::Scalar(dis(gen), + dis(gen), + dis(gen)); + + result.className = classes[result.class_id]; + result.box = boxes[idx]; + + detections.push_back(result); + } + + return detections; +} + +void Inference::loadClassesFromFile() { + std::ifstream inputFile(classesPath); + if (inputFile.is_open()) { + std::string classLine; + while (std::getline(inputFile, classLine)) + classes.push_back(classLine); + inputFile.close(); + } +} + +void Inference::loadOnnxNetwork() { + net = cv::dnn::readNetFromONNX(modelPath); + if (cudaEnabled) { + std::cout << "\nRunning on CUDA" << std::endl; + net.setPreferableBackend(cv::dnn::DNN_BACKEND_CUDA); + net.setPreferableTarget(cv::dnn::DNN_TARGET_CUDA); + } else { + std::cout << "\nRunning on CPU" << std::endl; + net.setPreferableBackend(cv::dnn::DNN_BACKEND_OPENCV); + net.setPreferableTarget(cv::dnn::DNN_TARGET_CPU); + } +} + +cv::Mat Inference::formatToSquare(const cv::Mat& source) { + int col = source.cols; + int row = source.rows; + int _max = MAX(col, row); + cv::Mat result = cv::Mat::zeros(_max, _max, CV_8UC3); + source.copyTo(result(cv::Rect(0, 0, col, row))); + return result; +} diff --git a/src/perception/object_detector/inference.h b/src/perception/object_detector/inference.h new file mode 100644 index 000000000..9e59ca7ed --- /dev/null +++ b/src/perception/object_detector/inference.h @@ -0,0 +1,51 @@ +#ifndef INFERENCE_H +#define INFERENCE_H + +// Cpp native +#include +#include +#include +#include + +// OpenCV / DNN / Inference +#include +#include +#include + +struct Detection { + int class_id{0}; + std::string className{}; + float confidence{0.0}; + cv::Scalar color{}; + cv::Rect box{}; +}; + +class Inference { +public: + Inference() = default; + Inference(const std::string& onnxModelPath, const cv::Size& modelInputShape = {640, 640}, const std::string& classesTxtFile = "", const bool& runWithCuda = true); + std::vector runInference(const cv::Mat& input); + +private: + void loadClassesFromFile(); + void loadOnnxNetwork(); + cv::Mat formatToSquare(const cv::Mat& source); + + std::string modelPath; + std::string classesPath; + bool cudaEnabled{}; + + std::vector classes{"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; + + cv::Size2f modelShape{}; + + float modelConfidenceThreshold{0.25}; + float modelScoreThreshold{0.45}; + float modelNMSThreshold{0.50}; + + bool letterBoxForSquare = true; + + cv::dnn::Net net; +}; + +#endif // INFERENCE_H diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index a10997338..9c1f83efa 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -1,4 +1,6 @@ #include "object_detector.hpp" +#include "inference.h" +#include namespace mrover { @@ -6,10 +8,13 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); + //Inference + + inference = Inference("/home/jabra/Downloads/yolov8s.pt", cv::Size(720, 480), "", false); + //read ONNX file into this mNet, YOLOV8, second smallest one // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases // TODO(percep/obj-detectr): make this configurable - mNet = cv::dnn::readNetFromTorch("/home/marshallvielmetti/Downloads/yolov8s.pt"); mImgSub = mNh.subscribe("image", 1, &ObjectDetectorNodelet::imageCallback, this); } diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 726e23656..d815c10a7 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -1,3 +1,4 @@ +#include "inference.h" #include "pch.hpp" namespace mrover { @@ -6,6 +7,8 @@ namespace mrover { private: ros::NodeHandle mNh, mPnh; + Inference inference; + // Publishers ros::Subscriber mImgSub; diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 145c9b484..19e63b86c 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -1,11 +1,13 @@ #include "object_detector.hpp" -#include "mrover/Object.h" +#include "inference.h" +#include #include #include #include #include #include #include +#include #include #include @@ -16,131 +18,40 @@ namespace mrover { assert(msg); assert(msg->height > 0); assert(msg->width > 0); + cv::Mat imageView = cv::imread("/home/jabra/Downloads/Water.jpg"); + //cv::Mat imageView{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; - std::vector classes = { - "person", - "bicycle", - "car", - "motorcycle", - "airplane", - "bus", - "train", - "truck", - "boat", - "traffic light", - "fire hydrant", - "stop sign", - "parking meter", - "bench", - "bird", - "cat", - "dog", - "horse", - "sheep", - "cow", - "elephant", - "bear", - "zebra", - "giraffe", - "backpack", - "umbrella", - "handbag", - "tie", - "suitcase", - "frisbee", - "skis", - "snowboard", - "sports ball", - "kite", - "baseball bat", - "baseball glove", - "skateboard", - "surfboard", - "tennis racket", - "bottle", - "wine glass", - "cup", - "fork", - "knife", - "spoon", - "bowl", - "banana", - "apple", - "sandwich", - "orange", - "broccoli", - "carrot", - "hot dog", - "pizza", - "donut", - "cake", - "chair", - "couch", - "potted plant", - "bed", - "dining table", - "toilet", - "tv", - "laptop", - "mouse", - "remote", - "keyboard", - "cell phone", - "microwave", - "oven", - "toaster", - "sink", - "refrigerator", - "book", - "clock", - "vase", - "scissors", - "teddy bear", - "hair drier", - "toothbrush"}; + std::vector detections = inference.runInference(imageView); - cv::Mat imageView{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; + // struct Detection { + // int class_id{0}; + // std::string className{}; + // float confidence{0.0}; + // cv::Scalar color{}; + // cv::Rect box{}; + // }; - cv::Mat imageBlob = cv::dnn::blobFromImage(imageView); - //Set the input data for the model - mNet.setInput(imageBlob); + Detection firstDetection = detections[0]; - //Run the network and get the results - std::vector outputsMats; - mNet.forward(outputsMats, mNet.getUnconnectedOutLayersNames()); - - //Result Variables - cv::Mat mat1 = outputsMats[0]; float classConfidence = 0.0; - cv::Rect box; - - //Get the Data from the MAT - auto data = (float*) mat1.data; //Why does it want me to use auto here? - - //Get the confidence - classConfidence = data[4]; - - //Get the bounding box - box.x = (int) data[0]; - box.y = (int) data[1]; - box.width = (int) data[2]; - box.height = (int) data[3]; + cv::Rect box = firstDetection.box; - //Get the ID of the object and its score - float* dataBegin = data + 5; //Start at the beginning of the scores - cv::Mat classScores(1, (int) classes.size(), CV_32FC1, dataBegin); //Make a new mat for minMaxLoc - cv::Point classID; //A place for the desired class id to go - double classScore; //The corresponding score - cv::minMaxLoc(classScore, nullptr, &classScore, nullptr, &classID); - Object msgData; - msgData.object_type = classes[classID.x]; + DetectedObject msgData; + msgData.object_type = firstDetection.className; msgData.detection_confidence = classConfidence; - msgData.heading = ; //I'm not sure what heading is - msgData.xBoxPixel = box.x; - msgData.yBoxPixel = box.y; - msgData.width = box.width; - msgData.height = box.height; + msgData.xBoxPixel = (float) box.x; + msgData.yBoxPixel = (float) box.y; + msgData.width = (float) box.width; + msgData.height = (float) box.height; + + //Get the heading + float objectHeading; + float zedFOV = 54; //54 @ 720; 42 @ 1080 + float fovPerPixel = (float) zedFOV / (float) (msg->width); + float xCenter = (float) box.x + ((float) box.width) / 2 - ((float) msg->width) / 2; + objectHeading = xCenter * fovPerPixel; + msgData.heading = objectHeading; //Look at yolov8 documentation for the output matrix From 8fbb215aa96b8d5b38b5ee6d5aac30e7ad216578 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Tue, 17 Oct 2023 09:42:48 -0400 Subject: [PATCH 029/197] imread error and linker error --- .../object_detector/object_detector.cpp | 2 +- .../object_detector.processing.cpp | 16 ++++++---------- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 9c1f83efa..b8ff8dca5 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -10,7 +10,7 @@ namespace mrover { //Inference - inference = Inference("/home/jabra/Downloads/yolov8s.pt", cv::Size(720, 480), "", false); + inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), "", false); //read ONNX file into this mNet, YOLOV8, second smallest one // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 19e63b86c..09d8de779 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -18,18 +19,12 @@ namespace mrover { assert(msg); assert(msg->height > 0); assert(msg->width > 0); - cv::Mat imageView = cv::imread("/home/jabra/Downloads/Water.jpg"); + cv::Mat rawImg = cv::imread("//home//jabra//Downloads//Water.jpg"); + cv::Mat sizedImg; + cv::resize(rawImg, sizedImg, cv::Size(640, 640)); //cv::Mat imageView{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; - std::vector detections = inference.runInference(imageView); - - // struct Detection { - // int class_id{0}; - // std::string className{}; - // float confidence{0.0}; - // cv::Scalar color{}; - // cv::Rect box{}; - // }; + std::vector detections = inference.runInference(sizedImg); Detection firstDetection = detections[0]; @@ -53,6 +48,7 @@ namespace mrover { objectHeading = xCenter * fovPerPixel; msgData.heading = objectHeading; + ROS_INFO("%f", msgData.xBoxPixel); //Look at yolov8 documentation for the output matrix From 20c3693986443bad4fb8e1f6ba1d4e3947f43a39 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Tue, 17 Oct 2023 14:23:28 -0400 Subject: [PATCH 030/197] Can detect water bottle --- .../object_detector/object_detector.cpp | 29 ++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index b8ff8dca5..2a5fcf6a3 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -1,6 +1,9 @@ #include "object_detector.hpp" #include "inference.h" +#include #include +#include +#include namespace mrover { @@ -11,11 +14,33 @@ namespace mrover { //Inference inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), "", false); - //read ONNX file into this mNet, YOLOV8, second smallest one // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases // TODO(percep/obj-detectr): make this configurable + cv::Mat rawImg = cv::imread("//home//jabra//Downloads//Water.jpg"); + cv::Mat sizedImg; + cv::resize(rawImg, sizedImg, cv::Size(640, 640)); + //cv::Mat imageView{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; + + std::vector detections = inference.runInference(sizedImg); + + Detection firstDetection = detections[0]; + + float classConfidence = 0.0; + cv::Rect box = firstDetection.box; + + + DetectedObject msgData; + msgData.object_type = firstDetection.className; + msgData.detection_confidence = classConfidence; + msgData.xBoxPixel = (float) box.x; + msgData.yBoxPixel = (float) box.y; + msgData.width = (float) box.width; + msgData.height = (float) box.height; + msgData.heading = 0; + ROS_INFO(firstDetection.className.c_str()); + mImgSub = mNh.subscribe("image", 1, &ObjectDetectorNodelet::imageCallback, this); } @@ -33,5 +58,7 @@ int main(int argc, char** argv) { return EXIT_SUCCESS; } +#ifdef MROVER_IS_NODELET #include PLUGINLIB_EXPORT_CLASS(mrover::ObjectDetectorNodelet, nodelet::Nodelet) +#endif From 154151da331a2f34400edcf46635a54c57954f86 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Tue, 17 Oct 2023 14:57:58 -0400 Subject: [PATCH 031/197] Static Bottle Detection Working --- CMakeLists.txt | 1 + .../object_detector/object_detector.cpp | 26 ++------------- .../object_detector.processing.cpp | 33 ++++++++++++------- 3 files changed, 25 insertions(+), 35 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dde0bff7f..fd2975856 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -214,6 +214,7 @@ if (ZED_FOUND) mrover_nodelet_link_libraries(object_detector PRIVATE opencv_core opencv_dnn opencv_imgproc lie) # Temporary mrover_nodelet_link_libraries(object_detector PRIVATE opencv_imgcodecs) + mrover_nodelet_link_libraries(object_detector PRIVATE opencv_highgui) mrover_add_nodelet(zed src/perception/zed_wrapper/*.c* src/perception/zed_wrapper src/perception/zed_wrapper/pch.hpp) mrover_nodelet_include_directories(zed ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 2a5fcf6a3..b1fedb2ce 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -18,29 +18,9 @@ namespace mrover { // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases // TODO(percep/obj-detectr): make this configurable - cv::Mat rawImg = cv::imread("//home//jabra//Downloads//Water.jpg"); - cv::Mat sizedImg; - cv::resize(rawImg, sizedImg, cv::Size(640, 640)); - //cv::Mat imageView{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; - - std::vector detections = inference.runInference(sizedImg); - - Detection firstDetection = detections[0]; - - float classConfidence = 0.0; - cv::Rect box = firstDetection.box; - - - DetectedObject msgData; - msgData.object_type = firstDetection.className; - msgData.detection_confidence = classConfidence; - msgData.xBoxPixel = (float) box.x; - msgData.yBoxPixel = (float) box.y; - msgData.width = (float) box.width; - msgData.height = (float) box.height; - msgData.heading = 0; - ROS_INFO(firstDetection.className.c_str()); - + while (true) { + ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr()); + } mImgSub = mNh.subscribe("image", 1, &ObjectDetectorNodelet::imageCallback, this); } diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 09d8de779..aac326567 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -16,9 +17,9 @@ namespace mrover { void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { - assert(msg); - assert(msg->height > 0); - assert(msg->width > 0); + //assert(msg); + //assert(msg->height > 0); + //assert(msg->width > 0); cv::Mat rawImg = cv::imread("//home//jabra//Downloads//Water.jpg"); cv::Mat sizedImg; cv::resize(rawImg, sizedImg, cv::Size(640, 640)); @@ -39,19 +40,27 @@ namespace mrover { msgData.yBoxPixel = (float) box.y; msgData.width = (float) box.width; msgData.height = (float) box.height; + msgData.heading = 0; - //Get the heading - float objectHeading; - float zedFOV = 54; //54 @ 720; 42 @ 1080 - float fovPerPixel = (float) zedFOV / (float) (msg->width); - float xCenter = (float) box.x + ((float) box.width) / 2 - ((float) msg->width) / 2; - objectHeading = xCenter * fovPerPixel; - msgData.heading = objectHeading; - ROS_INFO("%f", msgData.xBoxPixel); + //Put the rectangle on the image + cv::rectangle(sizedImg, box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); - //Look at yolov8 documentation for the output matrix + //Put the text on the image + cv::Point text_position(80, 80); + int font_size = 1; + cv::Scalar font_Color(0, 0, 0); + int font_weight = 2; + putText(sizedImg, msgData.object_type, text_position, cv::FONT_HERSHEY_COMPLEX, font_size, font_Color, font_weight); //Putting the text in the matrix// + + //Show the image + cv::imshow("obj detector", sizedImg); + cv::waitKey(0); + //Print the type of objected detected + ROS_INFO(firstDetection.className.c_str()); + + //Look at yolov8 documentation for the output matrix /* * TODO(percep/obj-detector): * 0. Google "OpenCV DNN example." View a couple of tutorials. Most will be in Python but the C++ API is similar. From 818ce3c6a0f2de40f50c6fe6f190bf5fb8f31612 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Tue, 17 Oct 2023 15:12:16 -0400 Subject: [PATCH 032/197] Working camera detection --- .../object_detector/object_detector.cpp | 1 + .../object_detector.processing.cpp | 58 ++++++++++--------- 2 files changed, 31 insertions(+), 28 deletions(-) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index b1fedb2ce..9713f49d7 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -18,6 +18,7 @@ namespace mrover { // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases // TODO(percep/obj-detectr): make this configurable + //TEMP CODE while (true) { ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr()); } diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index aac326567..32cdb0b6a 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include @@ -20,46 +21,47 @@ namespace mrover { //assert(msg); //assert(msg->height > 0); //assert(msg->width > 0); - cv::Mat rawImg = cv::imread("//home//jabra//Downloads//Water.jpg"); + cv::VideoCapture cap(0); + cv::Mat rawImg; // = cv::imread("//home//jabra//Downloads//Water.jpg"); + cap.read(rawImg); cv::Mat sizedImg; cv::resize(rawImg, sizedImg, cv::Size(640, 640)); //cv::Mat imageView{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; std::vector detections = inference.runInference(sizedImg); + if (!detections.empty()) { + Detection firstDetection = detections[0]; - Detection firstDetection = detections[0]; + float classConfidence = 0.0; + cv::Rect box = firstDetection.box; - float classConfidence = 0.0; - cv::Rect box = firstDetection.box; + DetectedObject msgData; + msgData.object_type = firstDetection.className; + msgData.detection_confidence = classConfidence; + msgData.xBoxPixel = (float) box.x; + msgData.yBoxPixel = (float) box.y; + msgData.width = (float) box.width; + msgData.height = (float) box.height; + msgData.heading = 0; - DetectedObject msgData; - msgData.object_type = firstDetection.className; - msgData.detection_confidence = classConfidence; - msgData.xBoxPixel = (float) box.x; - msgData.yBoxPixel = (float) box.y; - msgData.width = (float) box.width; - msgData.height = (float) box.height; - msgData.heading = 0; + //Put the rectangle on the image + cv::rectangle(sizedImg, box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); - //Put the rectangle on the image - cv::rectangle(sizedImg, box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); - - //Put the text on the image - cv::Point text_position(80, 80); - int font_size = 1; - cv::Scalar font_Color(0, 0, 0); - int font_weight = 2; - putText(sizedImg, msgData.object_type, text_position, cv::FONT_HERSHEY_COMPLEX, font_size, font_Color, font_weight); //Putting the text in the matrix// - - //Show the image - cv::imshow("obj detector", sizedImg); - cv::waitKey(0); - - //Print the type of objected detected - ROS_INFO(firstDetection.className.c_str()); + //Put the text on the image + cv::Point text_position(80, 80); + int font_size = 1; + cv::Scalar font_Color(0, 0, 0); + int font_weight = 2; + putText(sizedImg, msgData.object_type, text_position, cv::FONT_HERSHEY_COMPLEX, font_size, font_Color, font_weight); //Putting the text in the matrix// + //Show the image + cv::imshow("obj detector", sizedImg); + cv::waitKey(1); + //Print the type of objected detected + ROS_INFO(firstDetection.className.c_str()); + } //Look at yolov8 documentation for the output matrix /* * TODO(percep/obj-detector): From 9adc8601605836eb57a254849f2ed30502b0f49f Mon Sep 17 00:00:00 2001 From: jbrhm Date: Thu, 19 Oct 2023 12:51:46 -0400 Subject: [PATCH 033/197] Make Needs to be Updated --- src/perception/object_detector/object_detector.cpp | 9 ++++++++- .../object_detector/object_detector.processing.cpp | 9 ++++----- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 9713f49d7..ff792bce5 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -1,10 +1,13 @@ #include "object_detector.hpp" #include "inference.h" +#include #include #include +#include #include #include + namespace mrover { void ObjectDetectorNodelet::onInit() { @@ -19,8 +22,12 @@ namespace mrover { // TODO(percep/obj-detectr): make this configurable //TEMP CODE + cv::VideoCapture cap(0); + cv::Mat img; while (true) { - ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr()); + cap.read(img); + sensor_msgs::ImageConstPtr imgMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg(); + ObjectDetectorNodelet::imageCallback(imgMsg); } mImgSub = mNh.subscribe("image", 1, &ObjectDetectorNodelet::imageCallback, this); } diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 32cdb0b6a..a5ffaf5e2 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -18,15 +18,15 @@ namespace mrover { void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { - //assert(msg); - //assert(msg->height > 0); - //assert(msg->width > 0); + assert(msg); + assert(msg->height > 0); + assert(msg->width > 0); cv::VideoCapture cap(0); cv::Mat rawImg; // = cv::imread("//home//jabra//Downloads//Water.jpg"); cap.read(rawImg); cv::Mat sizedImg; cv::resize(rawImg, sizedImg, cv::Size(640, 640)); - //cv::Mat imageView{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; + cv::Mat imageView{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; std::vector detections = inference.runInference(sizedImg); if (!detections.empty()) { @@ -35,7 +35,6 @@ namespace mrover { float classConfidence = 0.0; cv::Rect box = firstDetection.box; - DetectedObject msgData; msgData.object_type = firstDetection.className; msgData.detection_confidence = classConfidence; From 112b13fc0372f42f49c311aa2e90ef555dc26446 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Thu, 19 Oct 2023 19:46:10 -0400 Subject: [PATCH 034/197] Segmentation Error --- CMakeLists.txt | 1 + src/perception/object_detector/object_detector.cpp | 8 ++++---- .../object_detector/object_detector.processing.cpp | 14 ++++++++++---- 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fd2975856..11649ce03 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,6 +27,7 @@ set(MROVER_ROS_DEPENDENCIES tf2_ros tf2_geometry_msgs gazebo_ros + cv_bridge ) # Search a path for all files matching a glob pattern and extract the filenames diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index ff792bce5..947d2e5bd 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -25,13 +25,13 @@ namespace mrover { cv::VideoCapture cap(0); cv::Mat img; while (true) { - cap.read(img); - sensor_msgs::ImageConstPtr imgMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg(); - ObjectDetectorNodelet::imageCallback(imgMsg); + if (cap.read(img)) { + sensor_msgs::ImageConstPtr imgMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg(); + ObjectDetectorNodelet::imageCallback(imgMsg); + } } mImgSub = mNh.subscribe("image", 1, &ObjectDetectorNodelet::imageCallback, this); } - } // namespace mrover int main(int argc, char** argv) { diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index a5ffaf5e2..c56ea9338 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -1,5 +1,6 @@ #include "object_detector.hpp" #include "inference.h" +#include #include #include #include @@ -21,12 +22,17 @@ namespace mrover { assert(msg); assert(msg->height > 0); assert(msg->width > 0); - cv::VideoCapture cap(0); - cv::Mat rawImg; // = cv::imread("//home//jabra//Downloads//Water.jpg"); - cap.read(rawImg); + typedef boost::shared_ptr CvImagePtr; + CvImagePtr ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); + cv::Mat rawImg = ptr->image.clone(); + //cv::Mat rawImg{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; + ROS_INFO("bool %d", rawImg.cols); cv::Mat sizedImg; + cv::imshow("window", rawImg); + cv::waitKey(0); + cv::resize(rawImg, sizedImg, cv::Size(640, 640)); - cv::Mat imageView{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; + // ROS_INFO("width %d height %d", rawImg.cols, rawImg.rows); std::vector detections = inference.runInference(sizedImg); if (!detections.empty()) { From b74bf908c19ee56677afc6fd82a136df8c725af8 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Sun, 22 Oct 2023 02:11:47 -0400 Subject: [PATCH 035/197] get camera images with gstreamer, add nodelet to cmakelists --- CMakeLists.txt | 6 +- .../long_range_cam/long_range_cam.cpp | 173 +++--------------- .../long_range_cam/long_range_cam.hpp | 24 +-- .../long_range_tag_detector.cpp | 12 +- .../long_range_tag_detector.hpp | 6 + .../long_range_tag_detector.processing.cpp | 1 - .../{long_range => long_range_cam}/pch.hpp | 0 7 files changed, 41 insertions(+), 181 deletions(-) rename src/perception/tag_detector/{long_range => long_range_cam}/long_range_tag_detector.cpp (92%) rename src/perception/tag_detector/{long_range => long_range_cam}/long_range_tag_detector.hpp (95%) rename src/perception/tag_detector/{long_range => long_range_cam}/long_range_tag_detector.processing.cpp (99%) rename src/perception/tag_detector/{long_range => long_range_cam}/pch.hpp (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index a2b5dc6d4..7cccf14f5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -241,9 +241,9 @@ else () target_precompile_headers(tag_detector_nodelet PRIVATE src/perception/tag_detector/zed/pch.hpp) target_link_libraries(tag_detector_nodelet PRIVATE opencv_core opencv_objdetect opencv_aruco opencv_imgproc lie) - # add_cpp_nodelet_macro(long_range_tag_detector_nodelet src/perception/long_range_cam/*.cpp src/perception/long_range_cam) - # target_precompile_headers(long_range_tag_detector_nodelet PRIVATE src/perception/long_range_cam/pch.hpp) - # target_link_libraries(long_range_tag_detector_nodelet PRIVATE ${OpenCV_LIBRARIES} lie) + add_cpp_nodelet_macro(long_range_tag_detector_nodelet src/perception/tag_detector/long_range_cam/*.cpp src/perception/tag_detector/long_range_cam) + target_precompile_headers(long_range_tag_detector_nodelet PRIVATE src/perception/tag_detector/long_range_cam/pch.hpp) + target_link_libraries(long_range_tag_detector_nodelet PRIVATE ${OpenCV_LIBRARIES} lie) add_cpp_library_macro(lie src/util/lie/*.cpp src/util/lie) diff --git a/src/perception/long_range_cam/long_range_cam.cpp b/src/perception/long_range_cam/long_range_cam.cpp index ecce0bc08..857e3c2a8 100644 --- a/src/perception/long_range_cam/long_range_cam.cpp +++ b/src/perception/long_range_cam/long_range_cam.cpp @@ -1,34 +1,14 @@ #include "long_range_cam.hpp" +#include +#include +#include namespace mrover { using namespace std::chrono_literals; - - cv::VideoCapture cap { - 0, - { cv::CAP_V4L2, - cv::CAP_PROP_FRAME_WIDTH, size.width, - cv::CAP_PROP_FRAME_HEIGHT, size.height, - cv::CAP_PROP_FPS, 30, - - } - void fillImageMessage(sl::Mat& bgra, sensor_msgs::ImagePtr const& msg) { - - - } - }; - void cv::VideoCapture::fillImageMessage(sl::Mat& bgra, sensor_msgs::ImagePtr const& msg) { - assert(bgra.getChannels() == 4); - assert(msg); - msg->height = bgra.getHeight(); - msg->width = bgra.getWidth(); - msg->encoding = sensor_msgs::image_encodings::BGRA8; - msg->step = bgra.getStepBytes(); - msg->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; - bytes_to_copy = image.total() * image.elemSize() - std::memcpy(msg->data.data(), bgra.data, bytes_to_copy) - } + // gst-launch-1.0 -e -v v4l2src device=/dev/video4 ! videoconvert ! videoscale ! videorate ! "video/x-raw,format=I420,width=640,height=480,framerate=30/1" ! x264enc key-int-max=15 ! rtph264pay ! udpsink host=localhost port=5000 + // gst-launch-1.0 -v udpsrc uri=udp://localhost:5000 ! application/x-rtp,media=video,clock-rate=90000,encoding-name=H264,payload=96 ! rtph264depay ! avdec_h264 ! autovideosink /** * @brief Load config, open the camera, and start our threads */ @@ -38,148 +18,47 @@ namespace mrover { // MT means multithreaded mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mCamInfoPub = mNh.advertise("camera/right/camera_info", 1); + mCamInfoPub = mNh.advertise("long_range_cam/camera_info", 1); image_transport::ImageTransport it{mNh}; - mRightImgPub = it.advertise("long_range_cam/image", 1); - - int imageWidth{}; - int imageHeight{}; - // TODO: edit these so they correspond to the long range cam's resolution - mPnh.param("image_width", imageWidth, 1280); - mPnh.param("image_height", imageHeight, 720); - - if (imageWidth < 0 || imageHeight < 0) { - throw std::invalid_argument("Invalid image dimensions"); - } - if (mGrabTargetFps < 0) { - throw std::invalid_argument("Invalid grab target framerate"); - } - - mImageResolution = sl::Resolution(imageWidth, imageHeight); - mPointResolution = sl::Resolution(imageWidth, imageHeight); + mImgPub = it.advertise("long_range_cam/image", 1); - NODELET_INFO("Resolution: %s image: %zux%zu points: %zux%zu", - grabResolutionString.c_str(), mImageResolution.width, mImageResolution.height); - - sl::InitParameters initParameters; - if (mSvoPath) { - initParameters.input.setFromSVOFile(mSvoPath); - } else { - initParameters.input.setFromCameraID(-1, sl::BUS_TYPE::USB); - } - initParameters.depth_stabilization = mUseDepthStabilization; - initParameters.camera_resolution = stringToZedEnum(grabResolutionString); - initParameters.depth_mode = stringToZedEnum(depthModeString); - initParameters.coordinate_units = sl::UNIT::METER; - initParameters.sdk_verbose = true; // Log useful information - initParameters.camera_fps = mGrabTargetFps; - initParameters.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; // Match ROS - initParameters.depth_maximum_distance = mDepthMaximumDistance; - - if (mZed.open(initParameters) != sl::ERROR_CODE::SUCCESS) { - throw std::runtime_error("ZED failed to open"); + // TODO: Fix hardcoded components of this string + mCapture = cv::VideoCapture("v4l2src device=/dev/video4 ! videoconvert ! videoscale ! videorate ! 'video / x - raw, format = I420, width = 640, height = 480, framerate = 30 / 1' ! x264enc key-int-max=15 ! rtph264pay ! udpsink"); + cv::VideoWriter out = cv::VideoWriter("udpsrc ! application/x-rtp,media=video,clock-rate=90000,encoding-name=H264,payload=96 ! rtph264depay ! avdec_h264 ! autovideosink host=localhost port =5000", cv::CAP_GSTREAMER, 0, 30, cv::Size(640, 480), true); + if (!mCapture.isOpened()) { + throw std::runtime_error("Long range cam failed to open"); } - - cudaDeviceProp prop{}; - cudaGetDeviceProperties(&prop, 0); - ROS_INFO("MP count: %d, Max threads/MP: %d, Max blocks/MP: %d, max threads/block: %d", - prop.multiProcessorCount, prop.maxThreadsPerMultiProcessor, prop.maxBlocksPerMultiProcessor, prop.maxThreadsPerBlock); - - mGrabThread = std::thread(&ZedNodelet::grabUpdate, this); - - } catch (std::exception const& e) { - NODELET_FATAL("Exception while starting: %s", e.what()); - ros::shutdown(); - } - } - - /** - * @brief Grabs measures from the ZED. - * - * This update loop needs to happen as fast as possible. - * grab() on the ZED updates positional tracking (visual odometry) which works best at high update rates. - * As such we retrieve the image and point cloud on the GPU to send to the other thread for processing. - * This only happens if the other thread is ready to avoid blocking, hence the try lock. - */ - void ZedNodelet::grabUpdate() { - try { - NODELET_INFO("Starting grab thread"); - while (ros::ok()) { - mGrabThreadProfiler.beginLoop(); - - sl::RuntimeParameters runtimeParameters; - runtimeParameters.confidence_threshold = mDepthConfidence; - runtimeParameters.texture_confidence_threshold = mTextureConfidence; - - if (mZed.grab(runtimeParameters) != sl::ERROR_CODE::SUCCESS) - throw std::runtime_error("ZED failed to grab"); - mGrabThreadProfiler.measureEvent("Grab"); - - // Retrieval has to happen on the same thread as grab so that the image and point cloud are synced - if (mRightImgPub.getNumSubscribers()) - if (mZed.retrieveImage(mGrabMeasures.rightImage, sl::VIEW::RIGHT, sl::MEM::GPU, mImageResolution) != sl::ERROR_CODE::SUCCESS) - throw std::runtime_error("ZED failed to retrieve right image"); - // Only left set is used for processing - if (mZed.retrieveImage(mGrabMeasures.leftImage, sl::VIEW::LEFT, sl::MEM::GPU, mImageResolution) != sl::ERROR_CODE::SUCCESS) - throw std::runtime_error("ZED failed to retrieve left image"); - if (mZed.retrieveMeasure(mGrabMeasures.leftPoints, sl::MEASURE::XYZ, sl::MEM::GPU, mPointResolution) != sl::ERROR_CODE::SUCCESS) - throw std::runtime_error("ZED failed to retrieve point cloud"); - - assert(mGrabMeasures.leftImage.timestamp == mGrabMeasures.leftPoints.timestamp); - - - mGrabMeasures.time = mSvoPath ? ros::Time::now() : slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); - mGrabThreadProfiler.measureEvent("Retrieve"); - - // If the processing thread is busy skip - // We want this thread to run as fast as possible for grab and positional tracking - if (mSwapMutex.try_lock()) { - std::swap(mGrabMeasures, mPcMeasures); - mIsSwapReady = true; - mSwapMutex.unlock(); - mSwapCv.notify_one(); + ROS_ERROR("NODELET LOADED"); + cv::Mat frame; + while (true) { + mCapture.read(frame); + if (frame.empty()) { + break; + } + out.write(frame); + cv::imshow("Sender", frame); + if (cv::waitKey(1) == 's') { + break; } - mGrabThreadProfiler.measureEvent("Try swap"); - - mGrabUpdateTick++; } - - mZed.close(); - NODELET_INFO("Grab thread finished"); - } catch (std::exception const& e) { - NODELET_FATAL("Exception while running grab thread: %s", e.what()); - mZed.close(); + NODELET_FATAL("Exception while starting: %s", e.what()); ros::shutdown(); - std::exit(EXIT_FAILURE); } } LongRangeCamNodelet::~LongRangeCamNodelet() { NODELET_INFO("Long range cam node shutting down"); - mPointCloudThread.join(); - mGrabThread.join(); } - ZedNodelet::Measures::Measures(ZedNodelet::Measures&& other) noexcept { - *this = std::move(other); - } - - ZedNodelet::Measures& ZedNodelet::Measures::operator=(ZedNodelet::Measures&& other) noexcept { - sl::Mat::swap(other.leftImage, leftImage); - sl::Mat::swap(other.rightImage, rightImage); - sl::Mat::swap(other.leftPoints, leftPoints); - std::swap(time, other.time); - return *this; - } } // namespace mrover int main(int argc, char** argv) { - ros::init(argc, argv, "zed_wrapper"); + ros::init(argc, argv, "long_range_cam"); // Start the ZED Nodelet nodelet::Loader nodelet; - nodelet.load(ros::this_node::getName(), "mrover/ZedNodelet", ros::names::getRemappings(), {}); + nodelet.load(ros::this_node::getName(), "mrover/LongRangeCamNodelet", ros::names::getRemappings(), {}); ros::spin(); diff --git a/src/perception/long_range_cam/long_range_cam.hpp b/src/perception/long_range_cam/long_range_cam.hpp index 8bd48c2ed..e2f776fc9 100644 --- a/src/perception/long_range_cam/long_range_cam.hpp +++ b/src/perception/long_range_cam/long_range_cam.hpp @@ -3,33 +3,18 @@ namespace mrover { class LongRangeCamNodelet : public nodelet::Nodelet { - + private: cv::VideoCapture mCapture; - struct Measures { - ros::Time time; - cv::OutputArray mImage; - - Measures() = default; - - Measures(Measures&) = delete; - Measures& operator=(Measures&) = delete; - - Measures(Measures&&) noexcept; - Measures& operator=(Measures&&) noexcept; - }; - ros::NodeHandle mNh, mPnh; ros::Publisher mCamInfoPub; image_transport::Publisher mImgPub; - Measures mGrabMeasures, mPubMeasures; - std::thread mReadThread; std::mutex mSwapMutex; - std::condition_variable mSwapCv; + boost::condition_variable mSwapCv; std::atomic_bool mIsSwapReady = false; LoopProfiler mGrabThreadProfiler{"Long Range Cam Grab"}; @@ -47,9 +32,4 @@ namespace mrover { void imagePubUpdate(); }; - void fillCameraInfoMessages(sl::CalibrationParameters& calibration, sl::Resolution const& resolution, - sensor_msgs::CameraInfoPtr const& leftInfoMsg, sensor_msgs::CameraInfoPtr const& rightInfoMsg); - - void fillImageMessage(sl::Mat& bgra, sensor_msgs::ImagePtr const& msg); - } // namespace mrover diff --git a/src/perception/tag_detector/long_range/long_range_tag_detector.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp similarity index 92% rename from src/perception/tag_detector/long_range/long_range_tag_detector.cpp rename to src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp index cec4a48b6..3159de9e5 100644 --- a/src/perception/tag_detector/long_range/long_range_tag_detector.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp @@ -1,4 +1,4 @@ -#include "tag_detector.hpp" +#include "long_range_tag_detector.hpp" namespace mrover { @@ -6,12 +6,10 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); mDetectorParams = new cv::aruco::DetectorParameters(); - auto defaultDetectorParams = cv::aruco::DetectorParameters::create(); + auto defaultDetectorParams = cv::makePtr(); int dictionaryNumber; mPnh.param("publish_images", mPublishImages, true); - using DictEnumType = std::underlying_type_t; - mPnh.param("dictionary", dictionaryNumber, static_cast(cv::aruco::DICT_4X4_50)); mPnh.param("min_hit_count_before_publish", mMinHitCountBeforePublish, 5); mPnh.param("max_hit_count", mMaxHitCount, 5); mPnh.param("tag_increment_weight", mTagIncrementWeight, 2); @@ -19,10 +17,8 @@ namespace mrover { mIt.emplace(mNh); mImgPub = mIt->advertise("long_range_tag_detection", 1); - mDictionary = cv::aruco::getPredefinedDictionary(dictionaryNumber); - mPcSub = mNh.subscribe("long_range_cam/image", 1, &LongRangeTagDetectorNodelet::imageCallback, this); - mServiceEnableDetections = mNh.advertiseService("enable_detections", &TagDetectorNodelet::enableDetectionsCallback, this); + mServiceEnableDetections = mNh.advertiseService("enable_detections", &LongRangeTagDetectorNodelet::enableDetectionsCallback, this); // Lambda handles passing class pointer (implicit first parameter) to configCallback mCallbackType = [this](mrover::DetectorParamsConfig& config, uint32_t level) { configCallback(config, level); }; @@ -90,7 +86,7 @@ namespace mrover { NODELET_INFO("Tag detection ready, use odom frame: %s, min hit count: %d, max hit count: %d, hit increment weight: %d, hit decrement weight: %d", mUseOdom ? "true" : "false", mMinHitCountBeforePublish, mMaxHitCount, mTagIncrementWeight, mTagDecrementWeight); } - void TagDetectorNodelet::configCallback(mrover::DetectorParamsConfig& config, uint32_t level) { + void LongRangeTagDetectorNodelet::configCallback(mrover::DetectorParamsConfig& config, uint32_t level) { // Don't load initial config, since it will overwrite the rosparam settings if (level == std::numeric_limits::max()) return; diff --git a/src/perception/tag_detector/long_range/long_range_tag_detector.hpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp similarity index 95% rename from src/perception/tag_detector/long_range/long_range_tag_detector.hpp rename to src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp index 4dc90f58e..d30016efd 100644 --- a/src/perception/tag_detector/long_range/long_range_tag_detector.hpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp @@ -29,9 +29,11 @@ namespace mrover { //Constatns set in ROS params for num hits needed to publish int mMinHitCountBeforePublish{}; + int mBaseHitCount{}; int mMaxHitCount{}; int mTagIncrementWeight{}; int mTagDecrementWeight{}; + int mTagRemoveWeight{}; cv::Ptr mDetectorParams; cv::Ptr mDictionary; @@ -142,5 +144,9 @@ namespace mrover { * only if mPublishImages and the topic has a subscriber */ void publishTagsOnImage(); + + void configCallback(mrover::DetectorParamsConfig& config, uint32_t level); + + bool enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); }; }; // namespace mrover diff --git a/src/perception/tag_detector/long_range/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp similarity index 99% rename from src/perception/tag_detector/long_range/long_range_tag_detector.processing.cpp rename to src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp index e14136639..6300f4780 100644 --- a/src/perception/tag_detector/long_range/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp @@ -1,6 +1,5 @@ #include "long_range_tag_detector.hpp" -#include "../../point.hpp" //Might not actually need?' #include "mrover/LongRangeTags.h" #include diff --git a/src/perception/tag_detector/long_range/pch.hpp b/src/perception/tag_detector/long_range_cam/pch.hpp similarity index 100% rename from src/perception/tag_detector/long_range/pch.hpp rename to src/perception/tag_detector/long_range_cam/pch.hpp From a8a814779d3f03d2fe084d90280cbf7ba4859033 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Tue, 24 Oct 2023 12:04:18 -0400 Subject: [PATCH 036/197] add bearing function defs --- CMakeLists.txt | 8 ++++++-- .../long_range/long_range_tag_detector.cpp | 4 ++-- .../long_range/long_range_tag_detector.hpp | 13 +++++++++++-- .../long_range_tag_detector.processing.cpp | 8 +++++++- .../tag_detector/zed/tag_detector.processing.cpp | 2 +- 5 files changed, 27 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1ba21c323..5dec0c05c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -236,10 +236,14 @@ else () # define an add and link macro # Put items here to build macro(add_and_link_macro) - add_cpp_nodelet_macro(tag_detector_nodelet "src/perception/tag_detector/*.cpp" "src/perception/tag_detector") - target_precompile_headers(tag_detector_nodelet PRIVATE src/perception/tag_detector/pch.hpp) + add_cpp_nodelet_macro(tag_detector_nodelet "src/perception/tag_detector/zed/*.cpp" "src/perception/tag_detector/zed") + target_precompile_headers(tag_detector_nodelet PRIVATE src/perception/tag_detector/zed/pch.hpp) target_link_libraries(tag_detector_nodelet PRIVATE ${OpenCV_LIBRARIES} lie) + add_cpp_nodelet_macro(long_range_tag_detector_nodelet "src/perception/tag_detector/long_range/*.cpp" "src/perception/tag_detector/long_range") + target_precompile_headers(tag_detector_nodelet PRIVATE src/perception/tag_detector/long_range/pch.hpp) + target_link_libraries(long_range_tag_detector_nodelet PRIVATE ${OpenCV_LIBRARIES} lie) + add_cpp_node_macro(brushed_motors "src/esw/brushed_motors/*.cpp") add_cpp_library_macro(lie "src/util/lie/*.cpp" "src/util/lie") diff --git a/src/perception/tag_detector/long_range/long_range_tag_detector.cpp b/src/perception/tag_detector/long_range/long_range_tag_detector.cpp index cec4a48b6..6b2eececd 100644 --- a/src/perception/tag_detector/long_range/long_range_tag_detector.cpp +++ b/src/perception/tag_detector/long_range/long_range_tag_detector.cpp @@ -1,4 +1,4 @@ -#include "tag_detector.hpp" +#include "long_range_tag_detector.hpp" namespace mrover { @@ -90,7 +90,7 @@ namespace mrover { NODELET_INFO("Tag detection ready, use odom frame: %s, min hit count: %d, max hit count: %d, hit increment weight: %d, hit decrement weight: %d", mUseOdom ? "true" : "false", mMinHitCountBeforePublish, mMaxHitCount, mTagIncrementWeight, mTagDecrementWeight); } - void TagDetectorNodelet::configCallback(mrover::DetectorParamsConfig& config, uint32_t level) { + void LongRangeTagDetectorNodelet::configCallback(mrover::DetectorParamsConfig& config, uint32_t level) { // Don't load initial config, since it will overwrite the rosparam settings if (level == std::numeric_limits::max()) return; diff --git a/src/perception/tag_detector/long_range/long_range_tag_detector.hpp b/src/perception/tag_detector/long_range/long_range_tag_detector.hpp index 4dc90f58e..ae5a7a77f 100644 --- a/src/perception/tag_detector/long_range/long_range_tag_detector.hpp +++ b/src/perception/tag_detector/long_range/long_range_tag_detector.hpp @@ -1,3 +1,4 @@ +#include "pch.hpp" #include "mrover/LongRangeTag.h" #include "mrover/LongRangeTags.h" @@ -96,10 +97,10 @@ namespace mrover { /** * @see updateNewlyIdentifiedTags() * @param tagId - the tagId of the current tag - * @param tagCorners - Reference to the mTagCorners vector of Point2fs for the current id of the lrt being created + * @param tag_bearing - Bearing of the current tag * @return a new LongRangeTag */ - LongRangeTagStruct createLrt(int tagId, std::vector& tagCorners); + LongRangeTagStruct createLrt(int tagId, float bearing); /** * @see getNormedTagCenter @@ -121,6 +122,14 @@ namespace mrover { cv::Point2f getNormedTagCenterOffset(std::vector& tagCorners) const; + /** + * Given the known tag information and the ZED FOV (hard code for now, we'll + * assign params later), calculate relative bearing of a detected tag + * @param tagCorners reference to tag corners, passed to @see getTagCenterPixels + * @return float of tag bearing + */ + float getTagBearing(std::vector& tagCorners) const; + /** * @see updateHitCounts() * Helper function used in updateHitCounts diff --git a/src/perception/tag_detector/long_range/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range/long_range_tag_detector.processing.cpp index 63975f713..4b69cd108 100644 --- a/src/perception/tag_detector/long_range/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range/long_range_tag_detector.processing.cpp @@ -1,6 +1,5 @@ #include "long_range_tag_detector.hpp" -#include "../point.hpp" //Might not actually need?' #include "mrover/LongRangeTags.h" #include @@ -90,6 +89,7 @@ namespace mrover { } LongRangeTagStruct LongRangeTagDetectorNodelet::createLrt(int tagId, std::vector& tagCorners) { + //TODO: Modify the LRT struct and this function to assign a bearing instead of an imageCenter LongRangeTagStruct lrt; lrt.hitCount = mBaseHitCount; //Start at base hit count value @@ -155,6 +155,12 @@ namespace mrover { return offsetCenterPoint; } + float getTagBearing(std::vector& tagCorners) const { + //TODO: Implement me! + + return {}; + } + void LongRangeTagDetectorNodelet::publishThresholdTags() { //Loop through all the tags LongRangeTags tagsMessage; // diff --git a/src/perception/tag_detector/zed/tag_detector.processing.cpp b/src/perception/tag_detector/zed/tag_detector.processing.cpp index b1033d61d..4dc8649bf 100644 --- a/src/perception/tag_detector/zed/tag_detector.processing.cpp +++ b/src/perception/tag_detector/zed/tag_detector.processing.cpp @@ -1,6 +1,6 @@ #include "tag_detector.hpp" -#include "../point.hpp" +#include "../../point.hpp" namespace mrover { From 2e63217c729532b05af5e38c9a85d576cc945c76 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Tue, 24 Oct 2023 17:57:05 -0400 Subject: [PATCH 037/197] Pre ZED Testing --- .../object_detector/object_detector.cpp | 9 --------- .../object_detector/object_detector.hpp | 1 + .../object_detector.processing.cpp | 17 +++++++++++------ 3 files changed, 12 insertions(+), 15 deletions(-) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 947d2e5bd..f9ee31650 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -21,15 +21,6 @@ namespace mrover { // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases // TODO(percep/obj-detectr): make this configurable - //TEMP CODE - cv::VideoCapture cap(0); - cv::Mat img; - while (true) { - if (cap.read(img)) { - sensor_msgs::ImageConstPtr imgMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg(); - ObjectDetectorNodelet::imageCallback(imgMsg); - } - } mImgSub = mNh.subscribe("image", 1, &ObjectDetectorNodelet::imageCallback, this); } } // namespace mrover diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index d815c10a7..6d5bb0786 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -1,5 +1,6 @@ #include "inference.h" #include "pch.hpp" +#include namespace mrover { diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index c56ea9338..f89eabb9c 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -24,15 +24,11 @@ namespace mrover { assert(msg->width > 0); typedef boost::shared_ptr CvImagePtr; CvImagePtr ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); - cv::Mat rawImg = ptr->image.clone(); + cv::Mat rawImg = ptr->image; //cv::Mat rawImg{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; - ROS_INFO("bool %d", rawImg.cols); cv::Mat sizedImg; - cv::imshow("window", rawImg); - cv::waitKey(0); cv::resize(rawImg, sizedImg, cv::Size(640, 640)); - // ROS_INFO("width %d height %d", rawImg.cols, rawImg.rows); std::vector detections = inference.runInference(sizedImg); if (!detections.empty()) { @@ -48,7 +44,16 @@ namespace mrover { msgData.yBoxPixel = (float) box.y; msgData.width = (float) box.width; msgData.height = (float) box.height; - msgData.heading = 0; + + //Get the heading + /* + float objectHeading; + float zedFOV = 54; //54 @ 720; 42 @ 1080 + float fovPerPixel = (float) zedFOV / (float) (msg->width); + float xCenter = (float) box.x + ((float) box.width) / 2 - ((float) msg->width) / 2; + objectHeading = xCenter * fovPerPixel; + msgData.heading = objectHeading; + */ //Put the rectangle on the image From d6dd4fec79653fce176061f12c035e45ae06473f Mon Sep 17 00:00:00 2001 From: jbrhm Date: Tue, 24 Oct 2023 19:05:15 -0400 Subject: [PATCH 038/197] ZED Object Detection (Needs Speeding up) --- CMakeLists.txt | 1 - .../object_detector/object_detector.cpp | 3 +- .../object_detector/object_detector.hpp | 6 ++- .../object_detector.processing.cpp | 43 +++++++++++++------ .../zed_wrapper/zed_wrapper.bridge.cu | 2 +- 5 files changed, 39 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8bb1497db..5912b3f58 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,7 +27,6 @@ set(MROVER_ROS_DEPENDENCIES tf2_ros tf2_geometry_msgs gazebo_ros - cv_bridge ) # Search a path for all files matching a glob pattern and extract the filenames diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index f9ee31650..6f577cfbf 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -21,7 +21,8 @@ namespace mrover { // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases // TODO(percep/obj-detectr): make this configurable - mImgSub = mNh.subscribe("image", 1, &ObjectDetectorNodelet::imageCallback, this); + mImgSub = mNh.subscribe("/camera/left/image", 1, &ObjectDetectorNodelet::imageCallback, this); + //mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); } } // namespace mrover diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 6d5bb0786..18ad5a9f9 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -1,6 +1,7 @@ #include "inference.h" #include "pch.hpp" #include +#include namespace mrover { @@ -12,10 +13,13 @@ namespace mrover { // Publishers - ros::Subscriber mImgSub; + ros::Publisher mDebugImgPub; // Subscribers + ros::Subscriber mImgSub; + + dynamic_reconfigure::Server mConfigServer; dynamic_reconfigure::Server::CallbackType mCallbackType; diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index f89eabb9c..d6ed2bc39 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include @@ -22,13 +23,13 @@ namespace mrover { assert(msg); assert(msg->height > 0); assert(msg->width > 0); - typedef boost::shared_ptr CvImagePtr; - CvImagePtr ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); - cv::Mat rawImg = ptr->image; - //cv::Mat rawImg{static_cast(msg->width), static_cast(msg->height), CV_8UC3, const_cast(msg->data.data())}; - cv::Mat sizedImg; - cv::resize(rawImg, sizedImg, cv::Size(640, 640)); + cv::Mat sizedImg{static_cast(msg->height), static_cast(msg->width), CV_8UC4, const_cast(msg->data.data())}; + + cv::cvtColor(sizedImg, sizedImg, cv::COLOR_BGRA2BGR); + + cv::imshow("heheh", sizedImg); + cv::waitKey(1); std::vector detections = inference.runInference(sizedImg); if (!detections.empty()) { @@ -40,10 +41,11 @@ namespace mrover { DetectedObject msgData; msgData.object_type = firstDetection.className; msgData.detection_confidence = classConfidence; - msgData.xBoxPixel = (float) box.x; - msgData.yBoxPixel = (float) box.y; - msgData.width = (float) box.width; - msgData.height = (float) box.height; + + msgData.xBoxPixel = static_cast(box.x); + msgData.yBoxPixel = static_cast(box.y); + msgData.width = static_cast(box.width); + msgData.height = static_cast(box.height); //Get the heading /* @@ -55,7 +57,6 @@ namespace mrover { msgData.heading = objectHeading; */ - //Put the rectangle on the image cv::rectangle(sizedImg, box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); @@ -69,8 +70,26 @@ namespace mrover { //Show the image cv::imshow("obj detector", sizedImg); cv::waitKey(1); + //Print the type of objected detected - ROS_INFO(firstDetection.className.c_str()); + ROS_INFO_STREAM(firstDetection.className.c_str()); + + // if (mDebugImgPub.getNumSubscribers() > 0) { + // // Create sensor msg image + // sensor_msgs::ImageConstPtr newMessage; + + // newMessage->height = sizedImg.getHeight(); + // newMessage->width = sizedImg.getWidth(); + // newMessage->encoding = sensor_msgs::image_encodings::BGRA8; + // newMessage->step = sizedImg.getStepBytes(); + // newMessage->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; + // auto* bgrGpuPtr = bgra.getPtr(sl::MEM::GPU); + // size_t size = msg->step * msg->height; + // msg->data.resize(size); + + // checkCudaError(cudaMemcpy(msg->data.data(), bgrGpuPtr, size, cudaMemcpyDeviceToHost)); + // } + // Publish to } //Look at yolov8 documentation for the output matrix /* diff --git a/src/perception/zed_wrapper/zed_wrapper.bridge.cu b/src/perception/zed_wrapper/zed_wrapper.bridge.cu index 62335d6e9..8a4423cbc 100644 --- a/src/perception/zed_wrapper/zed_wrapper.bridge.cu +++ b/src/perception/zed_wrapper/zed_wrapper.bridge.cu @@ -37,7 +37,7 @@ namespace mrover { assert(bgraGpu.getWidth() >= xyzGpu.getWidth()); assert(bgraGpu.getHeight() >= xyzGpu.getHeight()); assert(bgraGpu.getChannels() == 4); - assert(xyzGpu.getChannels() == 3); + assert(xyzGpu.getChannels() == 4); assert(msg); auto* bgraGpuPtr = bgraGpu.getPtr(sl::MEM::GPU); From a0a88c66a63aa05b19391d37f12ed8d78a8660b9 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Tue, 24 Oct 2023 19:33:41 -0400 Subject: [PATCH 039/197] Image Detections Published Properly --- .../object_detector/object_detector.cpp | 2 +- .../object_detector.processing.cpp | 48 +++++++++++-------- 2 files changed, 28 insertions(+), 22 deletions(-) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 6f577cfbf..185e42274 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -22,7 +22,7 @@ namespace mrover { // TODO(percep/obj-detectr): make this configurable mImgSub = mNh.subscribe("/camera/left/image", 1, &ObjectDetectorNodelet::imageCallback, this); - //mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); + mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); } } // namespace mrover diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index d6ed2bc39..e2895aa97 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -28,9 +28,6 @@ namespace mrover { cv::cvtColor(sizedImg, sizedImg, cv::COLOR_BGRA2BGR); - cv::imshow("heheh", sizedImg); - cv::waitKey(1); - std::vector detections = inference.runInference(sizedImg); if (!detections.empty()) { Detection firstDetection = detections[0]; @@ -67,28 +64,37 @@ namespace mrover { int font_weight = 2; putText(sizedImg, msgData.object_type, text_position, cv::FONT_HERSHEY_COMPLEX, font_size, font_Color, font_weight); //Putting the text in the matrix// - //Show the image - cv::imshow("obj detector", sizedImg); - cv::waitKey(1); //Print the type of objected detected ROS_INFO_STREAM(firstDetection.className.c_str()); - // if (mDebugImgPub.getNumSubscribers() > 0) { - // // Create sensor msg image - // sensor_msgs::ImageConstPtr newMessage; - - // newMessage->height = sizedImg.getHeight(); - // newMessage->width = sizedImg.getWidth(); - // newMessage->encoding = sensor_msgs::image_encodings::BGRA8; - // newMessage->step = sizedImg.getStepBytes(); - // newMessage->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; - // auto* bgrGpuPtr = bgra.getPtr(sl::MEM::GPU); - // size_t size = msg->step * msg->height; - // msg->data.resize(size); - - // checkCudaError(cudaMemcpy(msg->data.data(), bgrGpuPtr, size, cudaMemcpyDeviceToHost)); - // } + if (mDebugImgPub.getNumSubscribers() > 0 || true) { + // Create sensor msg image + sensor_msgs::Image newDebugImageMessage; + + //Change the mat to from bgr to bgra + cv::cvtColor(sizedImg, sizedImg, cv::COLOR_BGR2BGRA); + + + newDebugImageMessage.height = sizedImg.rows; + newDebugImageMessage.width = sizedImg.cols; + + newDebugImageMessage.encoding = sensor_msgs::image_encodings::BGRA8; + + //Calculate the step for the imgMsg + newDebugImageMessage.step = sizedImg.channels() * sizedImg.cols; + newDebugImageMessage.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; + + // auto* bgrGpuPtr = sizedImage.getPtr(sl::MEM::GPU); + auto imgPtr = sizedImg.data; + + size_t size = msg->step * msg->height; + newDebugImageMessage.data.resize(size); + + memcpy(newDebugImageMessage.data.data(), imgPtr, size); + + mDebugImgPub.publish(newDebugImageMessage); + } // Publish to } //Look at yolov8 documentation for the output matrix From 0bf35627d45688a677ab4810a1d8274c88cf67a1 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Tue, 24 Oct 2023 19:38:25 -0400 Subject: [PATCH 040/197] Publish Nav Data --- src/perception/object_detector/object_detector.cpp | 1 + src/perception/object_detector/object_detector.hpp | 2 ++ .../object_detector/object_detector.processing.cpp | 7 +++++-- 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 185e42274..9c1b39716 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -23,6 +23,7 @@ namespace mrover { mImgSub = mNh.subscribe("/camera/left/image", 1, &ObjectDetectorNodelet::imageCallback, this); mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); + mDetectionData = mNh.advertise("/object_detector/detected_object", 1); } } // namespace mrover diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 18ad5a9f9..8b9a48295 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -14,6 +14,8 @@ namespace mrover { // Publishers ros::Publisher mDebugImgPub; + ros::Publisher mDetectionData; + // Subscribers diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index e2895aa97..6708c3d2e 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -45,14 +45,14 @@ namespace mrover { msgData.height = static_cast(box.height); //Get the heading - /* + float objectHeading; float zedFOV = 54; //54 @ 720; 42 @ 1080 float fovPerPixel = (float) zedFOV / (float) (msg->width); float xCenter = (float) box.x + ((float) box.width) / 2 - ((float) msg->width) / 2; objectHeading = xCenter * fovPerPixel; msgData.heading = objectHeading; - */ + //Put the rectangle on the image cv::rectangle(sizedImg, box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); @@ -68,6 +68,9 @@ namespace mrover { //Print the type of objected detected ROS_INFO_STREAM(firstDetection.className.c_str()); + //Publish Dectection Data + mDetectionData.publish(msgData); + if (mDebugImgPub.getNumSubscribers() > 0 || true) { // Create sensor msg image sensor_msgs::Image newDebugImageMessage; From 488d9f70cde2ab69ee8c407a682e3ba23db6c6d2 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Wed, 25 Oct 2023 15:31:47 -0400 Subject: [PATCH 041/197] fix cmake and bearing starter code --- CMakeLists.txt | 14 ++++++-------- .../long_range_cam/long_range_tag_detector.hpp | 4 ++-- .../long_range_tag_detector.processing.cpp | 2 +- 3 files changed, 9 insertions(+), 11 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a1d0398c..66adc8ee6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -235,20 +235,18 @@ else () # define an add and link macro # Put items here to build macro(add_and_link_macro) - add_cpp_nodelet_macro(tag_detector_nodelet "src/perception/tag_detector/zed/*.cpp" src/perception/tag_detector/zed) + add_cpp_nodelet_macro(tag_detector_nodelet src/perception/tag_detector/zed/*.cpp src/perception/tag_detector/zed) target_precompile_headers(tag_detector_nodelet PRIVATE src/perception/tag_detector/zed/pch.hpp) target_link_libraries(tag_detector_nodelet PRIVATE ${OpenCV_LIBRARIES} opencv_core opencv_objdetect opencv_aruco opencv_imgproc lie) - add_cpp_nodelet_macro(long_range_tag_detector_nodelet "src/perception/tag_detector/long_range/*.cpp" src/perception/tag_detector/long_range) - target_precompile_headers(tag_detector_nodelet PRIVATE src/perception/tag_detector/long_range/pch.hpp) - target_link_libraries(long_range_tag_detector_nodelet PRIVATE ${OpenCV_LIBRARIES} opencv_core opencv_objdetect opencv_aruco opencv_imgproc lie) - - add_cpp_node_macro(brushed_motors "src/esw/brushed_motors/*.cpp") - add_cpp_nodelet_macro(long_range_tag_detector_nodelet src/perception/tag_detector/long_range_cam/*.cpp src/perception/tag_detector/long_range_cam) - target_precompile_headers(long_range_tag_detector_nodelet PRIVATE src/perception/tag_detector/long_range_cam/pch.hpp) + target_precompile_headers(tag_detector_nodelet PRIVATE src/perception/tag_detector/long_range_cam/pch.hpp) target_link_libraries(long_range_tag_detector_nodelet PRIVATE ${OpenCV_LIBRARIES} lie) + add_cpp_nodelet_macro(long_range_cam_nodelet src/perception/long_range_cam/*.cpp src/perception/long_range_cam) + target_precompile_headers(long_range_cam_nodelet PRIVATE src/perception/long_range_cam/pch.hpp) + target_link_libraries(long_range_cam_nodelet PRIVATE ${OpenCV_LIBRARIES} lie) + add_cpp_library_macro(lie src/util/lie/*.cpp src/util/lie) add_cpp_interface_library_macro(moteus deps/moteus/lib/cpp/mjbots) diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp index 28af9b33d..0278b8efb 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp @@ -1,6 +1,6 @@ -#include "pch.hpp" #include "mrover/LongRangeTag.h" #include "mrover/LongRangeTags.h" +#include "pch.hpp" #include #include @@ -102,7 +102,7 @@ namespace mrover { * @param tag_bearing - Bearing of the current tag * @return a new LongRangeTag */ - LongRangeTagStruct createLrt(int tagId, float bearing); + LongRangeTagStruct createLrt(int tagId, std::vector& tagCorners); /** * @see getNormedTagCenter diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp index 4b69cd108..44f71678d 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp @@ -155,7 +155,7 @@ namespace mrover { return offsetCenterPoint; } - float getTagBearing(std::vector& tagCorners) const { + float LongRangeTagDetectorNodelet::getTagBearing(std::vector& tagCorners) const { //TODO: Implement me! return {}; From d3c6d5028d1892f0cf6d05d75d89e201f76feae8 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Wed, 25 Oct 2023 22:04:23 -0400 Subject: [PATCH 042/197] add nodelet plugins --- .gitignore | 1 - package.xml | 9 ++++++--- plugins/long_range_cam_plugin.xml | 6 ++++++ plugins/long_range_tag_detector_plugin.xml | 6 ++++++ plugins/tag_detector_plugin.xml | 6 +++--- src/perception/long_range_cam/long_range_cam.cpp | 4 +--- src/perception/long_range_cam/long_range_cam.hpp | 2 ++ 7 files changed, 24 insertions(+), 10 deletions(-) create mode 100644 plugins/long_range_cam_plugin.xml create mode 100644 plugins/long_range_tag_detector_plugin.xml diff --git a/.gitignore b/.gitignore index 0f4fb421a..c12791555 100644 --- a/.gitignore +++ b/.gitignore @@ -42,5 +42,4 @@ RemoteSystemsTempFiles/ *.chm *.settings.xml -*.xml *.launch diff --git a/package.xml b/package.xml index 87ca4865b..8b6a46a9c 100644 --- a/package.xml +++ b/package.xml @@ -87,7 +87,10 @@ - - + + + + + - + \ No newline at end of file diff --git a/plugins/long_range_cam_plugin.xml b/plugins/long_range_cam_plugin.xml new file mode 100644 index 000000000..e717701de --- /dev/null +++ b/plugins/long_range_cam_plugin.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/plugins/long_range_tag_detector_plugin.xml b/plugins/long_range_tag_detector_plugin.xml new file mode 100644 index 000000000..6a7d82109 --- /dev/null +++ b/plugins/long_range_tag_detector_plugin.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/plugins/tag_detector_plugin.xml b/plugins/tag_detector_plugin.xml index 7f9242490..bd6238412 100644 --- a/plugins/tag_detector_plugin.xml +++ b/plugins/tag_detector_plugin.xml @@ -1,6 +1,6 @@ + type="mrover::TagDetectorNodelet" + base_class_type="nodelet::Nodelet"> - + \ No newline at end of file diff --git a/src/perception/long_range_cam/long_range_cam.cpp b/src/perception/long_range_cam/long_range_cam.cpp index 857e3c2a8..25f3de4ec 100644 --- a/src/perception/long_range_cam/long_range_cam.cpp +++ b/src/perception/long_range_cam/long_range_cam.cpp @@ -65,7 +65,5 @@ int main(int argc, char** argv) { return EXIT_SUCCESS; } -#ifdef MROVER_IS_NODELET #include -PLUGINLIB_EXPORT_CLASS(mrover::ZedNodelet, nodelet::Nodelet) -#endif +PLUGINLIB_EXPORT_CLASS(mrover::LongRangeCamNodelet, nodelet::Nodelet) diff --git a/src/perception/long_range_cam/long_range_cam.hpp b/src/perception/long_range_cam/long_range_cam.hpp index e2f776fc9..6c63faaa3 100644 --- a/src/perception/long_range_cam/long_range_cam.hpp +++ b/src/perception/long_range_cam/long_range_cam.hpp @@ -1,5 +1,7 @@ #pragma once +#include "pch.hpp" + namespace mrover { class LongRangeCamNodelet : public nodelet::Nodelet { From 49219f8d5118cbbc32d1370ccbc0f90e4888c1f7 Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Thu, 26 Oct 2023 16:04:04 -0400 Subject: [PATCH 043/197] NOT WORKING BUT TRIED --- src/perception/object_detector/inference.cpp | 9 +++++++-- src/perception/object_detector/inference.h | 2 ++ src/perception/object_detector/object_detector.cpp | 2 +- .../object_detector/object_detector.processing.cpp | 4 +++- 4 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/perception/object_detector/inference.cpp b/src/perception/object_detector/inference.cpp index d9f66d484..0cb6cf84e 100644 --- a/src/perception/object_detector/inference.cpp +++ b/src/perception/object_detector/inference.cpp @@ -19,9 +19,11 @@ std::vector Inference::runInference(const cv::Mat& input) { cv::dnn::blobFromImage(modelInput, blob, 1.0 / 255.0, modelShape, cv::Scalar(), true, false); net.setInput(blob); - std::vector outputs; + std::vector> outputs; net.forward(outputs, net.getUnconnectedOutLayersNames()); + std::cout << outputs[0].data << std::endl; + int rows = outputs[0].size[1]; int dimensions = outputs[0].size[2]; @@ -37,8 +39,10 @@ std::vector Inference::runInference(const cv::Mat& input) { outputs[0] = outputs[0].reshape(1, dimensions); cv::transpose(outputs[0], outputs[0]); } + float* data = (float*) outputs[0].data; + float x_factor = modelInput.cols / modelShape.width; float y_factor = modelInput.rows / modelShape.height; @@ -71,7 +75,8 @@ std::vector Inference::runInference(const cv::Mat& input) { int width = int(w * x_factor); int height = int(h * y_factor); - boxes.push_back(cv::Rect(left, top, width, height)); + + boxes.emplace_back(left, top, width, height); } } else // yolov5 { diff --git a/src/perception/object_detector/inference.h b/src/perception/object_detector/inference.h index 9e59ca7ed..5b9098b9c 100644 --- a/src/perception/object_detector/inference.h +++ b/src/perception/object_detector/inference.h @@ -12,6 +12,8 @@ #include #include +#include "pch.hpp" + struct Detection { int class_id{0}; std::string className{}; diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 9c1b39716..dd1ac7708 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -16,7 +16,7 @@ namespace mrover { //Inference - inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), "", false); + inference = Inference("//home//marshallvielmetti//Downloads//yolov8s.onnx", cv::Size(640, 640), "", true); //read ONNX file into this mNet, YOLOV8, second smallest one // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases // TODO(percep/obj-detectr): make this configurable diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 6708c3d2e..c7b96ee27 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -29,6 +29,8 @@ namespace mrover { cv::cvtColor(sizedImg, sizedImg, cv::COLOR_BGRA2BGR); std::vector detections = inference.runInference(sizedImg); + + if (!detections.empty()) { Detection firstDetection = detections[0]; @@ -66,7 +68,7 @@ namespace mrover { //Print the type of objected detected - ROS_INFO_STREAM(firstDetection.className.c_str()); + // ROS_INFO_STREAM(firstDetection.className.c_str()); //Publish Dectection Data mDetectionData.publish(msgData); From 1b219eabb7cbb75fa1574fdfd47a6e7e67ebc667 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 29 Oct 2023 12:49:09 -0400 Subject: [PATCH 044/197] Add cuda file --- .../object_detector/object_detector.cu | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 src/perception/object_detector/object_detector.cu diff --git a/src/perception/object_detector/object_detector.cu b/src/perception/object_detector/object_detector.cu new file mode 100644 index 000000000..e6ddcd1df --- /dev/null +++ b/src/perception/object_detector/object_detector.cu @@ -0,0 +1,16 @@ +#include +#include +#include + +struct Logger { + + void log(nvinfer1::ILogger::Severity severity, nvinfer1::AsciiChar const* msg) { + } +}; + +Logger logger; + +void setup() { + // TOOD: fix this + nvinfer1::createInferBuilder(logger); +} \ No newline at end of file From 2d6346ac038f3c6c62abd1a18f00703741664205 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Sun, 29 Oct 2023 12:59:59 -0400 Subject: [PATCH 045/197] edit object msg type --- msg/DetectedObject.msg | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/msg/DetectedObject.msg b/msg/DetectedObject.msg index 6b9a4e70a..a530156c4 100644 --- a/msg/DetectedObject.msg +++ b/msg/DetectedObject.msg @@ -1,7 +1,5 @@ string object_type float32 detection_confidence -float32 heading -float32 xBoxPixel -float32 yBoxPixel +float32 bearing float32 width float32 height \ No newline at end of file From 5471f2f7708531800f166a17902abfcbf1fc5b35 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Sun, 29 Oct 2023 14:00:20 -0400 Subject: [PATCH 046/197] Mid Tensor Development --- src/perception/object_detector/cudaWrapper.h | 72 +++++++++++++++ src/perception/object_detector/inference.h | 50 ++++++++++ src/perception/object_detector/ioHelper.cpp | 91 +++++++++++++++++++ src/perception/object_detector/ioHelper.h | 36 ++++++++ .../object_detector/newinference.cpp | 36 ++++++++ .../object_detector/object_detector.cpp | 2 +- .../object_detector/object_detector.cu | 45 +++++++-- .../object_detector.processing.cpp | 1 + 8 files changed, 323 insertions(+), 10 deletions(-) create mode 100644 src/perception/object_detector/cudaWrapper.h create mode 100644 src/perception/object_detector/ioHelper.cpp create mode 100644 src/perception/object_detector/ioHelper.h create mode 100644 src/perception/object_detector/newinference.cpp diff --git a/src/perception/object_detector/cudaWrapper.h b/src/perception/object_detector/cudaWrapper.h new file mode 100644 index 000000000..12591ff70 --- /dev/null +++ b/src/perception/object_detector/cudaWrapper.h @@ -0,0 +1,72 @@ +/* Copyright (c) 1993-2018, NVIDIA CORPORATION. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of NVIDIA CORPORATION nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY + * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef __CUDA_WRAPPER_H__ +#define __CUDA_WRAPPER_H__ + +#include + +namespace cudawrapper { + + class CudaStream { + public: + CudaStream() { + cudaStreamCreate(&mStream); + } + + operator cudaStream_t() { + return mStream; + } + + ~CudaStream() { + cudaStreamDestroy(mStream); + } + + private: + cudaStream_t mStream; + }; + + class CudaEvent { + public: + CudaEvent() { + cudaEventCreate(&mEvent); + } + + operator cudaEvent_t() { + return mEvent; + } + + ~CudaEvent() { + cudaEventDestroy(mEvent); + } + + private: + cudaEvent_t mEvent; + }; + +} // namespace cudawrapper + +#endif /*__CUDA_WRAPPER_H__*/ diff --git a/src/perception/object_detector/inference.h b/src/perception/object_detector/inference.h index 5b9098b9c..752d68869 100644 --- a/src/perception/object_detector/inference.h +++ b/src/perception/object_detector/inference.h @@ -3,10 +3,21 @@ // Cpp native #include +#include #include #include #include + +//Tensor-RT Specific +#include "cudaWrapper.h" +#include +#include +#include + + +#include "ioHelper.h" + // OpenCV / DNN / Inference #include #include @@ -14,6 +25,10 @@ #include "pch.hpp" +using nvinfer1::cuda; +using nvinfer1::ICudaEngine; +using nvinfer1::IExecutionContext; + struct Detection { int class_id{0}; std::string className{}; @@ -50,4 +65,39 @@ class Inference { cv::dnn::Net net; }; +class InferenceNew { +private: + static const int BATCH_SIZE = 1; + + Logger logger; + + + //Ptr to the engine + std::unique_ptr> enginePtr; + //Ptr to the context + std::unique_ptr> context; + //Input, output and reference tensors + std::vector inputTensor; + std::vector outputTensor; + std::vector referenceTensor; + + //Cuda Stream + CudaStream stream; + + //Bindings + std::array bindings{}; + +public: + InferenceNew() = default; + + InferenceNew(std::string_view onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string_view classesTxtFile = ""); + + std::vector runInference(cv::Mat const& input); + +private: + //Creates a ptr to the engine + void createCudaEngine(std::string_view onnxModelPath, int batchSize); + + void launchInference(IExecutionContext* context, cudaStream_t stream, std::vector const& inputTensor, std::vector& outputTensor, void** bindings, int batchSize); +} #endif // INFERENCE_H diff --git a/src/perception/object_detector/ioHelper.cpp b/src/perception/object_detector/ioHelper.cpp new file mode 100644 index 000000000..af3ae0a2a --- /dev/null +++ b/src/perception/object_detector/ioHelper.cpp @@ -0,0 +1,91 @@ +#include "ioHelper.h" +#include +#include +#include +#include +#include +#include +using namespace std; + +namespace nvinfer1 { + + string getBasename(string const& path) { +#ifdef _WIN32 + constexpr char SEPARATOR = '\\'; +#else + constexpr char SEPARATOR = '/'; +#endif + int baseId = path.rfind(SEPARATOR) + 1; + return path.substr(baseId, path.rfind('.') - baseId); + } + + ostream& operator<<(ostream& o, const nvinfer1::ILogger::Severity severity) { + switch (severity) { + case ILogger::Severity::kINTERNAL_ERROR: + o << "INTERNAL_ERROR"; + break; + case ILogger::Severity::kERROR: + o << "ERROR"; + break; + case ILogger::Severity::kWARNING: + o << "WARNING"; + break; + case ILogger::Severity::kINFO: + o << "INFO"; + break; + } + return o; + } + + // returns number of floats successfully read from tensor protobuf + size_t readTensorProto(string const& path, float* buffer) { + string const data{readBuffer(path)}; + onnx::TensorProto tensorProto; + if (!tensorProto.ParseFromString(data)) + return 0; + + assert(tensorProto.has_raw_data()); + assert(tensorProto.raw_data().size() % sizeof(float) == 0); + + memcpy(buffer, tensorProto.raw_data().data(), tensorProto.raw_data().size()); + return tensorProto.raw_data().size() / sizeof(float); + } + + // returns number of floats successfully read from tensorProtoPaths + size_t readTensor(vector const& tensorProtoPaths, vector& buffer) { + GOOGLE_PROTOBUF_VERIFY_VERSION; + size_t totalElements = 0; + + for (size_t i = 0; i < tensorProtoPaths.size(); ++i) { + size_t elements = readTensorProto(tensorProtoPaths[i], &buffer[totalElements]); + if (!elements) { + cout << "ERROR: could not read tensor from file " << tensorProtoPaths[i] << endl; + break; + } + totalElements += elements; + } + + return totalElements; + } + + void writeBuffer(void* buffer, size_t size, string const& path) { + ofstream stream(path.c_str(), ios::binary); + + if (stream) + stream.write(static_cast(buffer), size); + } + + // Returns empty string iff can't read the file + string readBuffer(string const& path) { + string buffer; + ifstream stream(path.c_str(), ios::binary); + + if (stream) { + stream >> noskipws; + copy(istream_iterator(stream), istream_iterator(), back_inserter(buffer)); + } + + return buffer; + } + +} // namespace nvinfer1 \ No newline at end of file diff --git a/src/perception/object_detector/ioHelper.h b/src/perception/object_detector/ioHelper.h new file mode 100644 index 000000000..eb7945e79 --- /dev/null +++ b/src/perception/object_detector/ioHelper.h @@ -0,0 +1,36 @@ +#ifndef __IO_HELPER_H__ +#define __IO_HELPER_H__ + +#include +#include +#include + +namespace nvinfer1 { + + std::ostream& operator<<(std::ostream& o, const ILogger::Severity severity); + + class Logger : public nvinfer1::ILogger { + public: + virtual void log(Severity severity, const char* msg) override { + std::cerr << severity << ": " << msg << std::endl; + } + }; + + template + struct Destroy { + void operator()(T* t) const { + t->destroy(); + } + }; + + std::string getBasename(std::string const& path); + + size_t readTensor(std::vector const& tensorProtoPaths, std::vector& buffer); + + void writeBuffer(void* buffer, size_t size, std::string const& path); + + std::string readBuffer(std::string const& path); + +} // namespace nvinfer1 + +#endif /*__IO_HELPER_H__*/ \ No newline at end of file diff --git a/src/perception/object_detector/newinference.cpp b/src/perception/object_detector/newinference.cpp new file mode 100644 index 000000000..3d4fc4b46 --- /dev/null +++ b/src/perception/object_detector/newinference.cpp @@ -0,0 +1,36 @@ +#include "inference.h" + + +using namespace nvinfer1; + +InferenceNew::InferenceNew(std::string_view onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string_view classesTxtFile = "") { + createCudaEngine(onnxModelPath, BATCH_SIZE); +} + +void InferenceNew::createCudaEngine(std::string_view onnxModelPath, int batchSize) { + const auto explicitBatch = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + std::unique_ptr> builder{nvinfer1::createInferBuilder(gLogger)}; + std::unique_ptr> network{builder->createNetworkV2(explicitBatch)}; + std::unique_ptr> parser{nvonnxparser::createParser(*network, gLogger)}; + std::unique_ptr> config{builder->createBuilderConfig()}; + + if (!parser->parseFromFile(onnxModelPath.c_str(), static_cast(ILogger::Severity::kINFO))) { + cout << "ERROR: could not parse input engine." << endl; + return nullptr; + } + + builder->setMaxBatchSize(batchSize); + config->setMaxWorkspaceSize((1 << 30)); + + auto profile = builder->createOptimizationProfile(); + profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMIN, Dims4{1, 3, 256, 256}); + profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kOPT, Dims4{1, 3, 256, 256}); + profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMAX, Dims4{32, 3, 256, 256}); + config->addOptimizationProfile(profile); + + InferenceNew::enginePtr = builder->buildEngineWithConfig(*network, *config); +} + +static int InferenceNew::getBindingInputIndex(nvinfer1::IExecutionContext* context) { + return !context->getEngine().bindingIsInput(0); // 0 (false) if bindingIsInput(0), 1 (true) otherwise +} \ No newline at end of file diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index dd1ac7708..7d58ff768 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -16,7 +16,7 @@ namespace mrover { //Inference - inference = Inference("//home//marshallvielmetti//Downloads//yolov8s.onnx", cv::Size(640, 640), "", true); + inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), "", true); //read ONNX file into this mNet, YOLOV8, second smallest one // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases // TODO(percep/obj-detectr): make this configurable diff --git a/src/perception/object_detector/object_detector.cu b/src/perception/object_detector/object_detector.cu index e6ddcd1df..085972c63 100644 --- a/src/perception/object_detector/object_detector.cu +++ b/src/perception/object_detector/object_detector.cu @@ -2,15 +2,42 @@ #include #include -struct Logger { +#include "ioHelper.h" - void log(nvinfer1::ILogger::Severity severity, nvinfer1::AsciiChar const* msg) { - } -}; +#include +#include -Logger logger; +#include "inference.h" -void setup() { - // TOOD: fix this - nvinfer1::createInferBuilder(logger); -} \ No newline at end of file +#include + +using namespace nvinfer1; + + +// struct Logger { + +// void log(nvinfer1::ILogger::Severity severity, nvinfer1::AsciiChar const* msg) { +// } +// }; + + +// static Logger logger; + +// __global__ void forward() { +// } + + +// void setup() { +// // TOOD: fix this +// nvinfer1::createInferBuilder(logger); +// } + +void InferenceNew::launchInference(IExecutionContext* context, cudaStream_t stream, std::vector const& inputTensor, std::vector& outputTensor, void** bindings, int batchSize) { + int inputId = InferenceNew::getBindingInputIndex(context); + + cudaMemcpyAsync(bindings[inputId], inputTensor.data(), inputTensor.size() * sizeof(float), cudaMemcpyHostToDevice, stream); + + context->enqueueV3(stream); + + cudaMemcpyAsync(outputTensor.data(), bindings[1 - inputId], outputTensor.size() * sizeof(float), cudaMemcpyDeviceToHost, stream); +} diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index c7b96ee27..84668906a 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -19,6 +19,7 @@ namespace mrover { + void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { assert(msg); assert(msg->height > 0); From f7ef94fba059beec52d0882e6ed18a702303107a Mon Sep 17 00:00:00 2001 From: jbrhm Date: Mon, 30 Oct 2023 09:00:38 -0400 Subject: [PATCH 047/197] Working on Inference Class --- src/perception/object_detector/inference.h | 5 +++++ .../object_detector/newinference.cpp | 21 ++++++++++++++++++- 2 files changed, 25 insertions(+), 1 deletion(-) diff --git a/src/perception/object_detector/inference.h b/src/perception/object_detector/inference.h index 752d68869..756ea46e7 100644 --- a/src/perception/object_detector/inference.h +++ b/src/perception/object_detector/inference.h @@ -87,6 +87,9 @@ class InferenceNew { //Bindings std::array bindings{}; + //ONNX Model Path + std::string onnxModelPath; + public: InferenceNew() = default; @@ -99,5 +102,7 @@ class InferenceNew { void createCudaEngine(std::string_view onnxModelPath, int batchSize); void launchInference(IExecutionContext* context, cudaStream_t stream, std::vector const& inputTensor, std::vector& outputTensor, void** bindings, int batchSize); + + static int getBindingInputIndex(nvinfer1::IExecutionContext* context); } #endif // INFERENCE_H diff --git a/src/perception/object_detector/newinference.cpp b/src/perception/object_detector/newinference.cpp index 3d4fc4b46..cbd4744b7 100644 --- a/src/perception/object_detector/newinference.cpp +++ b/src/perception/object_detector/newinference.cpp @@ -2,9 +2,29 @@ using namespace nvinfer1; +using namespace std; InferenceNew::InferenceNew(std::string_view onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string_view classesTxtFile = "") { createCudaEngine(onnxModelPath, BATCH_SIZE); + + + assert(InferenceNew::enginePtr->getNbBindings() == 2); + assert(InferenceNew::enginePtr->bindingIsInput(0) ^ enginePtr->bindingIsInput(1)); + + InferenceNew::onnxModelPath = onnxModelPath; + + for (int i = 0; i < enginePtr->getNbBindings(); ++i) { + Dims dims{enginePtr->getBindingDimensions(i)}; + size_t size = accumulate(dims.d + 1, dims.d + dims.nbDims, BATCH_SIZE, multiplies()); + // Create CUDA buffer for Tensor. + cudaMalloc(&bindings[i], BATCH_SIZE * size * sizeof(float)); + + // Resize CPU buffers to fit Tensor. + if (enginePtr->bindingIsInput(i)) { + inputTensor.resize(size); + } else + outputTensor.resize(size); + } } void InferenceNew::createCudaEngine(std::string_view onnxModelPath, int batchSize) { @@ -16,7 +36,6 @@ void InferenceNew::createCudaEngine(std::string_view onnxModelPath, int batchSiz if (!parser->parseFromFile(onnxModelPath.c_str(), static_cast(ILogger::Severity::kINFO))) { cout << "ERROR: could not parse input engine." << endl; - return nullptr; } builder->setMaxBatchSize(batchSize); From 3d5e10be228964ddd9b45a8862d2be7c6340d8be Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Mon, 30 Oct 2023 12:07:55 -0400 Subject: [PATCH 048/197] working greyscale stream with ros --- package.xml | 2 +- ...e_cam_plugin.xml => usb_camera_plugin.xml} | 2 +- .../long_range_cam/long_range_cam.cpp | 75 ++++++++++++++----- .../long_range_cam/long_range_cam.hpp | 7 +- 4 files changed, 64 insertions(+), 22 deletions(-) rename plugins/{long_range_cam_plugin.xml => usb_camera_plugin.xml} (76%) diff --git a/package.xml b/package.xml index 8b6a46a9c..3c865b925 100644 --- a/package.xml +++ b/package.xml @@ -90,7 +90,7 @@ - + \ No newline at end of file diff --git a/plugins/long_range_cam_plugin.xml b/plugins/usb_camera_plugin.xml similarity index 76% rename from plugins/long_range_cam_plugin.xml rename to plugins/usb_camera_plugin.xml index e717701de..98d1f99e2 100644 --- a/plugins/long_range_cam_plugin.xml +++ b/plugins/usb_camera_plugin.xml @@ -1,4 +1,4 @@ - + diff --git a/src/perception/long_range_cam/long_range_cam.cpp b/src/perception/long_range_cam/long_range_cam.cpp index c05cd8e3e..ab8a1fdfa 100644 --- a/src/perception/long_range_cam/long_range_cam.cpp +++ b/src/perception/long_range_cam/long_range_cam.cpp @@ -1,45 +1,83 @@ #include "long_range_cam.hpp" +#include +#include +#include +#include +#include +#include namespace mrover { using namespace std::chrono_literals; + // gst-launch-1.0 -e -v v4l2src device=/dev/video4 ! videoconvert ! videoscale ! videorate ! "video/x-raw,format=I420,width=640,height=480,framerate=30/1" ! x264enc key-int-max=15 ! rtph264pay ! udpsink host=localhost port=5000 // gst-launch-1.0 -v udpsrc uri=udp://localhost:5000 ! application/x-rtp,media=video,clock-rate=90000,encoding-name=H264,payload=96 ! rtph264depay ! avdec_h264 ! autovideosink /** - * @brief Load config, open the camera, and start our threads - */ + * @brief Load config, open the camera, and start our threads + */ void LongRangeCamNodelet::onInit() { try { - // See: http://wiki.ros.org/roscpp/Overview/NodeHandles for public vs. private node handle - // MT means multithreaded - mNh = getMTNodeHandle(); - mPnh = getMTPrivateNodeHandle(); - mCamInfoPub = mNh.advertise("long_range_cam/camera_info", 1); - mImgPub = mNh.advertise("long_range_cam/image", 1); + mGrabThread = std::jthread(&LongRangeCamNodelet::grabUpdate, this); + } catch (std::exception const& e) { + NODELET_FATAL("Exception while starting: %s", e.what()); + ros::shutdown(); + } + } + + void fillImageMessage(cv::Mat& grey, sensor_msgs::ImagePtr const& msg) { + // TODO: Uncomment this + // assert(img.channels() == 4); + assert(msg); - // TODO: Fix hardcoded components of this string - mCapture = cv::VideoCapture("v4l2src device=/dev/video4 ! videoconvert ! videoscale ! videorate ! 'video / x - raw, format = I420, width = 640, height = 480, framerate = 30 / 1' ! x264enc key-int-max=15 ! rtph264pay ! udpsink"); - cv::VideoWriter out = cv::VideoWriter("udpsrc ! application/x-rtp,media=video,clock-rate=90000,encoding-name=H264,payload=96 ! rtph264depay ! avdec_h264 ! autovideosink host=localhost port =5000", cv::CAP_GSTREAMER, 0, 30, cv::Size(640, 480), true); + msg->height = grey.rows; + msg->width = grey.cols; + msg->encoding = sensor_msgs::image_encodings::MONO8; + msg->step = grey.step[0]; + msg->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; + auto* imgPtr = grey.ptr(0); + size_t size = msg->step * msg->height; + msg->data.resize(size); + std::cout << "data size " << msg->data.size() << std::endl; + std::cout << "size obj " << size << std::endl; + memcpy(msg->data.data(), imgPtr, size); + } + + void LongRangeCamNodelet::grabUpdate() { + // See: http://wiki.ros.org/roscpp/Overview/NodeHandles for public vs. private node handle + // MT means multithreaded + mNh = getMTNodeHandle(); + mPnh = getMTPrivateNodeHandle(); + mCamInfoPub = mNh.advertise("long_range_cam/camera_info", 1); + mImgPub = mNh.advertise("long_range_cam/image", 1); + // While dtor not called + while (ros::ok()) { + cv::VideoCapture mCapture{std::format("v4l2src device=/dev/video4 ! videoconvert ! video/x-raw,width={},height={},format=I420,framerate={}/1 ! appsink", 1920, 1080, 5), cv::CAP_GSTREAMER}; if (!mCapture.isOpened()) { throw std::runtime_error("Long range cam failed to open"); } - ROS_ERROR("NODELET LOADED"); cv::Mat frame; while (true) { mCapture.read(frame); if (frame.empty()) { break; } - out.write(frame); cv::imshow("Sender", frame); + if (mImgPub.getNumSubscribers()) { + auto imgMsg = boost::make_shared(); + cv::Mat grey; + cv::cvtColor(frame, grey, cv::COLOR_YUV2GRAY_I420); + fillImageMessage(grey, imgMsg); + imgMsg->header.frame_id = "long_range_cam_frame"; + imgMsg->header.stamp = ros::Time::now(); + imgMsg->header.seq = mGrabUpdateTick; + mImgPub.publish(imgMsg); + } if (cv::waitKey(1) == 's') { break; } + ++mGrabUpdateTick; } - } catch (std::exception const& e) { - NODELET_FATAL("Exception while starting: %s", e.what()); - ros::shutdown(); } } @@ -50,10 +88,11 @@ namespace mrover { } // namespace mrover int main(int argc, char** argv) { - ros::init(argc, argv, "long_range_cam"); + ros::init(argc, argv, "usb_camera"); // Start the ZED Nodelet nodelet::Loader nodelet; + std::cout << ros::this_node::getName() << std::endl; nodelet.load(ros::this_node::getName(), "mrover/LongRangeCamNodelet", ros::names::getRemappings(), {}); ros::spin(); @@ -61,5 +100,7 @@ int main(int argc, char** argv) { return EXIT_SUCCESS; } +#ifdef MROVER_IS_NODELET #include PLUGINLIB_EXPORT_CLASS(mrover::LongRangeCamNodelet, nodelet::Nodelet) +#endif diff --git a/src/perception/long_range_cam/long_range_cam.hpp b/src/perception/long_range_cam/long_range_cam.hpp index c8ca73ee8..2de870686 100644 --- a/src/perception/long_range_cam/long_range_cam.hpp +++ b/src/perception/long_range_cam/long_range_cam.hpp @@ -14,7 +14,7 @@ namespace mrover { ros::Publisher mCamInfoPub; ros::Publisher mImgPub; - std::thread mReadThread; + std::jthread mGrabThread; std::mutex mSwapMutex; boost::condition_variable mSwapCv; std::atomic_bool mIsSwapReady = false; @@ -30,8 +30,9 @@ namespace mrover { ~LongRangeCamNodelet() override; - void readUpdate(); - void imagePubUpdate(); + void grabUpdate(); }; + void fillImageMessage(cv::Mat& grey, sensor_msgs::ImagePtr const& msg); + } // namespace mrover From 22036a99835284cc4e089ad176af01a30313dea3 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Mon, 30 Oct 2023 16:27:55 -0400 Subject: [PATCH 049/197] switch image type from grey -> bgra --- .../long_range_cam/long_range_cam.cpp | 18 +++++++++--------- .../long_range_cam/long_range_cam.hpp | 2 +- .../long_range_cam/long_range_tag_detector.cpp | 2 ++ .../long_range_cam/long_range_tag_detector.hpp | 10 ++++++---- .../long_range_tag_detector.processing.cpp | 16 ++++++++++------ 5 files changed, 28 insertions(+), 20 deletions(-) diff --git a/src/perception/long_range_cam/long_range_cam.cpp b/src/perception/long_range_cam/long_range_cam.cpp index ab8a1fdfa..8720f9657 100644 --- a/src/perception/long_range_cam/long_range_cam.cpp +++ b/src/perception/long_range_cam/long_range_cam.cpp @@ -25,17 +25,17 @@ namespace mrover { } } - void fillImageMessage(cv::Mat& grey, sensor_msgs::ImagePtr const& msg) { + void fillImageMessage(cv::Mat& bgra, sensor_msgs::ImagePtr const& msg) { // TODO: Uncomment this // assert(img.channels() == 4); assert(msg); - msg->height = grey.rows; - msg->width = grey.cols; - msg->encoding = sensor_msgs::image_encodings::MONO8; - msg->step = grey.step[0]; + msg->height = bgra.rows; + msg->width = bgra.cols; + msg->encoding = sensor_msgs::image_encodings::BGRA8; + msg->step = bgra.step[0]; msg->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; - auto* imgPtr = grey.ptr(0); + auto* imgPtr = bgra.ptr(0); size_t size = msg->step * msg->height; msg->data.resize(size); std::cout << "data size " << msg->data.size() << std::endl; @@ -65,9 +65,9 @@ namespace mrover { cv::imshow("Sender", frame); if (mImgPub.getNumSubscribers()) { auto imgMsg = boost::make_shared(); - cv::Mat grey; - cv::cvtColor(frame, grey, cv::COLOR_YUV2GRAY_I420); - fillImageMessage(grey, imgMsg); + cv::Mat bgra; + cv::cvtColor(frame, bgra, cv::COLOR_YUV2BGRA_I420); + fillImageMessage(bgra, imgMsg); imgMsg->header.frame_id = "long_range_cam_frame"; imgMsg->header.stamp = ros::Time::now(); imgMsg->header.seq = mGrabUpdateTick; diff --git a/src/perception/long_range_cam/long_range_cam.hpp b/src/perception/long_range_cam/long_range_cam.hpp index 2de870686..8c8141534 100644 --- a/src/perception/long_range_cam/long_range_cam.hpp +++ b/src/perception/long_range_cam/long_range_cam.hpp @@ -33,6 +33,6 @@ namespace mrover { void grabUpdate(); }; - void fillImageMessage(cv::Mat& grey, sensor_msgs::ImagePtr const& msg); + void fillImageMessage(cv::Mat& bgra, sensor_msgs::ImagePtr const& msg); } // namespace mrover diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp index b31eb97fe..4b34c8faa 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp @@ -16,6 +16,8 @@ namespace mrover { mPnh.param("tag_decrement_weight", mTagDecrementWeight, 1); mImgPub = mNh.advertise("long_range_tag_detection", 1); + + mImgSub = mNh.subscribe("long_range_cam/image", 1, &LongRangeTagDetectorNodelet::imageCallback, this); mServiceEnableDetections = mNh.advertiseService("enable_detections", &LongRangeTagDetectorNodelet::enableDetectionsCallback, this); // Lambda handles passing class pointer (implicit first parameter) to configCallback diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp index c7972500d..8e9160494 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp @@ -1,4 +1,5 @@ #include "pch.hpp" +#include namespace mrover { @@ -21,7 +22,8 @@ namespace mrover { //Publishing Flags bool mEnableDetections = true; - bool mPublishImages{}; // If set, we publish the images with the tags drawn on top + // TODO: change this to param + bool mPublishImages = true; // If set, we publish the images with the tags drawn on top //Constatns set in ROS params for num hits needed to publish int mMinHitCountBeforePublish{}; @@ -53,7 +55,7 @@ namespace mrover { std::optional mPrevDetectedCount; // Log spam prevention dynamic_reconfigure::Server mConfigServer; dynamic_reconfigure::Server::CallbackType mCallbackType; - LoopProfiler mProfiler{"Long RangeTag Detector"}; + LoopProfiler mProfiler{"Long Range Tag Detector"}; std::unordered_map mThreshPubs; // Map from threshold scale to publisher ros::ServiceServer mServiceEnableDetections; bool mUseOdom{}; @@ -66,7 +68,7 @@ namespace mrover { * 1. Updates mImg to store the underlying image matrix * @param msg image message */ - void imageCallback(sensor_msgs::Image const& msg); + void imageCallback(sensor_msgs::ImageConstPtr const& msg); /** * Stores msg to mImgMsg @@ -74,7 +76,7 @@ namespace mrover { * * @param msg Current Image Message */ - void updateImageMatrices(sensor_msgs::Image const& msg); + void updateImageMatrices(sensor_msgs::ImageConstPtr const& msg); /** * Runs the cv::aruco tag detection on the cv::mat diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp index 5a6e1fb4b..582d5761a 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp @@ -1,4 +1,6 @@ #include "long_range_tag_detector.hpp" +#include +#include namespace mrover { @@ -7,7 +9,7 @@ namespace mrover { * * @param msg image message */ - void LongRangeTagDetectorNodelet::imageCallback(sensor_msgs::Image const& msg) { + void LongRangeTagDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { if (!mEnableDetections) return; //Store image contents to member variables @@ -33,14 +35,16 @@ namespace mrover { //HELPER FUNCTIONS - void LongRangeTagDetectorNodelet::updateImageMatrices(sensor_msgs::Image const& msg) { + void LongRangeTagDetectorNodelet::updateImageMatrices(sensor_msgs::ImageConstPtr const& msg) { + assert(msg); + assert(msg->height > 0); + assert(msg->width > 0); //Store image message to mImgMsg member variable - mImgMsg = msg; - - cv::Mat cvImage{static_cast(msg.height), static_cast(msg.width), CV_8UC3, const_cast(msg.data.data())}; + mImgMsg = *msg; + mImg = cv::Mat{static_cast(msg->height), static_cast(msg->width), CV_8UC4, const_cast(msg->data.data())}; //Store to mImg member variable - unsure if necessary but need it to persist - cvImage.copyTo(mImg); + // cvImage.copyTo(mImg); // TODO: Set the grayImage if neccesary } From 5789ac0f5de0416e58adc86731a403176c23e705 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Tue, 31 Oct 2023 16:41:19 -0400 Subject: [PATCH 050/197] Constructor blocked: Didn't adjust for comments: Unsure about some of the out of line functions? --- .../{newinference.cpp => InferenceNew.cpp} | 14 +------------- src/perception/object_detector/object_detector.cu | 15 +++++++++++++++ 2 files changed, 16 insertions(+), 13 deletions(-) rename src/perception/object_detector/{newinference.cpp => InferenceNew.cpp} (80%) diff --git a/src/perception/object_detector/newinference.cpp b/src/perception/object_detector/InferenceNew.cpp similarity index 80% rename from src/perception/object_detector/newinference.cpp rename to src/perception/object_detector/InferenceNew.cpp index cbd4744b7..e8b665749 100644 --- a/src/perception/object_detector/newinference.cpp +++ b/src/perception/object_detector/InferenceNew.cpp @@ -7,24 +7,12 @@ using namespace std; InferenceNew::InferenceNew(std::string_view onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string_view classesTxtFile = "") { createCudaEngine(onnxModelPath, BATCH_SIZE); - assert(InferenceNew::enginePtr->getNbBindings() == 2); assert(InferenceNew::enginePtr->bindingIsInput(0) ^ enginePtr->bindingIsInput(1)); InferenceNew::onnxModelPath = onnxModelPath; - for (int i = 0; i < enginePtr->getNbBindings(); ++i) { - Dims dims{enginePtr->getBindingDimensions(i)}; - size_t size = accumulate(dims.d + 1, dims.d + dims.nbDims, BATCH_SIZE, multiplies()); - // Create CUDA buffer for Tensor. - cudaMalloc(&bindings[i], BATCH_SIZE * size * sizeof(float)); - - // Resize CPU buffers to fit Tensor. - if (enginePtr->bindingIsInput(i)) { - inputTensor.resize(size); - } else - outputTensor.resize(size); - } + InferenceNew::prepTensors(); } void InferenceNew::createCudaEngine(std::string_view onnxModelPath, int batchSize) { diff --git a/src/perception/object_detector/object_detector.cu b/src/perception/object_detector/object_detector.cu index 085972c63..56a42ed90 100644 --- a/src/perception/object_detector/object_detector.cu +++ b/src/perception/object_detector/object_detector.cu @@ -41,3 +41,18 @@ void InferenceNew::launchInference(IExecutionContext* context, cudaStream_t stre cudaMemcpyAsync(outputTensor.data(), bindings[1 - inputId], outputTensor.size() * sizeof(float), cudaMemcpyDeviceToHost, stream); } + +void InferenceNew::prepTensors() { + for (int i = 0; i < InferenceNew::enginePtr->getNbBindings(); ++i) { + Dims dims{InferenceNew::enginePtr->getBindingDimensions(i)}; + size_t size = accumulate(dims.d + 1, dims.d + dims.nbDims, InferenceNew::BATCH_SIZE, std::multiplies()); + // Create CUDA buffer for Tensor. + cudaMalloc(&InferenceNew::bindings[i], InferenceNew::BATCH_SIZE * size * sizeof(float)); + + // Resize CPU buffers to fit Tensor. + if (InferenceNew::enginePtr->bindingIsInput(i)) { + InferenceNew::inputTensor.resize(size); + } else + InferenceNew::outputTensor.resize(size); + } +} From be76f23b208d9d12e659b5ea438e4a83b9f9c036 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Tue, 31 Oct 2023 17:06:28 -0400 Subject: [PATCH 051/197] I'm confused --- src/perception/object_detector/InferenceNew.cpp | 17 +++++++++++++++-- src/perception/object_detector/inference.h | 2 ++ .../object_detector/object_detector.cu | 14 +++++++------- 3 files changed, 24 insertions(+), 9 deletions(-) diff --git a/src/perception/object_detector/InferenceNew.cpp b/src/perception/object_detector/InferenceNew.cpp index e8b665749..1101c558a 100644 --- a/src/perception/object_detector/InferenceNew.cpp +++ b/src/perception/object_detector/InferenceNew.cpp @@ -1,4 +1,5 @@ #include "inference.h" +#include using namespace nvinfer1; @@ -11,8 +12,6 @@ InferenceNew::InferenceNew(std::string_view onnxModelPath, cv::Size modelInputSh assert(InferenceNew::enginePtr->bindingIsInput(0) ^ enginePtr->bindingIsInput(1)); InferenceNew::onnxModelPath = onnxModelPath; - - InferenceNew::prepTensors(); } void InferenceNew::createCudaEngine(std::string_view onnxModelPath, int batchSize) { @@ -40,4 +39,18 @@ void InferenceNew::createCudaEngine(std::string_view onnxModelPath, int batchSiz static int InferenceNew::getBindingInputIndex(nvinfer1::IExecutionContext* context) { return !context->getEngine().bindingIsInput(0); // 0 (false) if bindingIsInput(0), 1 (true) otherwise +} + +void InferenceNew::setUpContext(std::string const inputFile, ) { + // Read input tensor from input picture file. + if (readTensor(inputFile, InferenceNew::inputTensor) != InferenceNew::inputTensor.size()) { + cout << "Couldn't read input Tensor" << endl; + } + + // Create Execution Context. + InferenceNew::context.reset(InferenceNew::enginePtr->createExecutionContext()); + + Dims dims_i{InferenceNew::enginePtr->getBindingDimensions(0)}; + Dims4 inputDims{InferenceNew::BATCH_SIZE, dims_i.d[1], dims_i.d[2], dims_i.d[3]}; + context->setBindingDimensions(0, inputDims); } \ No newline at end of file diff --git a/src/perception/object_detector/inference.h b/src/perception/object_detector/inference.h index 756ea46e7..c46749a1d 100644 --- a/src/perception/object_detector/inference.h +++ b/src/perception/object_detector/inference.h @@ -104,5 +104,7 @@ class InferenceNew { void launchInference(IExecutionContext* context, cudaStream_t stream, std::vector const& inputTensor, std::vector& outputTensor, void** bindings, int batchSize); static int getBindingInputIndex(nvinfer1::IExecutionContext* context); + + void InferenceNew::setUpContext(std::string const inputFile); //This will need to be adjusted to the img msg, but I think its ok to just run pictures as an intial test } #endif // INFERENCE_H diff --git a/src/perception/object_detector/object_detector.cu b/src/perception/object_detector/object_detector.cu index 56a42ed90..389c8b66e 100644 --- a/src/perception/object_detector/object_detector.cu +++ b/src/perception/object_detector/object_detector.cu @@ -42,17 +42,17 @@ void InferenceNew::launchInference(IExecutionContext* context, cudaStream_t stre cudaMemcpyAsync(outputTensor.data(), bindings[1 - inputId], outputTensor.size() * sizeof(float), cudaMemcpyDeviceToHost, stream); } -void InferenceNew::prepTensors() { - for (int i = 0; i < InferenceNew::enginePtr->getNbBindings(); ++i) { - Dims dims{InferenceNew::enginePtr->getBindingDimensions(i)}; +void InferenceNew::prepTensors(std::unique_ptr> enginePtr, std::array bindings, std::vector& inputTensor, std::vector& outputTensor) { + for (int i = 0; i < enginePtr->getNbBindings(); ++i) { + Dims dims{enginePtr->getBindingDimensions(i)}; size_t size = accumulate(dims.d + 1, dims.d + dims.nbDims, InferenceNew::BATCH_SIZE, std::multiplies()); // Create CUDA buffer for Tensor. - cudaMalloc(&InferenceNew::bindings[i], InferenceNew::BATCH_SIZE * size * sizeof(float)); + cudaMalloc(&bindings[i], InferenceNew::BATCH_SIZE * size * sizeof(float)); // Resize CPU buffers to fit Tensor. - if (InferenceNew::enginePtr->bindingIsInput(i)) { - InferenceNew::inputTensor.resize(size); + if (enginePtr->bindingIsInput(i)) { + inputTensor.resize(size); } else - InferenceNew::outputTensor.resize(size); + outputTensor.resize(size); } } From e73108846f70406ca30c84cf957af38c5a10a7c8 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Tue, 31 Oct 2023 20:03:45 -0400 Subject: [PATCH 052/197] DEATH TO THE RED SQUIGGLES: Forward Pass done. Need to parse output --- src/perception/object_detector/Inference.cpp | 17 ++ .../object_detector/InferenceNew.cpp | 56 ------ src/perception/object_detector/inference.cpp | 174 ------------------ src/perception/object_detector/inference.h | 47 ++--- src/perception/object_detector/ioHelper.h | 4 +- .../object_detector/object_detector.cu | 114 +++++++++--- 6 files changed, 117 insertions(+), 295 deletions(-) create mode 100644 src/perception/object_detector/Inference.cpp delete mode 100644 src/perception/object_detector/InferenceNew.cpp delete mode 100644 src/perception/object_detector/inference.cpp diff --git a/src/perception/object_detector/Inference.cpp b/src/perception/object_detector/Inference.cpp new file mode 100644 index 000000000..e9db30d75 --- /dev/null +++ b/src/perception/object_detector/Inference.cpp @@ -0,0 +1,17 @@ +#include "inference.h" +#include + + +using namespace nvinfer1; +using namespace std; + +// : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} +Inference::Inference(std::string_view onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string_view classesTxtFile = "") : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} { + + createCudaEngine(onnxModelPath, Inference::BATCH_SIZE); + + assert(this->enginePtr->getNbBindings() == 2); + assert(this->enginePtr->bindingIsInput(0) ^ enginePtr->bindingIsInput(1)); + + this->onnxModelPath = onnxModelPath; +} \ No newline at end of file diff --git a/src/perception/object_detector/InferenceNew.cpp b/src/perception/object_detector/InferenceNew.cpp deleted file mode 100644 index 1101c558a..000000000 --- a/src/perception/object_detector/InferenceNew.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#include "inference.h" -#include - - -using namespace nvinfer1; -using namespace std; - -InferenceNew::InferenceNew(std::string_view onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string_view classesTxtFile = "") { - createCudaEngine(onnxModelPath, BATCH_SIZE); - - assert(InferenceNew::enginePtr->getNbBindings() == 2); - assert(InferenceNew::enginePtr->bindingIsInput(0) ^ enginePtr->bindingIsInput(1)); - - InferenceNew::onnxModelPath = onnxModelPath; -} - -void InferenceNew::createCudaEngine(std::string_view onnxModelPath, int batchSize) { - const auto explicitBatch = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); - std::unique_ptr> builder{nvinfer1::createInferBuilder(gLogger)}; - std::unique_ptr> network{builder->createNetworkV2(explicitBatch)}; - std::unique_ptr> parser{nvonnxparser::createParser(*network, gLogger)}; - std::unique_ptr> config{builder->createBuilderConfig()}; - - if (!parser->parseFromFile(onnxModelPath.c_str(), static_cast(ILogger::Severity::kINFO))) { - cout << "ERROR: could not parse input engine." << endl; - } - - builder->setMaxBatchSize(batchSize); - config->setMaxWorkspaceSize((1 << 30)); - - auto profile = builder->createOptimizationProfile(); - profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMIN, Dims4{1, 3, 256, 256}); - profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kOPT, Dims4{1, 3, 256, 256}); - profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMAX, Dims4{32, 3, 256, 256}); - config->addOptimizationProfile(profile); - - InferenceNew::enginePtr = builder->buildEngineWithConfig(*network, *config); -} - -static int InferenceNew::getBindingInputIndex(nvinfer1::IExecutionContext* context) { - return !context->getEngine().bindingIsInput(0); // 0 (false) if bindingIsInput(0), 1 (true) otherwise -} - -void InferenceNew::setUpContext(std::string const inputFile, ) { - // Read input tensor from input picture file. - if (readTensor(inputFile, InferenceNew::inputTensor) != InferenceNew::inputTensor.size()) { - cout << "Couldn't read input Tensor" << endl; - } - - // Create Execution Context. - InferenceNew::context.reset(InferenceNew::enginePtr->createExecutionContext()); - - Dims dims_i{InferenceNew::enginePtr->getBindingDimensions(0)}; - Dims4 inputDims{InferenceNew::BATCH_SIZE, dims_i.d[1], dims_i.d[2], dims_i.d[3]}; - context->setBindingDimensions(0, inputDims); -} \ No newline at end of file diff --git a/src/perception/object_detector/inference.cpp b/src/perception/object_detector/inference.cpp deleted file mode 100644 index 0cb6cf84e..000000000 --- a/src/perception/object_detector/inference.cpp +++ /dev/null @@ -1,174 +0,0 @@ -#include "inference.h" - -Inference::Inference(const std::string& onnxModelPath, const cv::Size& modelInputShape, const std::string& classesTxtFile, const bool& runWithCuda) { - modelPath = onnxModelPath; - modelShape = modelInputShape; - classesPath = classesTxtFile; - cudaEnabled = runWithCuda; - - loadOnnxNetwork(); -} - -std::vector Inference::runInference(const cv::Mat& input) { - - cv::Mat modelInput = input; - if (letterBoxForSquare && modelShape.width == modelShape.height) - modelInput = formatToSquare(modelInput); - - cv::Mat blob; - cv::dnn::blobFromImage(modelInput, blob, 1.0 / 255.0, modelShape, cv::Scalar(), true, false); - net.setInput(blob); - - std::vector> outputs; - net.forward(outputs, net.getUnconnectedOutLayersNames()); - - std::cout << outputs[0].data << std::endl; - - int rows = outputs[0].size[1]; - int dimensions = outputs[0].size[2]; - - bool yolov8 = false; - // yolov5 has an output of shape (batchSize, 25200, 85) (Num classes + box[x,y,w,h] + confidence[c]) - // yolov8 has an output of shape (batchSize, 84, 8400) (Num classes + box[x,y,w,h]) - if (dimensions > rows) // Check if the shape[2] is more than shape[1] (yolov8) - { - yolov8 = true; - rows = outputs[0].size[2]; - dimensions = outputs[0].size[1]; - - outputs[0] = outputs[0].reshape(1, dimensions); - cv::transpose(outputs[0], outputs[0]); - } - - float* data = (float*) outputs[0].data; - - - float x_factor = modelInput.cols / modelShape.width; - float y_factor = modelInput.rows / modelShape.height; - - std::vector class_ids; - std::vector confidences; - std::vector boxes; - - for (int i = 0; i < rows; ++i) { - if (yolov8) { - float* classes_scores = data + 4; - - cv::Mat scores(1, classes.size(), CV_32FC1, classes_scores); - cv::Point class_id; - double maxClassScore; - - minMaxLoc(scores, 0, &maxClassScore, 0, &class_id); - - if (maxClassScore > modelScoreThreshold) { - confidences.push_back(maxClassScore); - class_ids.push_back(class_id.x); - - float x = data[0]; - float y = data[1]; - float w = data[2]; - float h = data[3]; - - int left = int((x - 0.5 * w) * x_factor); - int top = int((y - 0.5 * h) * y_factor); - - int width = int(w * x_factor); - int height = int(h * y_factor); - - - boxes.emplace_back(left, top, width, height); - } - } else // yolov5 - { - float confidence = data[4]; - - if (confidence >= modelConfidenceThreshold) { - float* classes_scores = data + 5; - - cv::Mat scores(1, classes.size(), CV_32FC1, classes_scores); - cv::Point class_id; - double max_class_score; - - minMaxLoc(scores, 0, &max_class_score, 0, &class_id); - - if (max_class_score > modelScoreThreshold) { - confidences.push_back(confidence); - class_ids.push_back(class_id.x); - - float x = data[0]; - float y = data[1]; - float w = data[2]; - float h = data[3]; - - int left = int((x - 0.5 * w) * x_factor); - int top = int((y - 0.5 * h) * y_factor); - - int width = int(w * x_factor); - int height = int(h * y_factor); - - boxes.push_back(cv::Rect(left, top, width, height)); - } - } - } - - data += dimensions; - } - - std::vector nms_result; - cv::dnn::NMSBoxes(boxes, confidences, modelScoreThreshold, modelNMSThreshold, nms_result); - - std::vector detections{}; - for (unsigned long i = 0; i < nms_result.size(); ++i) { - int idx = nms_result[i]; - - Detection result; - result.class_id = class_ids[idx]; - result.confidence = confidences[idx]; - - std::random_device rd; - std::mt19937 gen(rd()); - std::uniform_int_distribution dis(100, 255); - result.color = cv::Scalar(dis(gen), - dis(gen), - dis(gen)); - - result.className = classes[result.class_id]; - result.box = boxes[idx]; - - detections.push_back(result); - } - - return detections; -} - -void Inference::loadClassesFromFile() { - std::ifstream inputFile(classesPath); - if (inputFile.is_open()) { - std::string classLine; - while (std::getline(inputFile, classLine)) - classes.push_back(classLine); - inputFile.close(); - } -} - -void Inference::loadOnnxNetwork() { - net = cv::dnn::readNetFromONNX(modelPath); - if (cudaEnabled) { - std::cout << "\nRunning on CUDA" << std::endl; - net.setPreferableBackend(cv::dnn::DNN_BACKEND_CUDA); - net.setPreferableTarget(cv::dnn::DNN_TARGET_CUDA); - } else { - std::cout << "\nRunning on CPU" << std::endl; - net.setPreferableBackend(cv::dnn::DNN_BACKEND_OPENCV); - net.setPreferableTarget(cv::dnn::DNN_TARGET_CPU); - } -} - -cv::Mat Inference::formatToSquare(const cv::Mat& source) { - int col = source.cols; - int row = source.rows; - int _max = MAX(col, row); - cv::Mat result = cv::Mat::zeros(_max, _max, CV_8UC3); - source.copyTo(result(cv::Rect(0, 0, col, row))); - return result; -} diff --git a/src/perception/object_detector/inference.h b/src/perception/object_detector/inference.h index c46749a1d..88b44b128 100644 --- a/src/perception/object_detector/inference.h +++ b/src/perception/object_detector/inference.h @@ -38,34 +38,6 @@ struct Detection { }; class Inference { -public: - Inference() = default; - Inference(const std::string& onnxModelPath, const cv::Size& modelInputShape = {640, 640}, const std::string& classesTxtFile = "", const bool& runWithCuda = true); - std::vector runInference(const cv::Mat& input); - -private: - void loadClassesFromFile(); - void loadOnnxNetwork(); - cv::Mat formatToSquare(const cv::Mat& source); - - std::string modelPath; - std::string classesPath; - bool cudaEnabled{}; - - std::vector classes{"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; - - cv::Size2f modelShape{}; - - float modelConfidenceThreshold{0.25}; - float modelScoreThreshold{0.45}; - float modelNMSThreshold{0.50}; - - bool letterBoxForSquare = true; - - cv::dnn::Net net; -}; - -class InferenceNew { private: static const int BATCH_SIZE = 1; @@ -90,21 +62,28 @@ class InferenceNew { //ONNX Model Path std::string onnxModelPath; +private: + //STATIC FUNCTIONS + static int getBindingInputIndex(nvinfer1::IExecutionContext* context); + public: - InferenceNew() = default; + Inference() = default; + + Inference(std::string_view onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string_view classesTxtFile = ""); - InferenceNew(std::string_view onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string_view classesTxtFile = ""); std::vector runInference(cv::Mat const& input); private: //Creates a ptr to the engine - void createCudaEngine(std::string_view onnxModelPath, int batchSize); + void createCudaEngine(std::string& onnxModelPath); - void launchInference(IExecutionContext* context, cudaStream_t stream, std::vector const& inputTensor, std::vector& outputTensor, void** bindings, int batchSize); + void launchInference(); - static int getBindingInputIndex(nvinfer1::IExecutionContext* context); + void setUpContext(const std::string& inputFile); //This will need to be adjusted to the img msg, but I think its ok to just run pictures as an intial test + + void prepTensors(); - void InferenceNew::setUpContext(std::string const inputFile); //This will need to be adjusted to the img msg, but I think its ok to just run pictures as an intial test + void doDetection(); } #endif // INFERENCE_H diff --git a/src/perception/object_detector/ioHelper.h b/src/perception/object_detector/ioHelper.h index eb7945e79..ddacc28ed 100644 --- a/src/perception/object_detector/ioHelper.h +++ b/src/perception/object_detector/ioHelper.h @@ -7,11 +7,11 @@ namespace nvinfer1 { - std::ostream& operator<<(std::ostream& o, const ILogger::Severity severity); + std::ostream& operator<<(std::ostream& o, ILogger::Severity severity); class Logger : public nvinfer1::ILogger { public: - virtual void log(Severity severity, const char* msg) override { + void log(Severity severity, const char* msg) override { std::cerr << severity << ": " << msg << std::endl; } }; diff --git a/src/perception/object_detector/object_detector.cu b/src/perception/object_detector/object_detector.cu index 389c8b66e..111a678b2 100644 --- a/src/perception/object_detector/object_detector.cu +++ b/src/perception/object_detector/object_detector.cu @@ -1,58 +1,114 @@ #include +#include #include #include #include "ioHelper.h" #include -#include +#include +#include #include "inference.h" +#include +#include #include using namespace nvinfer1; +/** +* Example Code: @link https://github.com/NVIDIA-developer-blog/code-samples/blob/master/posts/TensorRT-introduction/simpleOnnx_1.cpp +* IExecutionContest @link https://docs.nvidia.com/deeplearning/tensorrt/api/c_api/classnvinfer1_1_1_i_execution_context.html +* ------------------------------------------------------ +* For additional context see @link https://www.edge-ai-vision.com/2020/04/speeding-up-deep-learning-inference-using-tensorrt/ +*/ -// struct Logger { -// void log(nvinfer1::ILogger::Severity severity, nvinfer1::AsciiChar const* msg) { -// } -// }; +/** +* cudaMemcpys CPU memory in inputTensor to GPU based on bindings +* Queues that tensor to be passed through model +* cudaMemcpys the result back to CPU memory +* Requires bindings, inputTensor, stream +* Modifies stream, outputTensor +*/ +void Inference::launchInference() { + int inputId = Inference::getBindingInputIndex(this->context); + //Copy data to GPU memory + cudaMemcpyAsync(this->bindings[inputId], this->inputTensor.data(), this->inputTensor.size() * sizeof(float), cudaMemcpyHostToDevice, this->stream); -// static Logger logger; + // + this->context->enqueueV3(this->stream); -// __global__ void forward() { -// } + //Copy data to CPU memory + cudaMemcpyAsync(this->outputTensor.data(), this->bindings[1 - inputId], this->outputTensor.size() * sizeof(float), cudaMemcpyDeviceToHost, this->stream); +} + +/** +* Takes tensor bindings and allocates memory on the GPU for input and output tensors +* Requires enginePtr, bindings, inputTensor, and outputTensor +* Modifies bindings, inputTensor, and outputTensor +*/ +void Inference::prepTensors() { + for (int i = 0; i < this->enginePtr->getNbIOTensors(); i++) { + const char* tensorName = this->enginePtr->getIOTensorName(i); + + Dims dims{this->enginePtr->getTensorShape(tensorName)}; + size_t size = accumulate(dims.d + 1, dims.d + dims.nbDims, Inference::BATCH_SIZE, std::multiplies<>()); + // Create CUDA buffer for Tensor. + cudaMalloc(&(this->bindings)[i], Inference::BATCH_SIZE * size * sizeof(float)); + + //Size the tensors based on tensor type + if (this->enginePtr->getTensorIOMode(this->enginePtr->getIOTensorName(i)) == nvinfer1::TensorIOMode::kINPUT) { + this->inputTensor.resize(size); + } else if (this->enginePtr->getTensorIOMode(this->enginePtr->getIOTensorName(i)) == nvinfer1::TensorIOMode::kOUTPUT) { + this->outputTensor.resize(size); + } + } +} -// void setup() { -// // TOOD: fix this -// nvinfer1::createInferBuilder(logger); -// } +// Initializes enginePtr with built engine +void Inference::createCudaEngine(std::string& onnxModelPath) { + // See link sfor additional context + const auto explicitBatch = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + std::unique_ptr> builder{nvinfer1::createInferBuilder(logger)}; + std::unique_ptr> network{builder->createNetworkV2(explicitBatch)}; + std::unique_ptr> parser{nvonnxparser::createParser(*network, logger)}; + std::unique_ptr> config{builder->createBuilderConfig()}; + + //Parse the onnx from file + if (!parser->parseFromFile(onnxModelPath.c_str(), static_cast(ILogger::Severity::kINFO))) { + std::cout << "ERROR: could not parse input engine." << std::endl; + } -void InferenceNew::launchInference(IExecutionContext* context, cudaStream_t stream, std::vector const& inputTensor, std::vector& outputTensor, void** bindings, int batchSize) { - int inputId = InferenceNew::getBindingInputIndex(context); + config->setMemoryPoolLimit(MemoryPoolType::kWORKSPACE, 1 << 30); - cudaMemcpyAsync(bindings[inputId], inputTensor.data(), inputTensor.size() * sizeof(float), cudaMemcpyHostToDevice, stream); + auto profile = builder->createOptimizationProfile(); + profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMIN, Dims4{1, 3, 256, 256}); + profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kOPT, Dims4{1, 3, 256, 256}); + profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMAX, Dims4{32, 3, 256, 256}); + config->addOptimizationProfile(profile); - context->enqueueV3(stream); + this->enginePtr = builder->buildSerializedNetwork(*network, *config); +} - cudaMemcpyAsync(outputTensor.data(), bindings[1 - inputId], outputTensor.size() * sizeof(float), cudaMemcpyDeviceToHost, stream); +int Inference::getBindingInputIndex(nvinfer1::IExecutionContext* context) { + return context->getEngine().getTensorIOMode(context->getEngine().getIOTensorName(0)) != nvinfer1::TensorIOMode::kINPUT; // 0 (false) if bindingIsInput(0), 1 (true) otherwise } -void InferenceNew::prepTensors(std::unique_ptr> enginePtr, std::array bindings, std::vector& inputTensor, std::vector& outputTensor) { - for (int i = 0; i < enginePtr->getNbBindings(); ++i) { - Dims dims{enginePtr->getBindingDimensions(i)}; - size_t size = accumulate(dims.d + 1, dims.d + dims.nbDims, InferenceNew::BATCH_SIZE, std::multiplies()); - // Create CUDA buffer for Tensor. - cudaMalloc(&bindings[i], InferenceNew::BATCH_SIZE * size * sizeof(float)); - // Resize CPU buffers to fit Tensor. - if (enginePtr->bindingIsInput(i)) { - inputTensor.resize(size); - } else - outputTensor.resize(size); +void Inference::setUpContext(const std::string& inputFile) { + // Read input tensor from input picture file. + if (readTensor(inputFile, this->inputTensor) != this->inputTensor.size()) { + std::cout << "Couldn't read input Tensor" << std::endl; } -} + + // Create Execution Context. + this->context.reset(this->enginePtr->createExecutionContext()); + + Dims dims_i{this->enginePtr->getBindingDimensions(0)}; + Dims4 inputDims{Inference::BATCH_SIZE, dims_i.d[1], dims_i.d[2], dims_i.d[3]}; + context->setBindingDimensions(0, inputDims); +} \ No newline at end of file From 718cb1add61202e28a19ded1c292752d262c9344 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Wed, 1 Nov 2023 21:57:06 -0400 Subject: [PATCH 053/197] Wrote Parser --- src/perception/object_detector/Parser.cpp | 112 ++++++++++++++++++ src/perception/object_detector/inference.h | 4 +- .../object_detector/object_detector.cu | 26 ++-- 3 files changed, 134 insertions(+), 8 deletions(-) create mode 100644 src/perception/object_detector/Parser.cpp diff --git a/src/perception/object_detector/Parser.cpp b/src/perception/object_detector/Parser.cpp new file mode 100644 index 000000000..8f20207a6 --- /dev/null +++ b/src/perception/object_detector/Parser.cpp @@ -0,0 +1,112 @@ +#pragma once + +//OPEN CV +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//STD +#include + +//INFERENCE +#include "inference.h" + +class Parser { +private: + //The tensor to be parsed + std::vector tensor; + + //MAT VALS + int width; + + //NET PARAMS + float modelScoreThreshold = 0.5; + float modelNMSThreshold = 0.50; + std::vector classes{"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; + + +public: + Parser(std::vector& tensor) { + this->tensor = tensor; + } + + std::vector parseTensor() { + int rows = 8400; //Number of outputs in model + int dimension = 84; //80 + 4 + int numClasses = 80; //The number of possible classes + + //Output vector of detections + std::vector detections; + + //Intermediary vectors of data + std::vector class_ids; + std::vector confidences; + std::vector boxes; + + for (int x = 0; x < rows; x++) { + int ptr = x; + + //Create box + float l = tensor[ptr]; + ptr += rows; + float h = tensor[ptr]; + ptr += rows; + float width = tensor[ptr]; + ptr += rows; + float height = tensor[ptr]; + + int left = int((l - 0.5 * width)); + int top = int((h - 0.5 * height)); + + + boxes.push_back(cv::Rect(left, top, static_cast(width), static_cast(height))); + + //Find class id and confidence + int maxIndex = 0; + float maxConfidence = 0; + + for (int i = 0; i < numClasses; i++) { + ptr += rows; + if (tensor[ptr] > maxConfidence) { + maxConfidence = tensor[ptr]; + maxIndex = i; + } + } + + confidences.push_back(maxConfidence); + class_ids.push_back(maxIndex); + } + + std::vector nms_result; + cv::dnn::NMSBoxes(boxes, confidences, modelScoreThreshold, modelNMSThreshold, nms_result); + + for (unsigned long i = 0; i < nms_result.size(); ++i) { + int idx = nms_result[i]; + + Detection result; + result.class_id = class_ids[idx]; + result.confidence = confidences[idx]; + + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution dis(100, 255); + result.color = cv::Scalar(dis(gen), + dis(gen), + dis(gen)); + + result.className = classes[result.class_id]; + result.box = boxes[idx]; + + detections.push_back(result); + } + + return detections; + } +}; \ No newline at end of file diff --git a/src/perception/object_detector/inference.h b/src/perception/object_detector/inference.h index 88b44b128..e56f34c7f 100644 --- a/src/perception/object_detector/inference.h +++ b/src/perception/object_detector/inference.h @@ -84,6 +84,8 @@ class Inference { void prepTensors(); - void doDetection(); + void Inference::setUpContext(); + + std::vector Inference::doDetections(const std::string& inputFile) } #endif // INFERENCE_H diff --git a/src/perception/object_detector/object_detector.cu b/src/perception/object_detector/object_detector.cu index 111a678b2..4ef5913c6 100644 --- a/src/perception/object_detector/object_detector.cu +++ b/src/perception/object_detector/object_detector.cu @@ -6,11 +6,16 @@ #include "ioHelper.h" #include +#include #include #include + #include "inference.h" +//Include Parser +#include "Parser.cpp" + #include #include #include @@ -99,16 +104,23 @@ int Inference::getBindingInputIndex(nvinfer1::IExecutionContext* context) { } -void Inference::setUpContext(const std::string& inputFile) { - // Read input tensor from input picture file. - if (readTensor(inputFile, this->inputTensor) != this->inputTensor.size()) { - std::cout << "Couldn't read input Tensor" << std::endl; - } - +void Inference::setUpContext() { // Create Execution Context. this->context.reset(this->enginePtr->createExecutionContext()); Dims dims_i{this->enginePtr->getBindingDimensions(0)}; Dims4 inputDims{Inference::BATCH_SIZE, dims_i.d[1], dims_i.d[2], dims_i.d[3]}; context->setBindingDimensions(0, inputDims); -} \ No newline at end of file +} + +std::vector Inference::doDetections(const std::string& inputFile) { + // Read input tensor from input picture file. + if (readTensor(inputFile, this->inputTensor) != this->inputTensor.size()) { + std::cout << "Couldn't read input Tensor" << std::endl; + } + + //Do the forward pass on the network + launchInference(); + + return Parser(this->outputTensor).parseTensor(); +} From ac14193813cad0bd71b07617d71b64524347c6b2 Mon Sep 17 00:00:00 2001 From: lorraine Date: Thu, 2 Nov 2023 19:55:54 -0400 Subject: [PATCH 054/197] implemented part of getTagBearing() --- .../long_range_cam/long_range_tag_detector.cpp | 2 +- .../long_range_cam/long_range_tag_detector.hpp | 2 +- .../long_range_tag_detector.processing.cpp | 14 ++++++++++---- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp index 4b34c8faa..03252cb8b 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp @@ -8,7 +8,7 @@ namespace mrover { mDetectorParams = new cv::aruco::DetectorParameters(); auto defaultDetectorParams = cv::makePtr(); int dictionaryNumber; - + mPnh.param("publish_images", mPublishImages, true); mPnh.param("min_hit_count_before_publish", mMinHitCountBeforePublish, 5); mPnh.param("max_hit_count", mMaxHitCount, 5); diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp index 8e9160494..4826554a8 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp @@ -25,7 +25,7 @@ namespace mrover { // TODO: change this to param bool mPublishImages = true; // If set, we publish the images with the tags drawn on top - //Constatns set in ROS params for num hits needed to publish + //Constants set in ROS params for num hits needed to publish int mMinHitCountBeforePublish{}; int mBaseHitCount{}; int mMaxHitCount{}; diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp index 582d5761a..9b9bb2ef3 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp @@ -141,7 +141,7 @@ namespace mrover { return centerPoint; } - cv::Point2f LongRangeTagDetectorNodelet::getNormedTagCenterOffset(std::vector& tagCorners) const { + cv::Poinjt2f LongRangeTagDetectorNodelet::getNormedTagCenterOffset(std::vector& tagCorners) const { cv::Point2f offsetCenterPoint = getTagCenterOffsetPixels(tagCorners); offsetCenterPoint.x /= static_cast(mImgMsg.width); @@ -151,9 +151,15 @@ namespace mrover { } float LongRangeTagDetectorNodelet::getTagBearing(std::vector& tagCorners) const { - //TODO: Implement me! - - return {}; + //TODO: FINISH + //for HD720 resolution + pair center = getTagCenterPixels(tagCorners); + float x = center.first; + int width = //get width of the image + int angleOfFOV = 104; + float bearing = ((x - width / 2) / width) * angleOfFOV; + + return bearing; } void LongRangeTagDetectorNodelet::publishThresholdTags() { From a12c347aaa6874cfae40469649bdb8a314b46a8a Mon Sep 17 00:00:00 2001 From: jbrhm Date: Fri, 3 Nov 2023 10:18:59 -0400 Subject: [PATCH 055/197] Implementing Mats --- msg/DetectedObjects.msg | 5 + src/perception/object_detector/Inference.cpp | 61 ++++++++- src/perception/object_detector/Inference.cu | 75 +++++++++++ src/perception/object_detector/Parser.cpp | 112 ---------------- src/perception/object_detector/inference.h | 26 +++- .../object_detector/object_detector.cpp | 65 +++++++-- .../object_detector/object_detector.cu | 126 ------------------ .../object_detector/object_detector.hpp | 53 ++++---- .../object_detector.processing.cpp | 1 + src/perception/object_detector/parser.cpp | 78 +++++++++++ src/perception/object_detector/parser.hpp | 39 ++++++ src/perception/object_detector/pch.hpp | 3 + 12 files changed, 360 insertions(+), 284 deletions(-) create mode 100644 msg/DetectedObjects.msg create mode 100644 src/perception/object_detector/Inference.cu delete mode 100644 src/perception/object_detector/Parser.cpp delete mode 100644 src/perception/object_detector/object_detector.cu create mode 100644 src/perception/object_detector/parser.cpp create mode 100644 src/perception/object_detector/parser.hpp diff --git a/msg/DetectedObjects.msg b/msg/DetectedObjects.msg new file mode 100644 index 000000000..b0fe335be --- /dev/null +++ b/msg/DetectedObjects.msg @@ -0,0 +1,5 @@ +Header header + +int num_detections + +DetectedObject[] detections \ No newline at end of file diff --git a/src/perception/object_detector/Inference.cpp b/src/perception/object_detector/Inference.cpp index e9db30d75..2d027800c 100644 --- a/src/perception/object_detector/Inference.cpp +++ b/src/perception/object_detector/Inference.cpp @@ -1,17 +1,74 @@ #include "inference.h" +#include +#include +#include +#include +#include #include +#include "ioHelper.h" + +#include "NvInfer.h" + using namespace nvinfer1; using namespace std; // : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} -Inference::Inference(std::string_view onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string_view classesTxtFile = "") : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} { +Inference::Inference(std::string onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string_view classesTxtFile = "") : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} { - createCudaEngine(onnxModelPath, Inference::BATCH_SIZE); + createCudaEngine(onnxModelPath); + + this->modelInputShape = modelInputShape; assert(this->enginePtr->getNbBindings() == 2); assert(this->enginePtr->bindingIsInput(0) ^ enginePtr->bindingIsInput(1)); this->onnxModelPath = onnxModelPath; +} + + +// Initializes enginePtr with built engine +void Inference::createCudaEngine(std::string& onnxModelPath) { + // See link sfor additional context + const auto explicitBatch = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + std::unique_ptr> builder{nvinfer1::createInferBuilder(logger)}; + std::unique_ptr> network{builder->createNetworkV2(explicitBatch)}; + std::unique_ptr> parser{nvonnxparser::createParser(*network, logger)}; + std::unique_ptr> config{builder->createBuilderConfig()}; + + //Parse the onnx from file + if (!parser->parseFromFile(onnxModelPath.c_str(), static_cast(ILogger::Severity::kINFO))) { + std::cout << "ERROR: could not parse input engine." << std::endl; + } + + config->setMemoryPoolLimit(MemoryPoolType::kWORKSPACE, 1 << 30); + + auto profile = builder->createOptimizationProfile(); + profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMIN, Dims4{1, 3, 256, 256}); + profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kOPT, Dims4{1, 3, 256, 256}); + profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMAX, Dims4{32, 3, 256, 256}); + config->addOptimizationProfile(profile); + + this->enginePtr = builder->buildSerializedNetwork(*network, *config); +} + +std::vector Inference::doDetections(cv::Mat& img) { + //Do the forward pass on the network + launchInference(img.data, this->outputTensor.data, ); + + return Parser(this->outputTensor).parseTensor(); +} + +int Inference::getBindingInputIndex(nvinfer1::IExecutionContext* context) { + return context->getEngine().getTensorIOMode(context->getEngine().getIOTensorName(0)) != nvinfer1::TensorIOMode::kINPUT; // 0 (false) if bindingIsInput(0), 1 (true) otherwise +} + +void Inference::setUpContext() { + // Create Execution Context. + this->context.reset(this->enginePtr->createExecutionContext()); + + Dims dims_i{this->enginePtr->getBindingDimensions(0)}; + Dims4 inputDims{Inference::BATCH_SIZE, dims_i.d[1], dims_i.d[2], dims_i.d[3]}; + context->setBindingDimensions(0, inputDims); } \ No newline at end of file diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu new file mode 100644 index 000000000..39a2754f5 --- /dev/null +++ b/src/perception/object_detector/Inference.cu @@ -0,0 +1,75 @@ +#include +#include +#include +#include + +#include "ioHelper.h" + +#include +#include +#include +#include + +#include "inference.h" + +#include +#include +#include + +using namespace nvinfer1; + +/** +* Example Code: @link https://github.com/NVIDIA-developer-blog/code-samples/blob/master/posts/TensorRT-introduction/simpleOnnx_1.cpp +* IExecutionContest @link https://docs.nvidia.com/deeplearning/tensorrt/api/c_api/classnvinfer1_1_1_i_execution_context.html +* ------------------------------------------------------ +* For additional context see @link https://www.edge-ai-vision.com/2020/04/speeding-up-deep-learning-inference-using-tensorrt/ +*/ + + +/** +* cudaMemcpys CPU memory in inputTensor to GPU based on bindings +* Queues that tensor to be passed through model +* cudaMemcpys the result back to CPU memory +* Requires bindings, inputTensor, stream +* Modifies stream, outputTensor +*/ +void Inference::launchInference(float* input, float* output) { + int inputId = Inference::getBindingInputIndex(this->context); + + //Copy data to GPU memory + cudaMemcpyAsync(this->bindings[inputId], input, inputEntries.d[0] * inputEntries.d[1] * inputEntries.d[2] * sizeof(float), cudaMemcpyHostToDevice, this->stream); + + // + this->context->enqueueV3(this->stream); + + //Copy data to CPU memory + cudaMemcpyAsync(output, this->bindings[1 - inputId], outputEntries.d[0] * outputEntries.d[1] * outputEntries.d[2] * sizeof(float), cudaMemcpyDeviceToHost, this->stream); +} + +/** +* Takes tensor bindings and allocates memory on the GPU for input and output tensors +* Requires enginePtr, bindings, inputTensor, and outputTensor +* Modifies bindings, inputTensor, and outputTensor +*/ +void Inference::prepTensors() { + for (int i = 0; i < this->enginePtr->getNbIOTensors(); i++) { + const char* tensorName = this->enginePtr->getIOTensorName(i); + + Dims dims{this->enginePtr->getTensorShape(tensorName)}; + + size_t size = accumulate(dims.d + 1, dims.d + dims.nbDims, Inference::BATCH_SIZE, std::multiplies<>()); + std::vector sizes = {dims.d[1], dims.d[2], dims.d[3]}; + + + // Create CUDA buffer for Tensor. + cudaMalloc(&(this->bindings)[i], Inference::BATCH_SIZE * size * sizeof(float)); + + + //Size the tensors based on tensor type + if (this->enginePtr->getTensorIOMode(this->enginePtr->getIOTensorName(i)) == nvinfer1::TensorIOMode::kINPUT) { + this->inputTensor.reshape(3, sizes); + } else if (this->enginePtr->getTensorIOMode(this->enginePtr->getIOTensorName(i)) == nvinfer1::TensorIOMode::kOUTPUT) { + this->outputTensor.resize(size); + } + } +} diff --git a/src/perception/object_detector/Parser.cpp b/src/perception/object_detector/Parser.cpp deleted file mode 100644 index 8f20207a6..000000000 --- a/src/perception/object_detector/Parser.cpp +++ /dev/null @@ -1,112 +0,0 @@ -#pragma once - -//OPEN CV -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -//STD -#include - -//INFERENCE -#include "inference.h" - -class Parser { -private: - //The tensor to be parsed - std::vector tensor; - - //MAT VALS - int width; - - //NET PARAMS - float modelScoreThreshold = 0.5; - float modelNMSThreshold = 0.50; - std::vector classes{"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; - - -public: - Parser(std::vector& tensor) { - this->tensor = tensor; - } - - std::vector parseTensor() { - int rows = 8400; //Number of outputs in model - int dimension = 84; //80 + 4 - int numClasses = 80; //The number of possible classes - - //Output vector of detections - std::vector detections; - - //Intermediary vectors of data - std::vector class_ids; - std::vector confidences; - std::vector boxes; - - for (int x = 0; x < rows; x++) { - int ptr = x; - - //Create box - float l = tensor[ptr]; - ptr += rows; - float h = tensor[ptr]; - ptr += rows; - float width = tensor[ptr]; - ptr += rows; - float height = tensor[ptr]; - - int left = int((l - 0.5 * width)); - int top = int((h - 0.5 * height)); - - - boxes.push_back(cv::Rect(left, top, static_cast(width), static_cast(height))); - - //Find class id and confidence - int maxIndex = 0; - float maxConfidence = 0; - - for (int i = 0; i < numClasses; i++) { - ptr += rows; - if (tensor[ptr] > maxConfidence) { - maxConfidence = tensor[ptr]; - maxIndex = i; - } - } - - confidences.push_back(maxConfidence); - class_ids.push_back(maxIndex); - } - - std::vector nms_result; - cv::dnn::NMSBoxes(boxes, confidences, modelScoreThreshold, modelNMSThreshold, nms_result); - - for (unsigned long i = 0; i < nms_result.size(); ++i) { - int idx = nms_result[i]; - - Detection result; - result.class_id = class_ids[idx]; - result.confidence = confidences[idx]; - - std::random_device rd; - std::mt19937 gen(rd()); - std::uniform_int_distribution dis(100, 255); - result.color = cv::Scalar(dis(gen), - dis(gen), - dis(gen)); - - result.className = classes[result.class_id]; - result.box = boxes[idx]; - - detections.push_back(result); - } - - return detections; - } -}; \ No newline at end of file diff --git a/src/perception/object_detector/inference.h b/src/perception/object_detector/inference.h index e56f34c7f..169975209 100644 --- a/src/perception/object_detector/inference.h +++ b/src/perception/object_detector/inference.h @@ -2,8 +2,12 @@ #define INFERENCE_H // Cpp native +#include +#include #include +#include #include +#include #include #include #include @@ -25,10 +29,12 @@ #include "pch.hpp" -using nvinfer1::cuda; using nvinfer1::ICudaEngine; using nvinfer1::IExecutionContext; +// ROS Mesage -> CV View Matrix -> cv::blobFromImage -> cv::Mat (input) +// Preallocated CV::Mat of floats of correct size -> transpose into CV::Mat (also preallocated + struct Detection { int class_id{0}; std::string className{}; @@ -46,12 +52,16 @@ class Inference { //Ptr to the engine std::unique_ptr> enginePtr; + //Ptr to the context std::unique_ptr> context; + //Input, output and reference tensors - std::vector inputTensor; - std::vector outputTensor; - std::vector referenceTensor; + cv::Mat inputTensor; + cv::Mat outputTensor; + //Num Entries + nvinfer1::Dims3 inputEntries; + nvinfer1::Dims3 outputEntries; //Cuda Stream CudaStream stream; @@ -62,6 +72,10 @@ class Inference { //ONNX Model Path std::string onnxModelPath; + //Size of Model + std::modelInputShape; + cv::Size modelOutputShape; + private: //STATIC FUNCTIONS static int getBindingInputIndex(nvinfer1::IExecutionContext* context); @@ -78,7 +92,7 @@ class Inference { //Creates a ptr to the engine void createCudaEngine(std::string& onnxModelPath); - void launchInference(); + void Inference::launchInference(float* input, float* output, int tensorSize); void setUpContext(const std::string& inputFile); //This will need to be adjusted to the img msg, but I think its ok to just run pictures as an intial test @@ -86,6 +100,6 @@ class Inference { void Inference::setUpContext(); - std::vector Inference::doDetections(const std::string& inputFile) + std::vector Inference::doDetections(const std::string& inputFile); } #endif // INFERENCE_H diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 7d58ff768..902986ff9 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -1,7 +1,12 @@ #include "object_detector.hpp" -#include "inference.h" +//#include "inference.h" #include + #include +//#include + +#include +#include #include #include #include @@ -14,30 +19,62 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - //Inference - - inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), "", true); + inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //read ONNX file into this mNet, YOLOV8, second smallest one // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases // TODO(percep/obj-detectr): make this configurable mImgSub = mNh.subscribe("/camera/left/image", 1, &ObjectDetectorNodelet::imageCallback, this); mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); - mDetectionData = mNh.advertise("/object_detector/detected_object", 1); + mDetectionData = mNh.advertise("/object_detector/detected_object", 1); + + this->imageBlob = cv::Mat{1, ObjectDetectorNodelet::NUM_CHANNELS, ObjectDetectorNodelet::IMG_WIDTH, ObjectDetectorNodelet::IMG_HEIGHT, CV_32F}; } -} // namespace mrover -int main(int argc, char** argv) { - ros::init(argc, argv, "object_detector"); + void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { + //Ensure a valid message was received + assert(msg); + + //Get a CV::Mat image view + cv::Mat imageView{static_cast(msg->height), static_cast(msg->width), CV_8UC3, const_cast(msg->data.data())}; + + cv::dnn::blobFromImage(imageView, this->imageBlob); + + //Run Image Detections + //Return type of Detection struct + std::vector detections = this->inference.doDetections(imageView); + + //just an array of DetectedObject + DetectedObjects detectionsMessage; - // Start the ZED Nodelet - nodelet::Loader nodelet; - nodelet.load(ros::this_node::getName(), "mrover/ObjectDetectorNodelet", ros::names::getRemappings(), {}); + detectionsMessage.num_detections = static_cast(detections.size()); - ros::spin(); + //Convert Vector of Detections to Output Message + for (Detection& detection: detections) { + DetectedObject objMsg = convertToObjMsg(detection); + detectionsMessage.push_back(objMsg); + } - return EXIT_SUCCESS; -} + // detectionsMessage.header.seq = mSeqNum; + detectionsMessage.header.stamp = ros::Time::now(); + // detectionsMessage.header.frame_id = "frame" + + this->publisher + } + + int main(int argc, char** argv) { + ros::init(argc, argv, "object_detector"); + + // Start the ZED Nodelet + nodelet::Loader nodelet; + nodelet.load(ros::this_node::getName(), "mrover/ObjectDetectorNodelet", ros::names::getRemappings(), {}); + + ros::spin(); + + return EXIT_SUCCESS; + } + +} // namespace mrover #ifdef MROVER_IS_NODELET #include diff --git a/src/perception/object_detector/object_detector.cu b/src/perception/object_detector/object_detector.cu deleted file mode 100644 index 4ef5913c6..000000000 --- a/src/perception/object_detector/object_detector.cu +++ /dev/null @@ -1,126 +0,0 @@ -#include -#include -#include -#include - -#include "ioHelper.h" - -#include -#include -#include -#include - - -#include "inference.h" - -//Include Parser -#include "Parser.cpp" - -#include -#include -#include - -using namespace nvinfer1; - -/** -* Example Code: @link https://github.com/NVIDIA-developer-blog/code-samples/blob/master/posts/TensorRT-introduction/simpleOnnx_1.cpp -* IExecutionContest @link https://docs.nvidia.com/deeplearning/tensorrt/api/c_api/classnvinfer1_1_1_i_execution_context.html -* ------------------------------------------------------ -* For additional context see @link https://www.edge-ai-vision.com/2020/04/speeding-up-deep-learning-inference-using-tensorrt/ -*/ - - -/** -* cudaMemcpys CPU memory in inputTensor to GPU based on bindings -* Queues that tensor to be passed through model -* cudaMemcpys the result back to CPU memory -* Requires bindings, inputTensor, stream -* Modifies stream, outputTensor -*/ -void Inference::launchInference() { - int inputId = Inference::getBindingInputIndex(this->context); - - //Copy data to GPU memory - cudaMemcpyAsync(this->bindings[inputId], this->inputTensor.data(), this->inputTensor.size() * sizeof(float), cudaMemcpyHostToDevice, this->stream); - - // - this->context->enqueueV3(this->stream); - - //Copy data to CPU memory - cudaMemcpyAsync(this->outputTensor.data(), this->bindings[1 - inputId], this->outputTensor.size() * sizeof(float), cudaMemcpyDeviceToHost, this->stream); -} - -/** -* Takes tensor bindings and allocates memory on the GPU for input and output tensors -* Requires enginePtr, bindings, inputTensor, and outputTensor -* Modifies bindings, inputTensor, and outputTensor -*/ -void Inference::prepTensors() { - for (int i = 0; i < this->enginePtr->getNbIOTensors(); i++) { - const char* tensorName = this->enginePtr->getIOTensorName(i); - - Dims dims{this->enginePtr->getTensorShape(tensorName)}; - - size_t size = accumulate(dims.d + 1, dims.d + dims.nbDims, Inference::BATCH_SIZE, std::multiplies<>()); - // Create CUDA buffer for Tensor. - cudaMalloc(&(this->bindings)[i], Inference::BATCH_SIZE * size * sizeof(float)); - - //Size the tensors based on tensor type - if (this->enginePtr->getTensorIOMode(this->enginePtr->getIOTensorName(i)) == nvinfer1::TensorIOMode::kINPUT) { - this->inputTensor.resize(size); - } else if (this->enginePtr->getTensorIOMode(this->enginePtr->getIOTensorName(i)) == nvinfer1::TensorIOMode::kOUTPUT) { - this->outputTensor.resize(size); - } - } -} - -// Initializes enginePtr with built engine -void Inference::createCudaEngine(std::string& onnxModelPath) { - // See link sfor additional context - const auto explicitBatch = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); - std::unique_ptr> builder{nvinfer1::createInferBuilder(logger)}; - std::unique_ptr> network{builder->createNetworkV2(explicitBatch)}; - std::unique_ptr> parser{nvonnxparser::createParser(*network, logger)}; - std::unique_ptr> config{builder->createBuilderConfig()}; - - //Parse the onnx from file - if (!parser->parseFromFile(onnxModelPath.c_str(), static_cast(ILogger::Severity::kINFO))) { - std::cout << "ERROR: could not parse input engine." << std::endl; - } - - config->setMemoryPoolLimit(MemoryPoolType::kWORKSPACE, 1 << 30); - - auto profile = builder->createOptimizationProfile(); - profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMIN, Dims4{1, 3, 256, 256}); - profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kOPT, Dims4{1, 3, 256, 256}); - profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMAX, Dims4{32, 3, 256, 256}); - config->addOptimizationProfile(profile); - - this->enginePtr = builder->buildSerializedNetwork(*network, *config); -} - -int Inference::getBindingInputIndex(nvinfer1::IExecutionContext* context) { - return context->getEngine().getTensorIOMode(context->getEngine().getIOTensorName(0)) != nvinfer1::TensorIOMode::kINPUT; // 0 (false) if bindingIsInput(0), 1 (true) otherwise -} - - -void Inference::setUpContext() { - // Create Execution Context. - this->context.reset(this->enginePtr->createExecutionContext()); - - Dims dims_i{this->enginePtr->getBindingDimensions(0)}; - Dims4 inputDims{Inference::BATCH_SIZE, dims_i.d[1], dims_i.d[2], dims_i.d[3]}; - context->setBindingDimensions(0, inputDims); -} - -std::vector Inference::doDetections(const std::string& inputFile) { - // Read input tensor from input picture file. - if (readTensor(inputFile, this->inputTensor) != this->inputTensor.size()) { - std::cout << "Couldn't read input Tensor" << std::endl; - } - - //Do the forward pass on the network - launchInference(); - - return Parser(this->outputTensor).parseTensor(); -} diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 8b9a48295..820ad7fad 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -1,46 +1,51 @@ -#include "inference.h" +//#include "inference.h" #include "pch.hpp" #include #include -namespace mrover { +class ObjectDetectorNodelet : public nodelet::Nodelet { + static const int NUM_CHANNELS = 3; + static const int IMG_WIDTH = 1280; + static const int IMG_HEIGHT = 720; - class ObjectDetectorNodelet : public nodelet::Nodelet { - private: - ros::NodeHandle mNh, mPnh; +private: + ros::NodeHandle mNh, mPnh; - Inference inference; + //Inference inference; - // Publishers + // Publishers - ros::Publisher mDebugImgPub; - ros::Publisher mDetectionData; + ros::Publisher mDebugImgPub; + ros::Publisher mDetectionData; - // Subscribers + // Subscribers - ros::Subscriber mImgSub; + ros::Subscriber mImgSub; + // Preallocated cv::Mats + cv::Mat imageBlob; - dynamic_reconfigure::Server mConfigServer; - dynamic_reconfigure::Server::CallbackType mCallbackType; + dynamic_reconfigure::Server mConfigServer; + dynamic_reconfigure::Server::CallbackType mCallbackType; - // Internal state + // Internal state - cv::dnn::Net mNet; + cv::dnn::Net mNet; - // Debug + // Debug - LoopProfiler mProfiler{"Object Detector"}; + LoopProfiler mProfiler{"Object Detector"}; - void onInit() override; + void onInit() override; - public: - ObjectDetectorNodelet() = default; + //DetectedObject convertToObjMsg(Detection& detection); - ~ObjectDetectorNodelet() override = default; - void imageCallback(sensor_msgs::ImageConstPtr const& msg); - }; +public: + ObjectDetectorNodelet() = default; -} // namespace mrover + ~ObjectDetectorNodelet() override = default; + + void imageCallback(sensor_msgs::ImageConstPtr const& msg); +}; diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 84668906a..c5fc4e03f 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -1,5 +1,6 @@ #include "object_detector.hpp" #include "inference.h" + #include #include #include diff --git a/src/perception/object_detector/parser.cpp b/src/perception/object_detector/parser.cpp new file mode 100644 index 000000000..6945aabf9 --- /dev/null +++ b/src/perception/object_detector/parser.cpp @@ -0,0 +1,78 @@ +#include "parser.h" + +Parser::Parser(std::vector& tensor) { + this->tensor = tensor; +} + +Parser::parseTensor() { + int cols = 8400; //Number of outputs in model + int dimension = 84; //80 + 4 + int numClasses = 80; //The number of possible classes + + //Output vector of detections + std::vector detections; + + //Intermediary vectors of data + std::vector class_ids; + std::vector confidences; + std::vector boxes; + + for (int x = 0; x < cols; x++) { + int ptr = x; + + //Create box + float l = tensor[ptr]; + ptr += cols; + float h = tensor[ptr]; + ptr += cols; + float width = tensor[ptr]; + ptr += cols; + float height = tensor[ptr]; + + int left = int((l - 0.5 * width)); + int top = int((h - 0.5 * height)); + + + boxes.push_back(cv::Rect(left, top, static_cast(width), static_cast(height))); + + //Find class id and confidence + int maxIndex = 0; + float maxConfidence = 0; + + for (int i = 0; i < numClasses; i++) { + ptr += cols; + if (tensor[ptr] > maxConfidence) { + maxConfidence = tensor[ptr]; + maxIndex = i; + } + } + + confidences.push_back(maxConfidence); + class_ids.push_back(maxIndex); + } + + std::vector nms_result; + cv::dnn::NMSBoxes(boxes, confidences, modelScoreThreshold, modelNMSThreshold, nms_result); + + for (unsigned long i = 0; i < nms_result.size(); ++i) { + int idx = nms_result[i]; + + Detection result; + result.class_id = class_ids[idx]; + result.confidence = confidences[idx]; + + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution dis(100, 255); + result.color = cv::Scalar(dis(gen), + dis(gen), + dis(gen)); + + result.className = classes[result.class_id]; + result.box = boxes[idx]; + + detections.push_back(result); + } + + return detections; +} \ No newline at end of file diff --git a/src/perception/object_detector/parser.hpp b/src/perception/object_detector/parser.hpp new file mode 100644 index 000000000..dfd980283 --- /dev/null +++ b/src/perception/object_detector/parser.hpp @@ -0,0 +1,39 @@ +#ifndef PARSER_H +#define PARSER_H + +//OPEN CV +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//STD +#include + +#include "inference.h" + +class Parser { +private: + std::vector tensor; + + //MAT VALS + int width; + + //NET PARAMS + float modelScoreThreshold = 0.5; + float modelNMSThreshold = 0.50; + std::vector classes{"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; + +public: + Parser(std::vector& tensor); + + std::vector parseTensor(); +} + +#endif \ No newline at end of file diff --git a/src/perception/object_detector/pch.hpp b/src/perception/object_detector/pch.hpp index a34fad005..bac77df18 100644 --- a/src/perception/object_detector/pch.hpp +++ b/src/perception/object_detector/pch.hpp @@ -30,5 +30,8 @@ #include +#include +#include + #include #include From 33d2fb4f14af11b85da42768543906ace90818f7 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Fri, 3 Nov 2023 11:04:22 -0400 Subject: [PATCH 056/197] Remove Until Built --- ...etectedObjects.msg => DetectedObjects.txt} | 0 .../{Inference.cpp => Inferencecpp.txt} | 0 .../{Inference.cu => Inferencecu.txt} | 0 .../{cudaWrapper.h => cudaWrapperh.txt} | 0 .../{inference.h => inferenceh.txt} | 0 .../{ioHelper.cpp => ioHelpercpp.txt} | 0 .../{ioHelper.h => ioHelperh.txt} | 0 .../object_detector/object_detector.cpp | 53 ++++----------- .../object_detector/object_detector.hpp | 65 +++++++++++-------- .../object_detector.processing.cpp | 45 +++++++++++-- .../{parser.cpp => parsercpp.txt} | 0 .../{parser.hpp => parserhpp.txt} | 0 src/perception/object_detector/pch.hpp | 2 +- 13 files changed, 91 insertions(+), 74 deletions(-) rename msg/{DetectedObjects.msg => DetectedObjects.txt} (100%) rename src/perception/object_detector/{Inference.cpp => Inferencecpp.txt} (100%) rename src/perception/object_detector/{Inference.cu => Inferencecu.txt} (100%) rename src/perception/object_detector/{cudaWrapper.h => cudaWrapperh.txt} (100%) rename src/perception/object_detector/{inference.h => inferenceh.txt} (100%) rename src/perception/object_detector/{ioHelper.cpp => ioHelpercpp.txt} (100%) rename src/perception/object_detector/{ioHelper.h => ioHelperh.txt} (100%) rename src/perception/object_detector/{parser.cpp => parsercpp.txt} (100%) rename src/perception/object_detector/{parser.hpp => parserhpp.txt} (100%) diff --git a/msg/DetectedObjects.msg b/msg/DetectedObjects.txt similarity index 100% rename from msg/DetectedObjects.msg rename to msg/DetectedObjects.txt diff --git a/src/perception/object_detector/Inference.cpp b/src/perception/object_detector/Inferencecpp.txt similarity index 100% rename from src/perception/object_detector/Inference.cpp rename to src/perception/object_detector/Inferencecpp.txt diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inferencecu.txt similarity index 100% rename from src/perception/object_detector/Inference.cu rename to src/perception/object_detector/Inferencecu.txt diff --git a/src/perception/object_detector/cudaWrapper.h b/src/perception/object_detector/cudaWrapperh.txt similarity index 100% rename from src/perception/object_detector/cudaWrapper.h rename to src/perception/object_detector/cudaWrapperh.txt diff --git a/src/perception/object_detector/inference.h b/src/perception/object_detector/inferenceh.txt similarity index 100% rename from src/perception/object_detector/inference.h rename to src/perception/object_detector/inferenceh.txt diff --git a/src/perception/object_detector/ioHelper.cpp b/src/perception/object_detector/ioHelpercpp.txt similarity index 100% rename from src/perception/object_detector/ioHelper.cpp rename to src/perception/object_detector/ioHelpercpp.txt diff --git a/src/perception/object_detector/ioHelper.h b/src/perception/object_detector/ioHelperh.txt similarity index 100% rename from src/perception/object_detector/ioHelper.h rename to src/perception/object_detector/ioHelperh.txt diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 902986ff9..2df9f4ce1 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -1,6 +1,5 @@ #include "object_detector.hpp" //#include "inference.h" -#include #include //#include @@ -19,62 +18,32 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); + //inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //read ONNX file into this mNet, YOLOV8, second smallest one // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases // TODO(percep/obj-detectr): make this configurable mImgSub = mNh.subscribe("/camera/left/image", 1, &ObjectDetectorNodelet::imageCallback, this); mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); - mDetectionData = mNh.advertise("/object_detector/detected_object", 1); + //mDetectionData = mNh.advertise("/object_detector/detected_object", 1); this->imageBlob = cv::Mat{1, ObjectDetectorNodelet::NUM_CHANNELS, ObjectDetectorNodelet::IMG_WIDTH, ObjectDetectorNodelet::IMG_HEIGHT, CV_32F}; } +} // namespace mrover - void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { - //Ensure a valid message was received - assert(msg); - - //Get a CV::Mat image view - cv::Mat imageView{static_cast(msg->height), static_cast(msg->width), CV_8UC3, const_cast(msg->data.data())}; - - cv::dnn::blobFromImage(imageView, this->imageBlob); - - //Run Image Detections - //Return type of Detection struct - std::vector detections = this->inference.doDetections(imageView); - - //just an array of DetectedObject - DetectedObjects detectionsMessage; - - detectionsMessage.num_detections = static_cast(detections.size()); - - //Convert Vector of Detections to Output Message - for (Detection& detection: detections) { - DetectedObject objMsg = convertToObjMsg(detection); - detectionsMessage.push_back(objMsg); - } - - // detectionsMessage.header.seq = mSeqNum; - detectionsMessage.header.stamp = ros::Time::now(); - // detectionsMessage.header.frame_id = "frame" - - this->publisher - } - int main(int argc, char** argv) { - ros::init(argc, argv, "object_detector"); +int main(int argc, char** argv) { + ros::init(argc, argv, "object_detector"); - // Start the ZED Nodelet - nodelet::Loader nodelet; - nodelet.load(ros::this_node::getName(), "mrover/ObjectDetectorNodelet", ros::names::getRemappings(), {}); + // Start the ZED Nodelet + nodelet::Loader nodelet; + nodelet.load(ros::this_node::getName(), "mrover/ObjectDetectorNodelet", ros::names::getRemappings(), {}); - ros::spin(); + ros::spin(); - return EXIT_SUCCESS; - } + return EXIT_SUCCESS; +} -} // namespace mrover #ifdef MROVER_IS_NODELET #include diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 820ad7fad..f24e66169 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -3,49 +3,60 @@ #include #include -class ObjectDetectorNodelet : public nodelet::Nodelet { - static const int NUM_CHANNELS = 3; - static const int IMG_WIDTH = 1280; - static const int IMG_HEIGHT = 720; +namespace mrover { -private: - ros::NodeHandle mNh, mPnh; + struct Detection { + int class_id{0}; + std::string className{}; + float confidence{0.0}; + cv::Scalar color{}; + cv::Rect box{}; + }; - //Inference inference; + class ObjectDetectorNodelet : public nodelet::Nodelet { + static const int NUM_CHANNELS = 3; + static const int IMG_WIDTH = 1280; + static const int IMG_HEIGHT = 720; - // Publishers + private: + ros::NodeHandle mNh, mPnh; - ros::Publisher mDebugImgPub; - ros::Publisher mDetectionData; + //Inference inference; + // Publishers - // Subscribers + ros::Publisher mDebugImgPub; + ros::Publisher mDetectionData; - ros::Subscriber mImgSub; - // Preallocated cv::Mats - cv::Mat imageBlob; + // Subscribers - dynamic_reconfigure::Server mConfigServer; - dynamic_reconfigure::Server::CallbackType mCallbackType; + ros::Subscriber mImgSub; - // Internal state + // Preallocated cv::Mats + cv::Mat imageBlob; - cv::dnn::Net mNet; + dynamic_reconfigure::Server mConfigServer; + dynamic_reconfigure::Server::CallbackType mCallbackType; - // Debug + // Internal state - LoopProfiler mProfiler{"Object Detector"}; + cv::dnn::Net mNet; - void onInit() override; + // Debug - //DetectedObject convertToObjMsg(Detection& detection); + LoopProfiler mProfiler{"Object Detector"}; + void onInit() override; -public: - ObjectDetectorNodelet() = default; + //DetectedObject convertToObjMsg(Detection& detection); - ~ObjectDetectorNodelet() override = default; - void imageCallback(sensor_msgs::ImageConstPtr const& msg); -}; + public: + ObjectDetectorNodelet() = default; + + ~ObjectDetectorNodelet() override = default; + + void imageCallback(sensor_msgs::ImageConstPtr const& msg); + }; +} // namespace mrover \ No newline at end of file diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index c5fc4e03f..5202310b7 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -1,7 +1,8 @@ #include "object_detector.hpp" -#include "inference.h" +//#include "inference.h" #include +#include #include #include #include @@ -22,6 +23,42 @@ namespace mrover { void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { + uint x; + x = msg->height; + std::cout << x << std::endl; + } + /* + void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { + //Ensure a valid message was received + assert(msg); + + //Get a CV::Mat image view + cv::Mat imageView{static_cast(msg->height), static_cast(msg->width), CV_8UC3, const_cast(msg->data.data())}; + + cv::dnn::blobFromImage(imageView, this->imageBlob); + + //Run Image Detections + //Return type of Detection struct + std::vector detections = this->inference.doDetections(imageView); + + //just an array of DetectedObject + DetectedObjects detectionsMessage; + + detectionsMessage.num_detections = static_cast(detections.size()); + + //Convert Vector of Detections to Output Message + for (Detection& detection: detections) { + DetectedObject objMsg = convertToObjMsg(detection); + detectionsMessage.push_back(objMsg); + } + + // detectionsMessage.header.seq = mSeqNum; + detectionsMessage.header.stamp = ros::Time::now(); + // detectionsMessage.header.frame_id = "frame" + + this->publisher; + } + assert(msg); assert(msg->height > 0); assert(msg->width > 0); @@ -104,8 +141,9 @@ namespace mrover { } // Publish to } - //Look at yolov8 documentation for the output matrix - /* + */ + //Look at yolov8 documentation for the output matrix + /* * TODO(percep/obj-detector): * 0. Google "OpenCV DNN example." View a couple of tutorials. Most will be in Python but the C++ API is similar. * 1. Use cv::dnn::blobFromImage to convert the image to a blob @@ -113,6 +151,5 @@ namespace mrover { * 3. Parse the output of the network to get the bounding boxes * 4. Publish the bounding boxes */ - } } // namespace mrover diff --git a/src/perception/object_detector/parser.cpp b/src/perception/object_detector/parsercpp.txt similarity index 100% rename from src/perception/object_detector/parser.cpp rename to src/perception/object_detector/parsercpp.txt diff --git a/src/perception/object_detector/parser.hpp b/src/perception/object_detector/parserhpp.txt similarity index 100% rename from src/perception/object_detector/parser.hpp rename to src/perception/object_detector/parserhpp.txt diff --git a/src/perception/object_detector/pch.hpp b/src/perception/object_detector/pch.hpp index bac77df18..93bb96dc7 100644 --- a/src/perception/object_detector/pch.hpp +++ b/src/perception/object_detector/pch.hpp @@ -31,7 +31,7 @@ #include #include -#include +//#include #include #include From 6e8c3c0cfb550b872bc43f9d4025bbede19b7bb0 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Fri, 3 Nov 2023 13:47:03 -0400 Subject: [PATCH 057/197] Pre ONNX Environment setup --- .../object_detector/{cudaWrapperh.txt => cudaWrapper.cuh} | 0 src/perception/object_detector/{ioHelpercpp.txt => ioHelper.cu} | 2 +- src/perception/object_detector/{ioHelperh.txt => ioHelper.cuh} | 0 3 files changed, 1 insertion(+), 1 deletion(-) rename src/perception/object_detector/{cudaWrapperh.txt => cudaWrapper.cuh} (100%) rename src/perception/object_detector/{ioHelpercpp.txt => ioHelper.cu} (99%) rename src/perception/object_detector/{ioHelperh.txt => ioHelper.cuh} (100%) diff --git a/src/perception/object_detector/cudaWrapperh.txt b/src/perception/object_detector/cudaWrapper.cuh similarity index 100% rename from src/perception/object_detector/cudaWrapperh.txt rename to src/perception/object_detector/cudaWrapper.cuh diff --git a/src/perception/object_detector/ioHelpercpp.txt b/src/perception/object_detector/ioHelper.cu similarity index 99% rename from src/perception/object_detector/ioHelpercpp.txt rename to src/perception/object_detector/ioHelper.cu index af3ae0a2a..85fc71309 100644 --- a/src/perception/object_detector/ioHelpercpp.txt +++ b/src/perception/object_detector/ioHelper.cu @@ -1,4 +1,4 @@ -#include "ioHelper.h" +#include "ioHelper.cuh" #include #include #include diff --git a/src/perception/object_detector/ioHelperh.txt b/src/perception/object_detector/ioHelper.cuh similarity index 100% rename from src/perception/object_detector/ioHelperh.txt rename to src/perception/object_detector/ioHelper.cuh From 56598963e2941c5f9f9cec7d4399b742db108a30 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Fri, 3 Nov 2023 22:40:14 -0400 Subject: [PATCH 058/197] Builds: Removed ReadProtoTensor --- src/perception/object_detector/ioHelper.cu | 33 +-------------------- src/perception/object_detector/ioHelper.cuh | 2 +- 2 files changed, 2 insertions(+), 33 deletions(-) diff --git a/src/perception/object_detector/ioHelper.cu b/src/perception/object_detector/ioHelper.cu index 85fc71309..4529dff31 100644 --- a/src/perception/object_detector/ioHelper.cu +++ b/src/perception/object_detector/ioHelper.cu @@ -4,7 +4,7 @@ #include #include #include -#include + using namespace std; namespace nvinfer1 { @@ -37,37 +37,6 @@ namespace nvinfer1 { return o; } - // returns number of floats successfully read from tensor protobuf - size_t readTensorProto(string const& path, float* buffer) { - string const data{readBuffer(path)}; - onnx::TensorProto tensorProto; - if (!tensorProto.ParseFromString(data)) - return 0; - - assert(tensorProto.has_raw_data()); - assert(tensorProto.raw_data().size() % sizeof(float) == 0); - - memcpy(buffer, tensorProto.raw_data().data(), tensorProto.raw_data().size()); - return tensorProto.raw_data().size() / sizeof(float); - } - - // returns number of floats successfully read from tensorProtoPaths - size_t readTensor(vector const& tensorProtoPaths, vector& buffer) { - GOOGLE_PROTOBUF_VERIFY_VERSION; - size_t totalElements = 0; - - for (size_t i = 0; i < tensorProtoPaths.size(); ++i) { - size_t elements = readTensorProto(tensorProtoPaths[i], &buffer[totalElements]); - if (!elements) { - cout << "ERROR: could not read tensor from file " << tensorProtoPaths[i] << endl; - break; - } - totalElements += elements; - } - - return totalElements; - } - void writeBuffer(void* buffer, size_t size, string const& path) { ofstream stream(path.c_str(), ios::binary); diff --git a/src/perception/object_detector/ioHelper.cuh b/src/perception/object_detector/ioHelper.cuh index ddacc28ed..e80316c28 100644 --- a/src/perception/object_detector/ioHelper.cuh +++ b/src/perception/object_detector/ioHelper.cuh @@ -11,7 +11,7 @@ namespace nvinfer1 { class Logger : public nvinfer1::ILogger { public: - void log(Severity severity, const char* msg) override { + void log(Severity severity, const char* msg) noexcept { std::cerr << severity << ": " << msg << std::endl; } }; From e95847966c92077e9b27d695534db102aec4eb06 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Sat, 4 Nov 2023 12:36:57 -0400 Subject: [PATCH 059/197] Inference.cu builds --- .../{Inferencecpp.txt => Inference.cpp} | 27 ++--- src/perception/object_detector/Inference.cu | 82 ++++++++++++++ .../object_detector/Inferencecu.txt | 75 ------------- src/perception/object_detector/inference.h | 103 +++++++++++++++++ src/perception/object_detector/inferenceh.txt | 105 ------------------ .../object_detector.processing.cpp | 2 + 6 files changed, 197 insertions(+), 197 deletions(-) rename src/perception/object_detector/{Inferencecpp.txt => Inference.cpp} (80%) create mode 100644 src/perception/object_detector/Inference.cu delete mode 100644 src/perception/object_detector/Inferencecu.txt create mode 100644 src/perception/object_detector/inference.h delete mode 100644 src/perception/object_detector/inferenceh.txt diff --git a/src/perception/object_detector/Inferencecpp.txt b/src/perception/object_detector/Inference.cpp similarity index 80% rename from src/perception/object_detector/Inferencecpp.txt rename to src/perception/object_detector/Inference.cpp index 2d027800c..12c88ad66 100644 --- a/src/perception/object_detector/Inferencecpp.txt +++ b/src/perception/object_detector/Inference.cpp @@ -1,21 +1,13 @@ -#include "inference.h" -#include -#include -#include -#include + #include #include -#include "ioHelper.h" - -#include "NvInfer.h" +namespace mrover { -using namespace nvinfer1; -using namespace std; - + /* // : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} -Inference::Inference(std::string onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string_view classesTxtFile = "") : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} { +Inference::Inference(std::string onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string classesTxtFile = "") : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} { createCudaEngine(onnxModelPath); @@ -59,11 +51,9 @@ std::vector Inference::doDetections(cv::Mat& img) { return Parser(this->outputTensor).parseTensor(); } +*/ -int Inference::getBindingInputIndex(nvinfer1::IExecutionContext* context) { - return context->getEngine().getTensorIOMode(context->getEngine().getIOTensorName(0)) != nvinfer1::TensorIOMode::kINPUT; // 0 (false) if bindingIsInput(0), 1 (true) otherwise -} - + /* void Inference::setUpContext() { // Create Execution Context. this->context.reset(this->enginePtr->createExecutionContext()); @@ -71,4 +61,7 @@ void Inference::setUpContext() { Dims dims_i{this->enginePtr->getBindingDimensions(0)}; Dims4 inputDims{Inference::BATCH_SIZE, dims_i.d[1], dims_i.d[2], dims_i.d[3]}; context->setBindingDimensions(0, inputDims); -} \ No newline at end of file +} +*/ + +} // namespace mrover \ No newline at end of file diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu new file mode 100644 index 000000000..a6c19bb01 --- /dev/null +++ b/src/perception/object_detector/Inference.cu @@ -0,0 +1,82 @@ +#include +#include +#include +#include + +#include "ioHelper.cuh" + +#include +#include +#include +#include + +#include "inference.h" + +#include +#include +#include + +using namespace nvinfer1; + +/** +* Example Code: @link https://github.com/NVIDIA-developer-blog/code-samples/blob/master/posts/TensorRT-introduction/simpleOnnx_1.cpp +* IExecutionContest @link https://docs.nvidia.com/deeplearning/tensorrt/api/c_api/classnvinfer1_1_1_i_execution_context.html +* ------------------------------------------------------ +* For additional context see @link https://www.edge-ai-vision.com/2020/04/speeding-up-deep-learning-inference-using-tensorrt/ +*/ + + +/** +* cudaMemcpys CPU memory in inputTensor to GPU based on bindings +* Queues that tensor to be passed through model +* cudaMemcpys the result back to CPU memory +* Requires bindings, inputTensor, stream +* Modifies stream, outputTensor +*/ +namespace mrover { + + void Inference::launchInference(float* input, float* output) { + int inputId = Inference::getBindingInputIndex(this->contextPtr.get()); + + //Copy data to GPU memory + cudaMemcpyAsync(this->bindings[inputId], input, inputEntries.d[0] * inputEntries.d[1] * inputEntries.d[2] * sizeof(float), cudaMemcpyHostToDevice, this->stream); + + // + this->contextPtr->enqueueV3(this->stream); + + //Copy data to CPU memory + cudaMemcpyAsync(output, this->bindings[1 - inputId], outputEntries.d[0] * outputEntries.d[1] * outputEntries.d[2] * sizeof(float), cudaMemcpyDeviceToHost, this->stream); + } + + /** +* Takes tensor bindings and allocates memory on the GPU for input and output tensors +* Requires enginePtr, bindings, inputTensor, and outputTensor +* Modifies bindings, inputTensor, and outputTensor +*/ + void Inference::prepTensors() { + for (int i = 0; i < this->enginePtr->getNbIOTensors(); i++) { + const char* tensorName = this->enginePtr->getIOTensorName(i); + + Dims dims{this->enginePtr->getTensorShape(tensorName)}; + + size_t size = accumulate(dims.d + 1, dims.d + dims.nbDims, Inference::BATCH_SIZE, std::multiplies<>()); + std::vector sizes = {dims.d[1], dims.d[2], dims.d[3]}; + + + // Create CUDA buffer for Tensor. + cudaMalloc(&(this->bindings)[i], Inference::BATCH_SIZE * size * sizeof(float)); + + + //Size the tensors based on tensor type + if (this->enginePtr->getTensorIOMode(this->enginePtr->getIOTensorName(i)) == nvinfer1::TensorIOMode::kINPUT) { + this->inputTensor.reshape(3, sizes); + } else if (this->enginePtr->getTensorIOMode(this->enginePtr->getIOTensorName(i)) == nvinfer1::TensorIOMode::kOUTPUT) { + this->outputTensor.resize(size); + } + } + } + + int Inference::getBindingInputIndex(nvinfer1::IExecutionContext* context) { + return context->getEngine().getTensorIOMode(context->getEngine().getIOTensorName(0)) != nvinfer1::TensorIOMode::kINPUT; // 0 (false) if bindingIsInput(0), 1 (true) otherwise + } +} // namespace mrover diff --git a/src/perception/object_detector/Inferencecu.txt b/src/perception/object_detector/Inferencecu.txt deleted file mode 100644 index 39a2754f5..000000000 --- a/src/perception/object_detector/Inferencecu.txt +++ /dev/null @@ -1,75 +0,0 @@ -#include -#include -#include -#include - -#include "ioHelper.h" - -#include -#include -#include -#include - -#include "inference.h" - -#include -#include -#include - -using namespace nvinfer1; - -/** -* Example Code: @link https://github.com/NVIDIA-developer-blog/code-samples/blob/master/posts/TensorRT-introduction/simpleOnnx_1.cpp -* IExecutionContest @link https://docs.nvidia.com/deeplearning/tensorrt/api/c_api/classnvinfer1_1_1_i_execution_context.html -* ------------------------------------------------------ -* For additional context see @link https://www.edge-ai-vision.com/2020/04/speeding-up-deep-learning-inference-using-tensorrt/ -*/ - - -/** -* cudaMemcpys CPU memory in inputTensor to GPU based on bindings -* Queues that tensor to be passed through model -* cudaMemcpys the result back to CPU memory -* Requires bindings, inputTensor, stream -* Modifies stream, outputTensor -*/ -void Inference::launchInference(float* input, float* output) { - int inputId = Inference::getBindingInputIndex(this->context); - - //Copy data to GPU memory - cudaMemcpyAsync(this->bindings[inputId], input, inputEntries.d[0] * inputEntries.d[1] * inputEntries.d[2] * sizeof(float), cudaMemcpyHostToDevice, this->stream); - - // - this->context->enqueueV3(this->stream); - - //Copy data to CPU memory - cudaMemcpyAsync(output, this->bindings[1 - inputId], outputEntries.d[0] * outputEntries.d[1] * outputEntries.d[2] * sizeof(float), cudaMemcpyDeviceToHost, this->stream); -} - -/** -* Takes tensor bindings and allocates memory on the GPU for input and output tensors -* Requires enginePtr, bindings, inputTensor, and outputTensor -* Modifies bindings, inputTensor, and outputTensor -*/ -void Inference::prepTensors() { - for (int i = 0; i < this->enginePtr->getNbIOTensors(); i++) { - const char* tensorName = this->enginePtr->getIOTensorName(i); - - Dims dims{this->enginePtr->getTensorShape(tensorName)}; - - size_t size = accumulate(dims.d + 1, dims.d + dims.nbDims, Inference::BATCH_SIZE, std::multiplies<>()); - std::vector sizes = {dims.d[1], dims.d[2], dims.d[3]}; - - - // Create CUDA buffer for Tensor. - cudaMalloc(&(this->bindings)[i], Inference::BATCH_SIZE * size * sizeof(float)); - - - //Size the tensors based on tensor type - if (this->enginePtr->getTensorIOMode(this->enginePtr->getIOTensorName(i)) == nvinfer1::TensorIOMode::kINPUT) { - this->inputTensor.reshape(3, sizes); - } else if (this->enginePtr->getTensorIOMode(this->enginePtr->getIOTensorName(i)) == nvinfer1::TensorIOMode::kOUTPUT) { - this->outputTensor.resize(size); - } - } -} diff --git a/src/perception/object_detector/inference.h b/src/perception/object_detector/inference.h new file mode 100644 index 000000000..07499817d --- /dev/null +++ b/src/perception/object_detector/inference.h @@ -0,0 +1,103 @@ +// Cpp native +#include +#include +#include + +#include +#include +#include + + +//Tensor-RT Specific +#include "cudaWrapper.cuh" +#include +#include + + +#include "ioHelper.cuh" + +// OpenCV / DNN / Inference +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//MROVER MADE FILES +#include "object_detector.hpp" +#include "pch.hpp" + +using nvinfer1::ICudaEngine; +using nvinfer1::IExecutionContext; + +// ROS Mesage -> CV View Matrix -> cv::blobFromImage -> cv::Mat (input) +// Preallocated CV::Mat of floats of correct size -> transpose into CV::Mat (also preallocated + +namespace mrover { + class Inference { + private: + static const int BATCH_SIZE = 1; + + nvinfer1::Logger logger; + + + //Ptr to the engine + std::unique_ptr> enginePtr; + + //Ptr to the context + std::unique_ptr> contextPtr; + + //Input, output and reference tensors + cv::Mat inputTensor; + cv::Mat outputTensor; + //Num Entries + nvinfer1::Dims3 inputEntries; + nvinfer1::Dims3 outputEntries; + + //Cuda Stream + cudawrapper::CudaStream stream; + + //Bindings + std::array bindings{}; + + //ONNX Model Path + std::string onnxModelPath; + + //Size of Model + cv::Size modelInputShape; + cv::Size modelOutputShape; + + private: + //STATIC FUNCTIONS + static int getBindingInputIndex(nvinfer1::IExecutionContext* context); + + public: + Inference() = default; + + Inference(std::string_view onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string_view classesTxtFile = ""); + + + std::vector runInference(cv::Mat const& input); + + private: + //Creates a ptr to the engine + void createCudaEngine(std::string& onnxModelPath); + + void launchInference(float* input, float* output); + + void setUpContext(const std::string& inputFile); //This will need to be adjusted to the img msg, but I think its ok to just run pictures as an intial test + + void prepTensors(); + + void setUpContext(); + + std::vector doDetections(const std::string& inputFile); + }; +} // namespace mrover diff --git a/src/perception/object_detector/inferenceh.txt b/src/perception/object_detector/inferenceh.txt deleted file mode 100644 index 169975209..000000000 --- a/src/perception/object_detector/inferenceh.txt +++ /dev/null @@ -1,105 +0,0 @@ -#ifndef INFERENCE_H -#define INFERENCE_H - -// Cpp native -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -//Tensor-RT Specific -#include "cudaWrapper.h" -#include -#include -#include - - -#include "ioHelper.h" - -// OpenCV / DNN / Inference -#include -#include -#include - -#include "pch.hpp" - -using nvinfer1::ICudaEngine; -using nvinfer1::IExecutionContext; - -// ROS Mesage -> CV View Matrix -> cv::blobFromImage -> cv::Mat (input) -// Preallocated CV::Mat of floats of correct size -> transpose into CV::Mat (also preallocated - -struct Detection { - int class_id{0}; - std::string className{}; - float confidence{0.0}; - cv::Scalar color{}; - cv::Rect box{}; -}; - -class Inference { -private: - static const int BATCH_SIZE = 1; - - Logger logger; - - - //Ptr to the engine - std::unique_ptr> enginePtr; - - //Ptr to the context - std::unique_ptr> context; - - //Input, output and reference tensors - cv::Mat inputTensor; - cv::Mat outputTensor; - //Num Entries - nvinfer1::Dims3 inputEntries; - nvinfer1::Dims3 outputEntries; - - //Cuda Stream - CudaStream stream; - - //Bindings - std::array bindings{}; - - //ONNX Model Path - std::string onnxModelPath; - - //Size of Model - std::modelInputShape; - cv::Size modelOutputShape; - -private: - //STATIC FUNCTIONS - static int getBindingInputIndex(nvinfer1::IExecutionContext* context); - -public: - Inference() = default; - - Inference(std::string_view onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string_view classesTxtFile = ""); - - - std::vector runInference(cv::Mat const& input); - -private: - //Creates a ptr to the engine - void createCudaEngine(std::string& onnxModelPath); - - void Inference::launchInference(float* input, float* output, int tensorSize); - - void setUpContext(const std::string& inputFile); //This will need to be adjusted to the img msg, but I think its ok to just run pictures as an intial test - - void prepTensors(); - - void Inference::setUpContext(); - - std::vector Inference::doDetections(const std::string& inputFile); -} -#endif // INFERENCE_H diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 5202310b7..800995be0 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -26,6 +26,8 @@ namespace mrover { uint x; x = msg->height; std::cout << x << std::endl; + + cv::Mat mat; } /* void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { From 6fb7fc6fe29a355c04a287ae030a5f17454bd902 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Sat, 4 Nov 2023 16:54:37 -0400 Subject: [PATCH 060/197] Pointer Errors with cv::Mat --- src/perception/object_detector/Inference.cpp | 15 +----- src/perception/object_detector/Inference.cu | 51 ++++++++++++++++++- .../object_detector/cudaWrapper.cuh | 2 +- .../{inference.h => inference.cuh} | 7 +-- 4 files changed, 55 insertions(+), 20 deletions(-) rename src/perception/object_detector/{inference.h => inference.cuh} (90%) diff --git a/src/perception/object_detector/Inference.cpp b/src/perception/object_detector/Inference.cpp index 12c88ad66..0c35a5cf1 100644 --- a/src/perception/object_detector/Inference.cpp +++ b/src/perception/object_detector/Inference.cpp @@ -1,24 +1,11 @@ - #include #include namespace mrover { - /* -// : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} -Inference::Inference(std::string onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string classesTxtFile = "") : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} { - - createCudaEngine(onnxModelPath); - - this->modelInputShape = modelInputShape; - - assert(this->enginePtr->getNbBindings() == 2); - assert(this->enginePtr->bindingIsInput(0) ^ enginePtr->bindingIsInput(1)); - - this->onnxModelPath = onnxModelPath; -} + /* // Initializes enginePtr with built engine void Inference::createCudaEngine(std::string& onnxModelPath) { diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index a6c19bb01..474617c88 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -10,7 +11,7 @@ #include #include -#include "inference.h" +#include "inference.cuh" #include #include @@ -35,13 +36,59 @@ using namespace nvinfer1; */ namespace mrover { + //Constructor + // : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} + Inference::Inference(std::string onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string classesTxtFile = "") : logger{}, inputTensor{}, outputTensor{}, stream{} { + + enginePtr.reset(createCudaEngine(onnxModelPath)); + + this->modelInputShape = modelInputShape; + + assert(this->enginePtr->getNbBindings() == 2); + assert(this->enginePtr->bindingIsInput(0) ^ enginePtr->bindingIsInput(1)); + + this->onnxModelPath = onnxModelPath; + } + + // Initializes enginePtr with built engine + nvinfer1::ICudaEngine* Inference::createCudaEngine(std::string& onnxModelPath) { + // See link sfor additional context + const auto explicitBatch = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + std::unique_ptr> builder{nvinfer1::createInferBuilder(logger)}; + std::unique_ptr> network{builder->createNetworkV2(explicitBatch)}; + std::unique_ptr> parser{nvonnxparser::createParser(*network, logger)}; + std::unique_ptr> config{builder->createBuilderConfig()}; + + //Parse the onnx from file + if (!parser->parseFromFile(onnxModelPath.c_str(), static_cast(ILogger::Severity::kINFO))) { + std::cout << "ERROR: could not parse input engine." << std::endl; + } + + config->setMemoryPoolLimit(MemoryPoolType::kWORKSPACE, 1 << 30); + + auto profile = builder->createOptimizationProfile(); + profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMIN, Dims4{1, 3, 256, 256}); + profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kOPT, Dims4{1, 3, 256, 256}); + profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMAX, Dims4{32, 3, 256, 256}); + config->addOptimizationProfile(profile); + + return builder->buildEngineWithConfig(*network, *config); + } + + std::vector Inference::doDetections(cv::Mat& img) { + //Do the forward pass on the network + launchInference(img.data, this->outputTensor.data); + + return Parser(this->outputTensor).parseTensor(); + } + void Inference::launchInference(float* input, float* output) { int inputId = Inference::getBindingInputIndex(this->contextPtr.get()); //Copy data to GPU memory cudaMemcpyAsync(this->bindings[inputId], input, inputEntries.d[0] * inputEntries.d[1] * inputEntries.d[2] * sizeof(float), cudaMemcpyHostToDevice, this->stream); - // + //Queue the async engine process this->contextPtr->enqueueV3(this->stream); //Copy data to CPU memory diff --git a/src/perception/object_detector/cudaWrapper.cuh b/src/perception/object_detector/cudaWrapper.cuh index 12591ff70..b9bb50487 100644 --- a/src/perception/object_detector/cudaWrapper.cuh +++ b/src/perception/object_detector/cudaWrapper.cuh @@ -27,7 +27,7 @@ #ifndef __CUDA_WRAPPER_H__ #define __CUDA_WRAPPER_H__ -#include +#include namespace cudawrapper { diff --git a/src/perception/object_detector/inference.h b/src/perception/object_detector/inference.cuh similarity index 90% rename from src/perception/object_detector/inference.h rename to src/perception/object_detector/inference.cuh index 07499817d..e3cc2d377 100644 --- a/src/perception/object_detector/inference.h +++ b/src/perception/object_detector/inference.cuh @@ -1,4 +1,5 @@ // Cpp native +#include #include #include #include @@ -81,14 +82,14 @@ namespace mrover { public: Inference() = default; - Inference(std::string_view onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string_view classesTxtFile = ""); + Inference(std::string onnxModelPath, cv::Size modelInputShape, std::string classesTxtFile); std::vector runInference(cv::Mat const& input); private: //Creates a ptr to the engine - void createCudaEngine(std::string& onnxModelPath); + nvinfer1::ICudaEngine* createCudaEngine(std::string& onnxModelPath); void launchInference(float* input, float* output); @@ -98,6 +99,6 @@ namespace mrover { void setUpContext(); - std::vector doDetections(const std::string& inputFile); + std::vector doDetections(cv::Mat& img); }; } // namespace mrover From d235b146856fabba7ba72edf5b716d244fb9663e Mon Sep 17 00:00:00 2001 From: jbrhm Date: Sun, 5 Nov 2023 15:29:34 -0500 Subject: [PATCH 061/197] Added NVINFER && NVONNXPARSER to CMake --- CMakeLists.txt | 2 +- src/perception/object_detector/Inference.cu | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5912b3f58..918a7a5ef 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -215,7 +215,7 @@ mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_a if (ZED_FOUND) mrover_add_nodelet(object_detector src/perception/object_detector/*.c* src/perception/object_detector src/perception/object_detector/pch.hpp) - mrover_nodelet_link_libraries(object_detector PRIVATE opencv_core opencv_dnn opencv_imgproc lie) + mrover_nodelet_link_libraries(object_detector PRIVATE opencv_core opencv_dnn opencv_imgproc lie nvinfer nvonnxparser) # Temporary mrover_nodelet_link_libraries(object_detector PRIVATE opencv_imgcodecs) mrover_nodelet_link_libraries(object_detector PRIVATE opencv_highgui) diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index 474617c88..d3ec838ff 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -74,7 +74,7 @@ namespace mrover { return builder->buildEngineWithConfig(*network, *config); } - + /* std::vector Inference::doDetections(cv::Mat& img) { //Do the forward pass on the network launchInference(img.data, this->outputTensor.data); @@ -94,6 +94,7 @@ namespace mrover { //Copy data to CPU memory cudaMemcpyAsync(output, this->bindings[1 - inputId], outputEntries.d[0] * outputEntries.d[1] * outputEntries.d[2] * sizeof(float), cudaMemcpyDeviceToHost, this->stream); } + */ /** * Takes tensor bindings and allocates memory on the GPU for input and output tensors From 28fc946260a7349417c07992438bf57f6164bdb8 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Mon, 6 Nov 2023 11:13:25 -0500 Subject: [PATCH 062/197] fixed segfaults, image wrong format --- .../long_range_tag_detector.cpp | 3 +- .../long_range_tag_detector.processing.cpp | 55 +++++++++++-------- 2 files changed, 34 insertions(+), 24 deletions(-) diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp index 03252cb8b..b77195acb 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp @@ -8,7 +8,7 @@ namespace mrover { mDetectorParams = new cv::aruco::DetectorParameters(); auto defaultDetectorParams = cv::makePtr(); int dictionaryNumber; - + mPnh.param("publish_images", mPublishImages, true); mPnh.param("min_hit_count_before_publish", mMinHitCountBeforePublish, 5); mPnh.param("max_hit_count", mMaxHitCount, 5); @@ -18,6 +18,7 @@ namespace mrover { mImgPub = mNh.advertise("long_range_tag_detection", 1); mImgSub = mNh.subscribe("long_range_cam/image", 1, &LongRangeTagDetectorNodelet::imageCallback, this); + mDictionary = cv::makePtr(cv::aruco::getPredefinedDictionary(dictionaryNumber)); mServiceEnableDetections = mNh.advertiseService("enable_detections", &LongRangeTagDetectorNodelet::enableDetectionsCallback, this); // Lambda handles passing class pointer (implicit first parameter) to configCallback diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp index 9b9bb2ef3..b80c02750 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp @@ -28,7 +28,7 @@ namespace mrover { } //Publish all tags that meet threshold - publishThresholdTags(); + // publishThresholdTags(); //PUBLISH TAGS } @@ -40,16 +40,16 @@ namespace mrover { assert(msg->height > 0); assert(msg->width > 0); //Store image message to mImgMsg member variable - mImgMsg = *msg; - mImg = cv::Mat{static_cast(msg->height), static_cast(msg->width), CV_8UC4, const_cast(msg->data.data())}; + cv::Mat cvImage = cv::Mat{static_cast(msg->height), static_cast(msg->width), CV_8UC3, const_cast(msg->data.data())}; //Store to mImg member variable - unsure if necessary but need it to persist - // cvImage.copyTo(mImg); + cvImage.copyTo(mImg); // TODO: Set the grayImage if neccesary } void LongRangeTagDetectorNodelet::runTagDetection() { + std::cout << mDetectorParams->adaptiveThreshConstant << std::endl; cv::aruco::detectMarkers(mImg, mDictionary, mImmediateCorners, mImmediateIds, mDetectorParams); } @@ -141,7 +141,7 @@ namespace mrover { return centerPoint; } - cv::Poinjt2f LongRangeTagDetectorNodelet::getNormedTagCenterOffset(std::vector& tagCorners) const { + cv::Point2f LongRangeTagDetectorNodelet::getNormedTagCenterOffset(std::vector& tagCorners) const { cv::Point2f offsetCenterPoint = getTagCenterOffsetPixels(tagCorners); offsetCenterPoint.x /= static_cast(mImgMsg.width); @@ -151,15 +151,16 @@ namespace mrover { } float LongRangeTagDetectorNodelet::getTagBearing(std::vector& tagCorners) const { - //TODO: FINISH - //for HD720 resolution - pair center = getTagCenterPixels(tagCorners); - float x = center.first; - int width = //get width of the image - int angleOfFOV = 104; - float bearing = ((x - width / 2) / width) * angleOfFOV; - - return bearing; + // //TODO: FINISH + // //for HD720 resolution + // pair center = getTagCenterPixels(tagCorners); + // float x = center.first; + // int width = //get width of the image + // int angleOfFOV = 104; + // float bearing = ((x - width / 2) / width) * angleOfFOV; + + // return bearing; + return {}; } void LongRangeTagDetectorNodelet::publishThresholdTags() { @@ -187,15 +188,23 @@ namespace mrover { } void LongRangeTagDetectorNodelet::publishTagsOnImage() { - cv::Mat markedImage; - mImg.copyTo(markedImage); - - cv::aruco::drawDetectedMarkers(markedImage, mImmediateCorners); - - sensor_msgs::Image imgMsgOut = mImgMsg; - imgMsgOut.data = markedImage; - - mImgPub.publish(imgMsgOut); + // cv::Mat markedImage; + // markedImage.copyTo(mImg); + // std::cout << markedImage.total() << ", " << markedImage.channels() << std::endl; + + cv::aruco::drawDetectedMarkers(mImg, mImmediateCorners, mImmediateIds); + mImgMsg.header.seq = mSeqNum; + mImgMsg.header.stamp = ros::Time::now(); + mImgMsg.header.frame_id = "long_range_cam_frame"; + mImgMsg.height = mImg.rows; + mImgMsg.width = mImg.cols; + mImgMsg.encoding = sensor_msgs::image_encodings::BGR8; + mImgMsg.step = mImg.step; + mImgMsg.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; + size_t size = mImgMsg.step * mImgMsg.height; + mImgMsg.data.resize(size); + std::uninitialized_copy(std::execution::par_unseq, mImg.data, mImg.data + size, mImgMsg.data.begin()); + mImgPub.publish(mImgMsg); } } // namespace mrover \ No newline at end of file From 0a4db393b61f35ecbeb92571b5e72e0d7942ef16 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Mon, 6 Nov 2023 11:30:13 -0500 Subject: [PATCH 063/197] tag detector works --- src/perception/long_range_cam/long_range_cam.cpp | 4 ++-- .../long_range_cam/long_range_tag_detector.processing.cpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/perception/long_range_cam/long_range_cam.cpp b/src/perception/long_range_cam/long_range_cam.cpp index 8720f9657..8b7fde3eb 100644 --- a/src/perception/long_range_cam/long_range_cam.cpp +++ b/src/perception/long_range_cam/long_range_cam.cpp @@ -32,7 +32,7 @@ namespace mrover { msg->height = bgra.rows; msg->width = bgra.cols; - msg->encoding = sensor_msgs::image_encodings::BGRA8; + msg->encoding = sensor_msgs::image_encodings::BGR8; msg->step = bgra.step[0]; msg->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; auto* imgPtr = bgra.ptr(0); @@ -66,7 +66,7 @@ namespace mrover { if (mImgPub.getNumSubscribers()) { auto imgMsg = boost::make_shared(); cv::Mat bgra; - cv::cvtColor(frame, bgra, cv::COLOR_YUV2BGRA_I420); + cv::cvtColor(frame, bgra, cv::COLOR_YUV2BGR_I420); fillImageMessage(bgra, imgMsg); imgMsg->header.frame_id = "long_range_cam_frame"; imgMsg->header.stamp = ros::Time::now(); diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp index b80c02750..fd582b3ab 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp @@ -24,7 +24,7 @@ namespace mrover { // 3. We only want to publish the tags if the topic has subscribers if (mPublishImages && mImgPub.getNumSubscribers()) { // Draw the tags on the image using OpenCV - publishTagsOnImage(); + // publishTagsOnImage(); } //Publish all tags that meet threshold @@ -45,6 +45,7 @@ namespace mrover { //Store to mImg member variable - unsure if necessary but need it to persist cvImage.copyTo(mImg); + publishTagsOnImage(); // TODO: Set the grayImage if neccesary } From 6dc1e10d8b55a79e1d883aa74e7ca357c72aebfe Mon Sep 17 00:00:00 2001 From: jbrhm Date: Mon, 6 Nov 2023 20:25:24 -0500 Subject: [PATCH 064/197] Everything compiles --- CMakeLists.txt | 1 + src/perception/object_detector/Inference.cu | 20 ++++++------------- src/perception/object_detector/inference.cuh | 5 +++-- .../object_detector/object_detector.hpp | 1 + .../object_detector.processing.cpp | 12 +++++++---- 5 files changed, 19 insertions(+), 20 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 918a7a5ef..92deb8630 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -144,6 +144,7 @@ find_package(OpenCV REQUIRED) find_package(ZED 2 QUIET) find_package(gazebo REQUIRED) find_package(Eigen3 REQUIRED) + if (ZED_FOUND) # Anything newer than C++17 combined with libstdc++13 is not supported just yet by NVCC (the CUDA compiler) set(CMAKE_CUDA_STANDARD 17) diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index d3ec838ff..d3cc41981 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -72,17 +72,17 @@ namespace mrover { profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMAX, Dims4{32, 3, 256, 256}); config->addOptimizationProfile(profile); - return builder->buildEngineWithConfig(*network, *config); + return nullptr; //builder->buildEngineWithConfig(*network, *config); } - /* - std::vector Inference::doDetections(cv::Mat& img) { + + void Inference::doDetections(cv::Mat& img) { //Do the forward pass on the network launchInference(img.data, this->outputTensor.data); - return Parser(this->outputTensor).parseTensor(); + //return Parser(this->outputTensor).parseTensor(); } - void Inference::launchInference(float* input, float* output) { + void Inference::launchInference(void* input, void* output) { int inputId = Inference::getBindingInputIndex(this->contextPtr.get()); //Copy data to GPU memory @@ -94,7 +94,7 @@ namespace mrover { //Copy data to CPU memory cudaMemcpyAsync(output, this->bindings[1 - inputId], outputEntries.d[0] * outputEntries.d[1] * outputEntries.d[2] * sizeof(float), cudaMemcpyDeviceToHost, this->stream); } - */ + /** * Takes tensor bindings and allocates memory on the GPU for input and output tensors @@ -113,14 +113,6 @@ namespace mrover { // Create CUDA buffer for Tensor. cudaMalloc(&(this->bindings)[i], Inference::BATCH_SIZE * size * sizeof(float)); - - - //Size the tensors based on tensor type - if (this->enginePtr->getTensorIOMode(this->enginePtr->getIOTensorName(i)) == nvinfer1::TensorIOMode::kINPUT) { - this->inputTensor.reshape(3, sizes); - } else if (this->enginePtr->getTensorIOMode(this->enginePtr->getIOTensorName(i)) == nvinfer1::TensorIOMode::kOUTPUT) { - this->outputTensor.resize(size); - } } } diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index e3cc2d377..f69342974 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -1,4 +1,5 @@ // Cpp native +#pragma once #include #include #include @@ -91,7 +92,7 @@ namespace mrover { //Creates a ptr to the engine nvinfer1::ICudaEngine* createCudaEngine(std::string& onnxModelPath); - void launchInference(float* input, float* output); + void launchInference(void* input, void* output); void setUpContext(const std::string& inputFile); //This will need to be adjusted to the img msg, but I think its ok to just run pictures as an intial test @@ -99,6 +100,6 @@ namespace mrover { void setUpContext(); - std::vector doDetections(cv::Mat& img); + void doDetections(cv::Mat& img); }; } // namespace mrover diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index f24e66169..8d356a417 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -23,6 +23,7 @@ namespace mrover { //Inference inference; + // Publishers ros::Publisher mDebugImgPub; diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 800995be0..87960d938 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -23,11 +23,15 @@ namespace mrover { void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { - uint x; - x = msg->height; - std::cout << x << std::endl; + assert(msg); + assert(msg->height > 0); + assert(msg->width > 0); + + cv::Mat imageView{static_cast(msg->height), static_cast(msg->width), CV_8UC4, const_cast(msg->data.data())}; - cv::Mat mat; + std::cout << "BRUHH" << std::endl; + cv::imshow("dumb", imageView); + cv::waitKey(100); } /* void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { From dc248627c5e7563cbc4186a08b618fab90ced498 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Tue, 7 Nov 2023 10:41:57 -0500 Subject: [PATCH 065/197] cuda_runtime_api.h' file not found --- src/perception/object_detector/object_detector.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 8d356a417..48b7bacb7 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -1,4 +1,4 @@ -//#include "inference.h" +#include "inference.cuh" #include "pch.hpp" #include #include From 2dba81cc7d9b5760d289577fb831579835f23f23 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Wed, 8 Nov 2023 14:29:13 -0500 Subject: [PATCH 066/197] CUDA COMPILING ::::::)))))))) --- src/perception/object_detector/Inference.cu | 3 +- src/perception/object_detector/inference.cuh | 1 + .../object_detector/inferenceWrapper.cu | 47 +++++++++++++++++++ .../object_detector/inferenceWrapper.hpp | 21 +++++++++ .../object_detector/object_detector.hpp | 2 +- 5 files changed, 72 insertions(+), 2 deletions(-) create mode 100644 src/perception/object_detector/inferenceWrapper.cu create mode 100644 src/perception/object_detector/inferenceWrapper.hpp diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index d3cc41981..e5b0a5836 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -72,7 +72,8 @@ namespace mrover { profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMAX, Dims4{32, 3, 256, 256}); config->addOptimizationProfile(profile); - return nullptr; //builder->buildEngineWithConfig(*network, *config); + return nullptr; + builder->buildEngineWithConfig(*network, *config); } void Inference::doDetections(cv::Mat& img) { diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index f69342974..c3429cd5f 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -100,6 +100,7 @@ namespace mrover { void setUpContext(); + public: void doDetections(cv::Mat& img); }; } // namespace mrover diff --git a/src/perception/object_detector/inferenceWrapper.cu b/src/perception/object_detector/inferenceWrapper.cu new file mode 100644 index 000000000..80f857281 --- /dev/null +++ b/src/perception/object_detector/inferenceWrapper.cu @@ -0,0 +1,47 @@ +#include +#include +#include +#include +#include + +#include "ioHelper.cuh" + +#include +#include +#include +#include + +#include "inference.cuh" +#include "inferenceWrapper.hpp" + +#include +#include +#include + +using namespace nvinfer1; + +/** +* Example Code: @link https://github.com/NVIDIA-developer-blog/code-samples/blob/master/posts/TensorRT-introduction/simpleOnnx_1.cpp +* IExecutionContest @link https://docs.nvidia.com/deeplearning/tensorrt/api/c_api/classnvinfer1_1_1_i_execution_context.html +* ------------------------------------------------------ +* For additional context see @link https://www.edge-ai-vision.com/2020/04/speeding-up-deep-learning-inference-using-tensorrt/ +*/ + + +/** +* cudaMemcpys CPU memory in inputTensor to GPU based on bindings +* Queues that tensor to be passed through model +* cudaMemcpys the result back to CPU memory +* Requires bindings, inputTensor, stream +* Modifies stream, outputTensor +*/ +namespace mrover { + //Initialize the unique_ptr to the inference class + InferenceWrapper::InferenceWrapper(std::string onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string classesTxtFile = "") { + inferencePtr.reset(new Inference(onnxModelPath, modelInputShape, classesTxtFile)); + } + + void InferenceWrapper::doDetections(cv::Mat& img) { + inferencePtr->doDetections(img); + } +} // namespace mrover diff --git a/src/perception/object_detector/inferenceWrapper.hpp b/src/perception/object_detector/inferenceWrapper.hpp new file mode 100644 index 000000000..8ee720d34 --- /dev/null +++ b/src/perception/object_detector/inferenceWrapper.hpp @@ -0,0 +1,21 @@ +#pragma once +#include "pch.hpp" +#include +#include +#include + +namespace mrover { + class Inference; + + class InferenceWrapper { + private: + //The pointer to the inference class + std::unique_ptr inferencePtr; + + public: + //The constructor for the inferencewrapper + InferenceWrapper(std::string onnxModelPath, cv::Size modelInputShape, std::string classesTxtFile); + + void doDetections(cv::Mat& img); + }; +} // namespace mrover \ No newline at end of file diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 48b7bacb7..d194241eb 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -1,4 +1,4 @@ -#include "inference.cuh" +#include "inferenceWrapper.hpp" #include "pch.hpp" #include #include From 1f90062110a09ce5b2f1d837d0a806299dccfb10 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Wed, 8 Nov 2023 17:39:46 -0500 Subject: [PATCH 067/197] Code Compiles with Wrapper Constructor called --- src/perception/object_detector/Inference.cu | 2 ++ src/perception/object_detector/inference.cuh | 2 -- src/perception/object_detector/inferenceWrapper.hpp | 6 +++++- src/perception/object_detector/object_detector.cpp | 3 ++- src/perception/object_detector/object_detector.hpp | 2 +- .../object_detector/object_detector.processing.cpp | 7 +------ 6 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index e5b0a5836..30482ffa2 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -48,6 +48,8 @@ namespace mrover { assert(this->enginePtr->bindingIsInput(0) ^ enginePtr->bindingIsInput(1)); this->onnxModelPath = onnxModelPath; + + prepTensors(); } // Initializes enginePtr with built engine diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index c3429cd5f..5e3d62c32 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -81,8 +81,6 @@ namespace mrover { static int getBindingInputIndex(nvinfer1::IExecutionContext* context); public: - Inference() = default; - Inference(std::string onnxModelPath, cv::Size modelInputShape, std::string classesTxtFile); diff --git a/src/perception/object_detector/inferenceWrapper.hpp b/src/perception/object_detector/inferenceWrapper.hpp index 8ee720d34..45b081874 100644 --- a/src/perception/object_detector/inferenceWrapper.hpp +++ b/src/perception/object_detector/inferenceWrapper.hpp @@ -10,9 +10,13 @@ namespace mrover { class InferenceWrapper { private: //The pointer to the inference class - std::unique_ptr inferencePtr; + std::shared_ptr inferencePtr; public: + InferenceWrapper() = default; + + ~InferenceWrapper() = default; + //The constructor for the inferencewrapper InferenceWrapper(std::string onnxModelPath, cv::Size modelInputShape, std::string classesTxtFile); diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 2df9f4ce1..3d4998110 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -1,4 +1,5 @@ #include "object_detector.hpp" +#include "inferenceWrapper.hpp" //#include "inference.h" #include @@ -17,7 +18,7 @@ namespace mrover { void ObjectDetectorNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - + inferenceWrapper = InferenceWrapper("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //read ONNX file into this mNet, YOLOV8, second smallest one // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index d194241eb..b5b68a690 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -22,7 +22,7 @@ namespace mrover { ros::NodeHandle mNh, mPnh; //Inference inference; - + InferenceWrapper inferenceWrapper; // Publishers diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 87960d938..088744e87 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -1,6 +1,5 @@ #include "object_detector.hpp" -//#include "inference.h" - +#include "inferenceWrapper.hpp" #include #include #include @@ -28,10 +27,6 @@ namespace mrover { assert(msg->width > 0); cv::Mat imageView{static_cast(msg->height), static_cast(msg->width), CV_8UC4, const_cast(msg->data.data())}; - - std::cout << "BRUHH" << std::endl; - cv::imshow("dumb", imageView); - cv::waitKey(100); } /* void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { From 13c10d4b1c596afb5ccfa3bf9d389157310e53a4 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Wed, 8 Nov 2023 18:27:42 -0500 Subject: [PATCH 068/197] No More Seg Fault --- CMakeLists.txt | 4 ++++ src/perception/object_detector/Inference.cu | 13 ++++++++----- src/perception/object_detector/inference.cuh | 2 +- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 92deb8630..754b23f47 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -220,6 +220,9 @@ if (ZED_FOUND) # Temporary mrover_nodelet_link_libraries(object_detector PRIVATE opencv_imgcodecs) mrover_nodelet_link_libraries(object_detector PRIVATE opencv_highgui) + mrover_nodelet_defines(object_detector + __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__ # Eigen includes some files it should not, ignore + ) mrover_add_nodelet(zed src/perception/zed_wrapper/*.c* src/perception/zed_wrapper src/perception/zed_wrapper/pch.hpp) mrover_nodelet_include_directories(zed ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) @@ -227,6 +230,7 @@ if (ZED_FOUND) mrover_nodelet_defines(zed ALLOW_BUILD_DEBUG # Ignore ZED warnings about Debug mode __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__ # Eigen includes some files it should not, ignore + ) endif () diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index 30482ffa2..b983f14ca 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -40,7 +40,10 @@ namespace mrover { // : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} Inference::Inference(std::string onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string classesTxtFile = "") : logger{}, inputTensor{}, outputTensor{}, stream{} { - enginePtr.reset(createCudaEngine(onnxModelPath)); + enginePtr = std::unique_ptr>{createCudaEngine(onnxModelPath)}; + if (!enginePtr) { + throw std::runtime_error("Failed to create CUAD engine"); + } this->modelInputShape = modelInputShape; @@ -53,7 +56,7 @@ namespace mrover { } // Initializes enginePtr with built engine - nvinfer1::ICudaEngine* Inference::createCudaEngine(std::string& onnxModelPath) { + ICudaEngine* Inference::createCudaEngine(std::string& onnxModelPath) { // See link sfor additional context const auto explicitBatch = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); std::unique_ptr> builder{nvinfer1::createInferBuilder(logger)}; @@ -73,9 +76,9 @@ namespace mrover { profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kOPT, Dims4{1, 3, 256, 256}); profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMAX, Dims4{32, 3, 256, 256}); config->addOptimizationProfile(profile); - - return nullptr; - builder->buildEngineWithConfig(*network, *config); + nvinfer1::IHostMemory* serializedEngine = builder->buildSerializedNetwork(*network, *config); + nvinfer1::IRuntime* runtime = createInferRuntime(logger); + return runtime->deserializeCudaEngine(serializedEngine->data(), serializedEngine->size()); } void Inference::doDetections(cv::Mat& img) { diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index 5e3d62c32..685e0fd77 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -88,7 +88,7 @@ namespace mrover { private: //Creates a ptr to the engine - nvinfer1::ICudaEngine* createCudaEngine(std::string& onnxModelPath); + ICudaEngine* createCudaEngine(std::string& onnxModelPath); void launchInference(void* input, void* output); From f214adddbf6a9bacba08ceb92cb82e4726c312d0 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Wed, 8 Nov 2023 19:17:11 -0500 Subject: [PATCH 069/197] Something is up with running the model --- src/perception/object_detector/Inference.cu | 9 ++- .../object_detector.processing.cpp | 4 + src/perception/object_detector/parser.cpp.txt | 81 +++++++++++++++++++ src/perception/object_detector/parser.hpp | 41 ++++++++++ src/perception/object_detector/parsercpp.txt | 78 ------------------ src/perception/object_detector/parserhpp.txt | 39 --------- 6 files changed, 134 insertions(+), 118 deletions(-) create mode 100644 src/perception/object_detector/parser.cpp.txt create mode 100644 src/perception/object_detector/parser.hpp delete mode 100644 src/perception/object_detector/parsercpp.txt delete mode 100644 src/perception/object_detector/parserhpp.txt diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index b983f14ca..b5459c09c 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -9,6 +9,8 @@ #include #include #include +#include +#include #include #include "inference.cuh" @@ -83,8 +85,9 @@ namespace mrover { void Inference::doDetections(cv::Mat& img) { //Do the forward pass on the network + std::cout << "HI" << std::endl; launchInference(img.data, this->outputTensor.data); - + std::cout << *(this->outputTensor.data) << std::endl; //return Parser(this->outputTensor).parseTensor(); } @@ -120,6 +123,10 @@ namespace mrover { // Create CUDA buffer for Tensor. cudaMalloc(&(this->bindings)[i], Inference::BATCH_SIZE * size * sizeof(float)); } + + inputEntries = nvinfer1::Dims3(modelInputShape.width, modelInputShape.height, 3); + inputEntries = nvinfer1::Dims3(1, 84, 8400); + cv::resize(outputTensor, outputTensor, cv::Size(84, 840)); } int Inference::getBindingInputIndex(nvinfer1::IExecutionContext* context) { diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 088744e87..32b23fe06 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -27,6 +27,10 @@ namespace mrover { assert(msg->width > 0); cv::Mat imageView{static_cast(msg->height), static_cast(msg->width), CV_8UC4, const_cast(msg->data.data())}; + cv::Mat sizedImage; + cv::resize(imageView, sizedImage, cv::Size(640, 640)); + cv::Mat blob = cv::dnn::blobFromImage(sizedImage); + inferenceWrapper.doDetections(sizedImage); } /* void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { diff --git a/src/perception/object_detector/parser.cpp.txt b/src/perception/object_detector/parser.cpp.txt new file mode 100644 index 000000000..18129d2b4 --- /dev/null +++ b/src/perception/object_detector/parser.cpp.txt @@ -0,0 +1,81 @@ +#include "parser.hpp" +namespace mrover { + + + Parser::Parser(std::vector& tensor) { + this->tensor = tensor; + } + + std::vector Parser::parseTensor(cv::Mat& outputTensor) { + int cols = 8400; //Number of outputs in model + int dimension = 84; //80 + 4 + int numClasses = 80; //The number of possible classes + + //Output vector of detections + std::vector detections; + + //Intermediary vectors of data + std::vector class_ids; + std::vector confidences; + std::vector boxes; + + for (int x = 0; x < cols; x++) { + int ptr = x; + + //Create box + float l = tensor[ptr]; + ptr += cols; + float h = tensor[ptr]; + ptr += cols; + float width = tensor[ptr]; + ptr += cols; + float height = tensor[ptr]; + + int left = int((l - 0.5 * width)); + int top = int((h - 0.5 * height)); + + + boxes.push_back(cv::Rect(left, top, static_cast(width), static_cast(height))); + + //Find class id and confidence + int maxIndex = 0; + float maxConfidence = 0; + + for (int i = 0; i < numClasses; i++) { + ptr += cols; + if (tensor[ptr] > maxConfidence) { + maxConfidence = tensor[ptr]; + maxIndex = i; + } + } + + confidences.push_back(maxConfidence); + class_ids.push_back(maxIndex); + } + + std::vector nms_result; + cv::dnn::NMSBoxes(boxes, confidences, modelScoreThreshold, modelNMSThreshold, nms_result); + + for (unsigned long i = 0; i < nms_result.size(); ++i) { + int idx = nms_result[i]; + + Detection result; + result.class_id = class_ids[idx]; + result.confidence = confidences[idx]; + + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution dis(100, 255); + result.color = cv::Scalar(dis(gen), + dis(gen), + dis(gen)); + + result.className = classes[result.class_id]; + result.box = boxes[idx]; + + detections.push_back(result); + } + + return detections; + } +} // namespace mrover \ No newline at end of file diff --git a/src/perception/object_detector/parser.hpp b/src/perception/object_detector/parser.hpp new file mode 100644 index 000000000..0ea8d74cc --- /dev/null +++ b/src/perception/object_detector/parser.hpp @@ -0,0 +1,41 @@ +#include "object_detector.hpp" +#ifndef PARSER_H +#define PARSER_H + +//OPEN CV +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//STD +#include + +namespace mrover { + + + class Parser { + private: + std::vector tensor; + + //MAT VALS + int width; + + //NET PARAMS + float modelScoreThreshold = 0.5; + float modelNMSThreshold = 0.50; + std::vector classes{"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; + + public: + Parser(std::vector& tensor); + + std::vector parseTensor(cv::Mat& outputTensor); + }; +} // namespace mrover +#endif \ No newline at end of file diff --git a/src/perception/object_detector/parsercpp.txt b/src/perception/object_detector/parsercpp.txt deleted file mode 100644 index 6945aabf9..000000000 --- a/src/perception/object_detector/parsercpp.txt +++ /dev/null @@ -1,78 +0,0 @@ -#include "parser.h" - -Parser::Parser(std::vector& tensor) { - this->tensor = tensor; -} - -Parser::parseTensor() { - int cols = 8400; //Number of outputs in model - int dimension = 84; //80 + 4 - int numClasses = 80; //The number of possible classes - - //Output vector of detections - std::vector detections; - - //Intermediary vectors of data - std::vector class_ids; - std::vector confidences; - std::vector boxes; - - for (int x = 0; x < cols; x++) { - int ptr = x; - - //Create box - float l = tensor[ptr]; - ptr += cols; - float h = tensor[ptr]; - ptr += cols; - float width = tensor[ptr]; - ptr += cols; - float height = tensor[ptr]; - - int left = int((l - 0.5 * width)); - int top = int((h - 0.5 * height)); - - - boxes.push_back(cv::Rect(left, top, static_cast(width), static_cast(height))); - - //Find class id and confidence - int maxIndex = 0; - float maxConfidence = 0; - - for (int i = 0; i < numClasses; i++) { - ptr += cols; - if (tensor[ptr] > maxConfidence) { - maxConfidence = tensor[ptr]; - maxIndex = i; - } - } - - confidences.push_back(maxConfidence); - class_ids.push_back(maxIndex); - } - - std::vector nms_result; - cv::dnn::NMSBoxes(boxes, confidences, modelScoreThreshold, modelNMSThreshold, nms_result); - - for (unsigned long i = 0; i < nms_result.size(); ++i) { - int idx = nms_result[i]; - - Detection result; - result.class_id = class_ids[idx]; - result.confidence = confidences[idx]; - - std::random_device rd; - std::mt19937 gen(rd()); - std::uniform_int_distribution dis(100, 255); - result.color = cv::Scalar(dis(gen), - dis(gen), - dis(gen)); - - result.className = classes[result.class_id]; - result.box = boxes[idx]; - - detections.push_back(result); - } - - return detections; -} \ No newline at end of file diff --git a/src/perception/object_detector/parserhpp.txt b/src/perception/object_detector/parserhpp.txt deleted file mode 100644 index dfd980283..000000000 --- a/src/perception/object_detector/parserhpp.txt +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef PARSER_H -#define PARSER_H - -//OPEN CV -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -//STD -#include - -#include "inference.h" - -class Parser { -private: - std::vector tensor; - - //MAT VALS - int width; - - //NET PARAMS - float modelScoreThreshold = 0.5; - float modelNMSThreshold = 0.50; - std::vector classes{"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; - -public: - Parser(std::vector& tensor); - - std::vector parseTensor(); -} - -#endif \ No newline at end of file From 383760a70b2dd115ba102abe669c7573147f94d1 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Thu, 9 Nov 2023 18:51:57 -0500 Subject: [PATCH 070/197] SEG FAULT :)))))) --- src/perception/object_detector/Inference.cu | 3 ++- src/perception/object_detector/object_detector.cpp | 2 ++ src/perception/object_detector/object_detector.processing.cpp | 1 + 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index b5459c09c..c1951dad3 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -80,12 +80,13 @@ namespace mrover { config->addOptimizationProfile(profile); nvinfer1::IHostMemory* serializedEngine = builder->buildSerializedNetwork(*network, *config); nvinfer1::IRuntime* runtime = createInferRuntime(logger); + std::cout << (serializedEngine->data()) << std::endl; return runtime->deserializeCudaEngine(serializedEngine->data(), serializedEngine->size()); } void Inference::doDetections(cv::Mat& img) { //Do the forward pass on the network - std::cout << "HI" << std::endl; + ROS_INFO("HI"); launchInference(img.data, this->outputTensor.data); std::cout << *(this->outputTensor.data) << std::endl; //return Parser(this->outputTensor).parseTensor(); diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 3d4998110..588e65077 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -19,6 +19,8 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); inferenceWrapper = InferenceWrapper("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); + ROS_INFO("hi"); + //inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //read ONNX file into this mNet, YOLOV8, second smallest one // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 32b23fe06..1387010fd 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -30,6 +30,7 @@ namespace mrover { cv::Mat sizedImage; cv::resize(imageView, sizedImage, cv::Size(640, 640)); cv::Mat blob = cv::dnn::blobFromImage(sizedImage); + ROS_INFO("hi"); inferenceWrapper.doDetections(sizedImage); } /* From 1843aa91eecc9e024232a0712ffc30cbbd203a38 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Thu, 9 Nov 2023 19:47:18 -0500 Subject: [PATCH 071/197] sadness --- src/perception/object_detector/Inference.cu | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index c1951dad3..7af688cad 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -59,6 +59,7 @@ namespace mrover { // Initializes enginePtr with built engine ICudaEngine* Inference::createCudaEngine(std::string& onnxModelPath) { + logger = Logger(); // See link sfor additional context const auto explicitBatch = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); std::unique_ptr> builder{nvinfer1::createInferBuilder(logger)}; @@ -78,9 +79,9 @@ namespace mrover { profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kOPT, Dims4{1, 3, 256, 256}); profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMAX, Dims4{32, 3, 256, 256}); config->addOptimizationProfile(profile); + std::cout << network.get() << std::endl; nvinfer1::IHostMemory* serializedEngine = builder->buildSerializedNetwork(*network, *config); nvinfer1::IRuntime* runtime = createInferRuntime(logger); - std::cout << (serializedEngine->data()) << std::endl; return runtime->deserializeCudaEngine(serializedEngine->data(), serializedEngine->size()); } From cccf7b0249d7d36387772da3410d144185eddb7b Mon Sep 17 00:00:00 2001 From: lorraine Date: Sun, 12 Nov 2023 13:59:15 -0500 Subject: [PATCH 072/197] builds correctly now --- .../long_range_tag_detector.processing.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp index fd582b3ab..f42653e6c 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp @@ -152,20 +152,17 @@ namespace mrover { } float LongRangeTagDetectorNodelet::getTagBearing(std::vector& tagCorners) const { - // //TODO: FINISH - // //for HD720 resolution - // pair center = getTagCenterPixels(tagCorners); - // float x = center.first; - // int width = //get width of the image - // int angleOfFOV = 104; - // float bearing = ((x - width / 2) / width) * angleOfFOV; - - // return bearing; - return {}; + //for HD720 resolution + cv::Point2f center = getTagCenterPixels(tagCorners); + float width = 2 * center.x; + float angleOfFOV = 104; + float bearing = ((center.x - width / 2) / width) * angleOfFOV; + + return bearing; } void LongRangeTagDetectorNodelet::publishThresholdTags() { - //Loop through all the tags + //Loop through all the tagsj LongRangeTags tagsMessage; // for (auto& tag: mTags) { From 23758747d728041b96e2865556204c0f073436e3 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Tue, 14 Nov 2023 18:19:56 -0500 Subject: [PATCH 073/197] add launch --- launch/long_range_cam.launch | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 launch/long_range_cam.launch diff --git a/launch/long_range_cam.launch b/launch/long_range_cam.launch new file mode 100644 index 000000000..be21fde31 --- /dev/null +++ b/launch/long_range_cam.launch @@ -0,0 +1,19 @@ + + + + + + + + + \ No newline at end of file From 56c6f39ae262727c988a9a259173607c886aa920 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Tue, 14 Nov 2023 20:12:32 -0500 Subject: [PATCH 074/197] Create and Parse Engine File --- launch/zed.launch | 60 +++++++++++---------- src/perception/object_detector/Inference.cu | 49 ++++++++++++++--- 2 files changed, 72 insertions(+), 37 deletions(-) diff --git a/launch/zed.launch b/launch/zed.launch index fb10687f3..ccb0facf1 100644 --- a/launch/zed.launch +++ b/launch/zed.launch @@ -1,54 +1,56 @@ - + - + - + - + - + args="load mrover/ZedNodelet perception_nodelet_manager" output="screen"> + + args="0 0 0 0 0 0 base_link zed_mount_link 100" /> - + camera_model:=$(arg zed_model)" /> - - + + - - - - - - - - - - - + pkg="nodelet" type="nodelet" name="rtabmap_stereo_odometry" respawn="true" + args="load rtabmap_odom/stereo_odometry perception_nodelet_manager" output="screen"> + + + + + + + + + + + - + - + - + - + \ No newline at end of file diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index 7af688cad..052bc447d 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -16,6 +16,7 @@ #include "inference.cuh" #include +#include #include #include @@ -43,8 +44,9 @@ namespace mrover { Inference::Inference(std::string onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string classesTxtFile = "") : logger{}, inputTensor{}, outputTensor{}, stream{} { enginePtr = std::unique_ptr>{createCudaEngine(onnxModelPath)}; + if (!enginePtr) { - throw std::runtime_error("Failed to create CUAD engine"); + throw std::runtime_error("Failed to create CUDA engine"); } this->modelInputShape = modelInputShape; @@ -74,15 +76,46 @@ namespace mrover { config->setMemoryPoolLimit(MemoryPoolType::kWORKSPACE, 1 << 30); - auto profile = builder->createOptimizationProfile(); - profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMIN, Dims4{1, 3, 256, 256}); - profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kOPT, Dims4{1, 3, 256, 256}); - profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMAX, Dims4{32, 3, 256, 256}); - config->addOptimizationProfile(profile); + // auto profile = builder->createOptimizationProfile(); + // profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMIN, Dims4{1, 3, 256, 256}); + // profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kOPT, Dims4{1, 3, 256, 256}); + // profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMAX, Dims4{32, 3, 256, 256}); + + // config->addOptimizationProfile(profile); + + std::cout << "FLAG" << std::endl; std::cout << network.get() << std::endl; - nvinfer1::IHostMemory* serializedEngine = builder->buildSerializedNetwork(*network, *config); + + //Create runtime engine nvinfer1::IRuntime* runtime = createInferRuntime(logger); - return runtime->deserializeCudaEngine(serializedEngine->data(), serializedEngine->size()); + + std::filesystem::path enginePath("./tensorrt-engine.engine"); + + bool isEngineFile = std::filesystem::exists(enginePath); + + //Check if engine file exists + if (isEngineFile) { + //Load engine from file + std::ifstream inputFileStream("./tensorrt-engine.engine", std::ios::binary); + std::stringstream engineBuffer; + + engineBuffer << inputFileStream.rdbuf(); + std::string enginePlan = engineBuffer.str(); + return runtime->deserializeCudaEngine((void*) enginePlan.data(), enginePlan.size(), nullptr); + } else { + nvinfer1::IHostMemory* serializedEngine = builder->buildSerializedNetwork(*network, *config); + + //Create temporary engine for serializing + auto tempEng = runtime->deserializeCudaEngine(serializedEngine->data(), serializedEngine->size()); + + //Save Engine to File + auto trtModelStream = tempEng->serialize(); + std::ofstream outputFileStream("./tensorrt-engine.engine", std::ios::binary); + outputFileStream.write((const char*) trtModelStream->data(), trtModelStream->size()); + outputFileStream.close(); + + return tempEng; + } } void Inference::doDetections(cv::Mat& img) { From f065a639c7d86330d2ce0052cc0599e5a29355e4 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Wed, 15 Nov 2023 19:09:52 -0500 Subject: [PATCH 075/197] seg fault on Cuda stream sad :((((((( --- src/perception/object_detector/Inference.cu | 20 ++++++++++++++++--- .../object_detector/object_detector.cpp | 3 ++- .../object_detector.processing.cpp | 6 +++++- 3 files changed, 24 insertions(+), 5 deletions(-) diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index 052bc447d..ef835a4c9 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -55,13 +55,18 @@ namespace mrover { assert(this->enginePtr->bindingIsInput(0) ^ enginePtr->bindingIsInput(1)); this->onnxModelPath = onnxModelPath; + std::cout << "reach2.5" << std::endl; prepTensors(); + + setUpContext(); + std::cout << "reach3" << std::endl; } // Initializes enginePtr with built engine ICudaEngine* Inference::createCudaEngine(std::string& onnxModelPath) { logger = Logger(); + this->stream = cudawrapper::CudaStream(); // See link sfor additional context const auto explicitBatch = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); std::unique_ptr> builder{nvinfer1::createInferBuilder(logger)}; @@ -118,6 +123,15 @@ namespace mrover { } } + void Inference::setUpContext() { + // Create Execution Context. + this->contextPtr.reset(this->enginePtr->createExecutionContext()); + + Dims dims_i{this->enginePtr->getBindingDimensions(0)}; + Dims4 inputDims{Inference::BATCH_SIZE, dims_i.d[1], dims_i.d[2], dims_i.d[3]}; + contextPtr->setBindingDimensions(0, inputDims); + } + void Inference::doDetections(cv::Mat& img) { //Do the forward pass on the network ROS_INFO("HI"); @@ -130,6 +144,7 @@ namespace mrover { int inputId = Inference::getBindingInputIndex(this->contextPtr.get()); //Copy data to GPU memory + std::cout << "ptr " << this->bindings[inputId] << " size " << inputEntries.d[0] * inputEntries.d[1] * inputEntries.d[2] * sizeof(float) << std::endl; cudaMemcpyAsync(this->bindings[inputId], input, inputEntries.d[0] * inputEntries.d[1] * inputEntries.d[2] * sizeof(float), cudaMemcpyHostToDevice, this->stream); //Queue the async engine process @@ -146,6 +161,7 @@ namespace mrover { * Modifies bindings, inputTensor, and outputTensor */ void Inference::prepTensors() { + for (int i = 0; i < this->enginePtr->getNbIOTensors(); i++) { const char* tensorName = this->enginePtr->getIOTensorName(i); @@ -159,9 +175,7 @@ namespace mrover { cudaMalloc(&(this->bindings)[i], Inference::BATCH_SIZE * size * sizeof(float)); } - inputEntries = nvinfer1::Dims3(modelInputShape.width, modelInputShape.height, 3); - inputEntries = nvinfer1::Dims3(1, 84, 8400); - cv::resize(outputTensor, outputTensor, cv::Size(84, 840)); + inputEntries = nvinfer1::Dims3(modelInputShape.width, modelInputShape.height, 3); //3 Is for the 3 RGB pixels } int Inference::getBindingInputIndex(nvinfer1::IExecutionContext* context) { diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 588e65077..ac33c9b7a 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -19,7 +20,7 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); inferenceWrapper = InferenceWrapper("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); - ROS_INFO("hi"); + std::cout << "bruh" << std::endl; //inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //read ONNX file into this mNet, YOLOV8, second smallest one diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 1387010fd..e687bdeeb 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -28,8 +28,12 @@ namespace mrover { cv::Mat imageView{static_cast(msg->height), static_cast(msg->width), CV_8UC4, const_cast(msg->data.data())}; cv::Mat sizedImage; + cv::resize(imageView, sizedImage, cv::Size(640, 640)); - cv::Mat blob = cv::dnn::blobFromImage(sizedImage); + cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGRA2BGR); + cv::Mat finalMat; + sizedImage.convertTo(finalMat, CV_32FC3); + //cv::Mat blob = cv::dnn::blobFromImage(sizedImage); ROS_INFO("hi"); inferenceWrapper.doDetections(sizedImage); } From 90293cb87fe337eed41b87c43793b54870733b98 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Thu, 16 Nov 2023 17:32:26 -0500 Subject: [PATCH 076/197] I FORGET WHAT THESE CHANGES ARE --- src/perception/object_detector/Inference.cu | 5 +++-- .../object_detector/object_detector.processing.cpp | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index ef835a4c9..973466d54 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -53,6 +53,7 @@ namespace mrover { assert(this->enginePtr->getNbBindings() == 2); assert(this->enginePtr->bindingIsInput(0) ^ enginePtr->bindingIsInput(1)); + this->stream = cudawrapper::CudaStream(); this->onnxModelPath = onnxModelPath; std::cout << "reach2.5" << std::endl; @@ -66,7 +67,6 @@ namespace mrover { // Initializes enginePtr with built engine ICudaEngine* Inference::createCudaEngine(std::string& onnxModelPath) { logger = Logger(); - this->stream = cudawrapper::CudaStream(); // See link sfor additional context const auto explicitBatch = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); std::unique_ptr> builder{nvinfer1::createInferBuilder(logger)}; @@ -99,7 +99,7 @@ namespace mrover { bool isEngineFile = std::filesystem::exists(enginePath); //Check if engine file exists - if (isEngineFile) { + if (false) { //Load engine from file std::ifstream inputFileStream("./tensorrt-engine.engine", std::ios::binary); std::stringstream engineBuffer; @@ -144,6 +144,7 @@ namespace mrover { int inputId = Inference::getBindingInputIndex(this->contextPtr.get()); //Copy data to GPU memory + std::cout << input << std::endl; std::cout << "ptr " << this->bindings[inputId] << " size " << inputEntries.d[0] * inputEntries.d[1] * inputEntries.d[2] * sizeof(float) << std::endl; cudaMemcpyAsync(this->bindings[inputId], input, inputEntries.d[0] * inputEntries.d[1] * inputEntries.d[2] * sizeof(float), cudaMemcpyHostToDevice, this->stream); diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index e687bdeeb..c853e74d2 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -34,8 +34,9 @@ namespace mrover { cv::Mat finalMat; sizedImage.convertTo(finalMat, CV_32FC3); //cv::Mat blob = cv::dnn::blobFromImage(sizedImage); - ROS_INFO("hi"); - inferenceWrapper.doDetections(sizedImage); + std::cout << finalMat.elemSize() * 640 * 640 << std::endl; + std::cout << finalMat.data << " awdasda" << std::endl; + inferenceWrapper.doDetections(finalMat); } /* void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { From 243d5901527e351b9f3563320fc6736a1be90f52 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 16 Nov 2023 18:39:55 -0500 Subject: [PATCH 077/197] Initia changes --- src/perception/object_detector/Inference.cu | 116 ++++++++++-------- src/perception/object_detector/inference.cuh | 44 +++---- .../object_detector/object_detector.cpp | 5 +- .../object_detector/object_detector.hpp | 15 ++- 4 files changed, 94 insertions(+), 86 deletions(-) diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index 973466d54..ab0f3485b 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -41,42 +41,52 @@ namespace mrover { //Constructor // : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} - Inference::Inference(std::string onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string classesTxtFile = "") : logger{}, inputTensor{}, outputTensor{}, stream{} { + Inference::Inference(std::string const& onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string const& classesTxtFile = "") + : mModelInputShape{modelInputShape} { - enginePtr = std::unique_ptr>{createCudaEngine(onnxModelPath)}; + mEngine = std::unique_ptr>{createCudaEngine(onnxModelPath)}; + if (!mEngine) throw std::runtime_error("Failed to create CUDA engine"); - if (!enginePtr) { - throw std::runtime_error("Failed to create CUDA engine"); - } + mLogger.log(ILogger::Severity::kINFO, "Created CUDA Engine"); - this->modelInputShape = modelInputShape; + // TODO: these are deprecated + assert(mEngine->getNbBindings() == 2); + assert(mEngine->bindingIsInput(0) ^ mEngine->bindingIsInput(1)); - assert(this->enginePtr->getNbBindings() == 2); - assert(this->enginePtr->bindingIsInput(0) ^ enginePtr->bindingIsInput(1)); - this->stream = cudawrapper::CudaStream(); + mStream.emplace(); - this->onnxModelPath = onnxModelPath; - std::cout << "reach2.5" << std::endl; + mLogger.log(ILogger::Severity::kINFO, "Created CUDA stream"); prepTensors(); setUpContext(); - std::cout << "reach3" << std::endl; } // Initializes enginePtr with built engine - ICudaEngine* Inference::createCudaEngine(std::string& onnxModelPath) { - logger = Logger(); + ICudaEngine* Inference::createCudaEngine(std::string const& onnxModelPath) { // See link sfor additional context - const auto explicitBatch = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); - std::unique_ptr> builder{nvinfer1::createInferBuilder(logger)}; - std::unique_ptr> network{builder->createNetworkV2(explicitBatch)}; - std::unique_ptr> parser{nvonnxparser::createParser(*network, logger)}; - std::unique_ptr> config{builder->createBuilderConfig()}; + constexpr auto explicitBatch = 1U << static_cast(NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + + std::unique_ptr> builder{createInferBuilder(mLogger)}; + if (!builder) throw std::runtime_error("Failed to create Infer Builder"); + mLogger.log(ILogger::Severity::kINFO, "Created Infer Builder"); + + std::unique_ptr> network{builder->createNetworkV2(explicitBatch)}; + if (!network) throw std::runtime_error("Failed to create Network Definition"); + mLogger.log(ILogger::Severity::kINFO, "Created Network Definition"); + + std::unique_ptr> parser{nvonnxparser::createParser(*network, mLogger)}; + if (!parser) throw std::runtime_error("Failed to create ONNX Parser"); + mLogger.log(ILogger::Severity::kINFO, "Created ONNX Parser"); + std::unique_ptr> config{builder->createBuilderConfig()}; + if (!config) throw std::runtime_error("Failed to create Builder Config"); + mLogger.log(ILogger::Severity::kINFO, "Created Builder Config"); + + // TODO: Not needed if we already have the engine file //Parse the onnx from file if (!parser->parseFromFile(onnxModelPath.c_str(), static_cast(ILogger::Severity::kINFO))) { - std::cout << "ERROR: could not parse input engine." << std::endl; + throw std::runtime_error("Failed to parse ONNX file"); } config->setMemoryPoolLimit(MemoryPoolType::kWORKSPACE, 1 << 30); @@ -88,35 +98,34 @@ namespace mrover { // config->addOptimizationProfile(profile); - std::cout << "FLAG" << std::endl; - std::cout << network.get() << std::endl; - //Create runtime engine - nvinfer1::IRuntime* runtime = createInferRuntime(logger); + IRuntime* runtime = createInferRuntime(mLogger); std::filesystem::path enginePath("./tensorrt-engine.engine"); - bool isEngineFile = std::filesystem::exists(enginePath); - //Check if engine file exists - if (false) { + if (exists(enginePath)) { + // TODO: error checking //Load engine from file std::ifstream inputFileStream("./tensorrt-engine.engine", std::ios::binary); std::stringstream engineBuffer; engineBuffer << inputFileStream.rdbuf(); std::string enginePlan = engineBuffer.str(); - return runtime->deserializeCudaEngine((void*) enginePlan.data(), enginePlan.size(), nullptr); + // TODO: deprecated + return runtime->deserializeCudaEngine(enginePlan.data(), enginePlan.size(), nullptr); } else { - nvinfer1::IHostMemory* serializedEngine = builder->buildSerializedNetwork(*network, *config); + IHostMemory* serializedEngine = builder->buildSerializedNetwork(*network, *config); + if (!serializedEngine) throw std::runtime_error("Failed to serialize engine"); //Create temporary engine for serializing - auto tempEng = runtime->deserializeCudaEngine(serializedEngine->data(), serializedEngine->size()); + ICudaEngine* tempEng = runtime->deserializeCudaEngine(serializedEngine->data(), serializedEngine->size()); + if (!tempEng) throw std::runtime_error("Failed to create temporary engine"); //Save Engine to File auto trtModelStream = tempEng->serialize(); std::ofstream outputFileStream("./tensorrt-engine.engine", std::ios::binary); - outputFileStream.write((const char*) trtModelStream->data(), trtModelStream->size()); + outputFileStream.write(static_cast(trtModelStream->data()), trtModelStream->size()); outputFileStream.close(); return tempEng; @@ -125,34 +134,39 @@ namespace mrover { void Inference::setUpContext() { // Create Execution Context. - this->contextPtr.reset(this->enginePtr->createExecutionContext()); + mContext.reset(mEngine->createExecutionContext()); - Dims dims_i{this->enginePtr->getBindingDimensions(0)}; - Dims4 inputDims{Inference::BATCH_SIZE, dims_i.d[1], dims_i.d[2], dims_i.d[3]}; - contextPtr->setBindingDimensions(0, inputDims); + Dims dims_i{mEngine->getBindingDimensions(0)}; + Dims4 inputDims{BATCH_SIZE, dims_i.d[1], dims_i.d[2], dims_i.d[3]}; + mContext->setBindingDimensions(0, inputDims); } void Inference::doDetections(cv::Mat& img) { //Do the forward pass on the network ROS_INFO("HI"); - launchInference(img.data, this->outputTensor.data); - std::cout << *(this->outputTensor.data) << std::endl; - //return Parser(this->outputTensor).parseTensor(); + launchInference(img.data, mOutputTensor.data); + std::cout << *(mOutputTensor.data) << std::endl; + //return Parser(outputTensor).parseTensor(); } void Inference::launchInference(void* input, void* output) { - int inputId = Inference::getBindingInputIndex(this->contextPtr.get()); + assert(input); + assert(output); + assert(mContext); + assert(mStream); + + int inputId = getBindingInputIndex(mContext.get()); //Copy data to GPU memory std::cout << input << std::endl; - std::cout << "ptr " << this->bindings[inputId] << " size " << inputEntries.d[0] * inputEntries.d[1] * inputEntries.d[2] * sizeof(float) << std::endl; - cudaMemcpyAsync(this->bindings[inputId], input, inputEntries.d[0] * inputEntries.d[1] * inputEntries.d[2] * sizeof(float), cudaMemcpyHostToDevice, this->stream); + std::cout << "ptr " << mBindings[inputId] << " size " << mInputDimensions.d[0] * mInputDimensions.d[1] * mInputDimensions.d[2] * sizeof(float) << std::endl; + cudaMemcpyAsync(mBindings[inputId], input, mInputDimensions.d[0] * mInputDimensions.d[1] * mInputDimensions.d[2] * sizeof(float), cudaMemcpyHostToDevice, mStream.value()); //Queue the async engine process - this->contextPtr->enqueueV3(this->stream); + mContext->enqueueV3(mStream.value()); //Copy data to CPU memory - cudaMemcpyAsync(output, this->bindings[1 - inputId], outputEntries.d[0] * outputEntries.d[1] * outputEntries.d[2] * sizeof(float), cudaMemcpyDeviceToHost, this->stream); + cudaMemcpyAsync(output, mBindings[1 - inputId], mOutputDimensions.d[0] * mOutputDimensions.d[1] * mOutputDimensions.d[2] * sizeof(float), cudaMemcpyDeviceToHost, mStream.value()); } @@ -163,23 +177,23 @@ namespace mrover { */ void Inference::prepTensors() { - for (int i = 0; i < this->enginePtr->getNbIOTensors(); i++) { - const char* tensorName = this->enginePtr->getIOTensorName(i); + for (int i = 0; i < mEngine->getNbIOTensors(); i++) { + const char* tensorName = mEngine->getIOTensorName(i); - Dims dims{this->enginePtr->getTensorShape(tensorName)}; + Dims dims{mEngine->getTensorShape(tensorName)}; - size_t size = accumulate(dims.d + 1, dims.d + dims.nbDims, Inference::BATCH_SIZE, std::multiplies<>()); + size_t size = accumulate(dims.d + 1, dims.d + dims.nbDims, BATCH_SIZE, std::multiplies<>()); std::vector sizes = {dims.d[1], dims.d[2], dims.d[3]}; // Create CUDA buffer for Tensor. - cudaMalloc(&(this->bindings)[i], Inference::BATCH_SIZE * size * sizeof(float)); + cudaMalloc(&(mBindings)[i], BATCH_SIZE * size * sizeof(float)); } - inputEntries = nvinfer1::Dims3(modelInputShape.width, modelInputShape.height, 3); //3 Is for the 3 RGB pixels + mInputDimensions = Dims3(mModelInputShape.width, mModelInputShape.height, 3); //3 Is for the 3 RGB pixels } - int Inference::getBindingInputIndex(nvinfer1::IExecutionContext* context) { - return context->getEngine().getTensorIOMode(context->getEngine().getIOTensorName(0)) != nvinfer1::TensorIOMode::kINPUT; // 0 (false) if bindingIsInput(0), 1 (true) otherwise + int Inference::getBindingInputIndex(IExecutionContext* context) { + return context->getEngine().getTensorIOMode(context->getEngine().getIOTensorName(0)) != TensorIOMode::kINPUT; // 0 (false) if bindingIsInput(0), 1 (true) otherwise } } // namespace mrover diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index 685e0fd77..f3481e8e9 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -1,8 +1,10 @@ // Cpp native #pragma once + #include #include #include + #include #include @@ -43,57 +45,50 @@ using nvinfer1::IExecutionContext; // Preallocated CV::Mat of floats of correct size -> transpose into CV::Mat (also preallocated namespace mrover { - class Inference { - private: - static const int BATCH_SIZE = 1; - nvinfer1::Logger logger; + class Inference { + constexpr static int BATCH_SIZE = 1; + nvinfer1::Logger mLogger; //Ptr to the engine - std::unique_ptr> enginePtr; + std::unique_ptr> mEngine; //Ptr to the context - std::unique_ptr> contextPtr; + std::unique_ptr> mContext; //Input, output and reference tensors - cv::Mat inputTensor; - cv::Mat outputTensor; + cv::Mat mInputTensor; + cv::Mat mOutputTensor; //Num Entries - nvinfer1::Dims3 inputEntries; - nvinfer1::Dims3 outputEntries; + nvinfer1::Dims3 mInputDimensions; + nvinfer1::Dims3 mOutputDimensions; //Cuda Stream - cudawrapper::CudaStream stream; + std::optional mStream; //Bindings - std::array bindings{}; + std::array mBindings{}; //ONNX Model Path - std::string onnxModelPath; + std::string mOnnxModelPath; //Size of Model - cv::Size modelInputShape; - cv::Size modelOutputShape; + cv::Size mModelInputShape; + cv::Size mModelOutputShape; - private: //STATIC FUNCTIONS - static int getBindingInputIndex(nvinfer1::IExecutionContext* context); + static int getBindingInputIndex(IExecutionContext* context); public: - Inference(std::string onnxModelPath, cv::Size modelInputShape, std::string classesTxtFile); - - - std::vector runInference(cv::Mat const& input); + Inference(std::string const& onnxModelPath, cv::Size modelInputShape, std::string const& classesTxtFile); private: //Creates a ptr to the engine - ICudaEngine* createCudaEngine(std::string& onnxModelPath); + ICudaEngine* createCudaEngine(std::string const& onnxModelPath); void launchInference(void* input, void* output); - void setUpContext(const std::string& inputFile); //This will need to be adjusted to the img msg, but I think its ok to just run pictures as an intial test - void prepTensors(); void setUpContext(); @@ -101,4 +96,5 @@ namespace mrover { public: void doDetections(cv::Mat& img); }; + } // namespace mrover diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index ac33c9b7a..d836d9fd6 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -19,8 +19,7 @@ namespace mrover { void ObjectDetectorNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - inferenceWrapper = InferenceWrapper("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); - std::cout << "bruh" << std::endl; + inferenceWrapper = InferenceWrapper("/home/quintin/catkin_ws/src/mrover/yolov8s.onnx", cv::Size(640, 640), ""); //inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //read ONNX file into this mNet, YOLOV8, second smallest one @@ -31,7 +30,7 @@ namespace mrover { mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); //mDetectionData = mNh.advertise("/object_detector/detected_object", 1); - this->imageBlob = cv::Mat{1, ObjectDetectorNodelet::NUM_CHANNELS, ObjectDetectorNodelet::IMG_WIDTH, ObjectDetectorNodelet::IMG_HEIGHT, CV_32F}; + this->imageBlob = cv::Mat{1, NUM_CHANNELS, IMG_WIDTH, IMG_HEIGHT, CV_32F}; } } // namespace mrover diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index b5b68a690..28df16cae 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -7,18 +7,18 @@ namespace mrover { struct Detection { int class_id{0}; - std::string className{}; + std::string className; float confidence{0.0}; cv::Scalar color{}; cv::Rect box{}; }; class ObjectDetectorNodelet : public nodelet::Nodelet { - static const int NUM_CHANNELS = 3; - static const int IMG_WIDTH = 1280; - static const int IMG_HEIGHT = 720; - private: + constexpr static int NUM_CHANNELS = 3; + constexpr static int IMG_WIDTH = 1280; + constexpr static int IMG_HEIGHT = 720; + ros::NodeHandle mNh, mPnh; //Inference inference; @@ -29,7 +29,6 @@ namespace mrover { ros::Publisher mDebugImgPub; ros::Publisher mDetectionData; - // Subscribers ros::Subscriber mImgSub; @@ -37,8 +36,8 @@ namespace mrover { // Preallocated cv::Mats cv::Mat imageBlob; - dynamic_reconfigure::Server mConfigServer; - dynamic_reconfigure::Server::CallbackType mCallbackType; + dynamic_reconfigure::Server mConfigServer; + dynamic_reconfigure::Server::CallbackType mCallbackType; // Internal state From 2e719a9c287eef202adbd8ec5c2c71908562a0ce Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 16 Nov 2023 19:19:38 -0500 Subject: [PATCH 078/197] Minor changeS --- src/perception/object_detector/Inference.cu | 11 +++++------ src/perception/object_detector/cudaWrapper.cuh | 16 ++++++++++------ src/perception/object_detector/inference.cuh | 2 +- 3 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index ab0f3485b..b8b7db0d5 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -141,7 +141,7 @@ namespace mrover { mContext->setBindingDimensions(0, inputDims); } - void Inference::doDetections(cv::Mat& img) { + void Inference::doDetections(const cv::Mat& img) { //Do the forward pass on the network ROS_INFO("HI"); launchInference(img.data, mOutputTensor.data); @@ -151,7 +151,7 @@ namespace mrover { void Inference::launchInference(void* input, void* output) { assert(input); - assert(output); + // assert(output); assert(mContext); assert(mStream); @@ -182,12 +182,11 @@ namespace mrover { Dims dims{mEngine->getTensorShape(tensorName)}; - size_t size = accumulate(dims.d + 1, dims.d + dims.nbDims, BATCH_SIZE, std::multiplies<>()); - std::vector sizes = {dims.d[1], dims.d[2], dims.d[3]}; - + std::size_t size = accumulate(dims.d + 1, dims.d + dims.nbDims, BATCH_SIZE, std::multiplies<>()); + std::vector sizes{dims.d[1], dims.d[2], dims.d[3]}; // Create CUDA buffer for Tensor. - cudaMalloc(&(mBindings)[i], BATCH_SIZE * size * sizeof(float)); + cudaMalloc(&mBindings[i], BATCH_SIZE * size * sizeof(float)); } mInputDimensions = Dims3(mModelInputShape.width, mModelInputShape.height, 3); //3 Is for the 3 RGB pixels diff --git a/src/perception/object_detector/cudaWrapper.cuh b/src/perception/object_detector/cudaWrapper.cuh index b9bb50487..9ffeacc7d 100644 --- a/src/perception/object_detector/cudaWrapper.cuh +++ b/src/perception/object_detector/cudaWrapper.cuh @@ -24,17 +24,23 @@ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef __CUDA_WRAPPER_H__ -#define __CUDA_WRAPPER_H__ + +#pragma once #include +inline void check(cudaError_t result) { + if (result == cudaSuccess) return; + + throw std::runtime_error{std::string{"CUDA Error:"} + cudaGetErrorString(result)}; +} + namespace cudawrapper { class CudaStream { public: CudaStream() { - cudaStreamCreate(&mStream); + check(cudaStreamCreate(&mStream)); } operator cudaStream_t() { @@ -52,7 +58,7 @@ namespace cudawrapper { class CudaEvent { public: CudaEvent() { - cudaEventCreate(&mEvent); + check(cudaEventCreate(&mEvent)); } operator cudaEvent_t() { @@ -68,5 +74,3 @@ namespace cudawrapper { }; } // namespace cudawrapper - -#endif /*__CUDA_WRAPPER_H__*/ diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index f3481e8e9..3cde6b160 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -94,7 +94,7 @@ namespace mrover { void setUpContext(); public: - void doDetections(cv::Mat& img); + void doDetections(const cv::Mat& img); }; } // namespace mrover From 5502240daa96cc58a79e231ca0cfb9e2c1e2616c Mon Sep 17 00:00:00 2001 From: jbrhm Date: Thu, 16 Nov 2023 20:04:15 -0500 Subject: [PATCH 079/197] Null Ptr :((((((( --- src/perception/object_detector/Inference.cu | 14 ++++++++------ src/perception/object_detector/object_detector.cpp | 2 +- .../object_detector/object_detector.processing.cpp | 4 ++-- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index b8b7db0d5..6a508bef3 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -7,6 +7,7 @@ #include "ioHelper.cuh" #include +#include #include #include #include @@ -41,8 +42,7 @@ namespace mrover { //Constructor // : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} - Inference::Inference(std::string const& onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string const& classesTxtFile = "") - : mModelInputShape{modelInputShape} { + Inference::Inference(std::string const& onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string const& classesTxtFile = "") { mEngine = std::unique_ptr>{createCudaEngine(onnxModelPath)}; if (!mEngine) throw std::runtime_error("Failed to create CUDA engine"); @@ -145,7 +145,7 @@ namespace mrover { //Do the forward pass on the network ROS_INFO("HI"); launchInference(img.data, mOutputTensor.data); - std::cout << *(mOutputTensor.data) << std::endl; + std::cout << (mOutputTensor.data) << " bruh " << std::endl; //return Parser(outputTensor).parseTensor(); } @@ -160,13 +160,13 @@ namespace mrover { //Copy data to GPU memory std::cout << input << std::endl; std::cout << "ptr " << mBindings[inputId] << " size " << mInputDimensions.d[0] * mInputDimensions.d[1] * mInputDimensions.d[2] * sizeof(float) << std::endl; - cudaMemcpyAsync(mBindings[inputId], input, mInputDimensions.d[0] * mInputDimensions.d[1] * mInputDimensions.d[2] * sizeof(float), cudaMemcpyHostToDevice, mStream.value()); + cudaMemcpy(mBindings[inputId], input, mInputDimensions.d[0] * mInputDimensions.d[1] * mInputDimensions.d[2] * sizeof(float), cudaMemcpyHostToDevice); //Queue the async engine process - mContext->enqueueV3(mStream.value()); + mContext->executeV2(mBindings.data()); //Copy data to CPU memory - cudaMemcpyAsync(output, mBindings[1 - inputId], mOutputDimensions.d[0] * mOutputDimensions.d[1] * mOutputDimensions.d[2] * sizeof(float), cudaMemcpyDeviceToHost, mStream.value()); + cudaMemcpy(output, mBindings[1 - inputId], mOutputDimensions.d[0] * mOutputDimensions.d[1] * mOutputDimensions.d[2] * sizeof(float), cudaMemcpyDeviceToHost); } @@ -190,6 +190,8 @@ namespace mrover { } mInputDimensions = Dims3(mModelInputShape.width, mModelInputShape.height, 3); //3 Is for the 3 RGB pixels + int outputDims[] = {1, 84, 8400}; + mOutputTensor = cv::Mat{3, outputDims, CV_32F}; } int Inference::getBindingInputIndex(IExecutionContext* context) { diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index d836d9fd6..c90c4d003 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -19,7 +19,7 @@ namespace mrover { void ObjectDetectorNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - inferenceWrapper = InferenceWrapper("/home/quintin/catkin_ws/src/mrover/yolov8s.onnx", cv::Size(640, 640), ""); + inferenceWrapper = InferenceWrapper("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //read ONNX file into this mNet, YOLOV8, second smallest one diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index c853e74d2..7b64fb19c 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -34,8 +34,8 @@ namespace mrover { cv::Mat finalMat; sizedImage.convertTo(finalMat, CV_32FC3); //cv::Mat blob = cv::dnn::blobFromImage(sizedImage); - std::cout << finalMat.elemSize() * 640 * 640 << std::endl; - std::cout << finalMat.data << " awdasda" << std::endl; + //std::cout << finalMat.elemSize() * 640 * 640 << std::endl; + //std::cout << finalMat.data << " awdasda" << std::endl; inferenceWrapper.doDetections(finalMat); } /* From 4dcfd13efa814142bedb555ab225d5018c05e8ce Mon Sep 17 00:00:00 2001 From: qhdwight Date: Fri, 17 Nov 2023 13:54:02 -0500 Subject: [PATCH 080/197] Get rid of uneeded mat --- src/perception/object_detector/object_detector.cpp | 5 +++-- src/perception/object_detector/object_detector.hpp | 4 ++-- .../object_detector/object_detector.processing.cpp | 5 ++--- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index c90c4d003..52e9f6c2e 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -19,7 +19,7 @@ namespace mrover { void ObjectDetectorNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - inferenceWrapper = InferenceWrapper("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); + mInferenceWrapper = InferenceWrapper("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //read ONNX file into this mNet, YOLOV8, second smallest one @@ -30,7 +30,8 @@ namespace mrover { mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); //mDetectionData = mNh.advertise("/object_detector/detected_object", 1); - this->imageBlob = cv::Mat{1, NUM_CHANNELS, IMG_WIDTH, IMG_HEIGHT, CV_32F}; + // TODO(quintin): I don't think this is the constructor you want. You should set channels via the type (e.g. "CV_32FC2" for 2 float channels) + mImageBlob = cv::Mat{1, NUM_CHANNELS, IMG_WIDTH, IMG_HEIGHT, CV_32F}; } } // namespace mrover diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 28df16cae..a736dfdc0 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -22,7 +22,7 @@ namespace mrover { ros::NodeHandle mNh, mPnh; //Inference inference; - InferenceWrapper inferenceWrapper; + InferenceWrapper mInferenceWrapper; // Publishers @@ -34,7 +34,7 @@ namespace mrover { ros::Subscriber mImgSub; // Preallocated cv::Mats - cv::Mat imageBlob; + cv::Mat mImageBlob; dynamic_reconfigure::Server mConfigServer; dynamic_reconfigure::Server::CallbackType mCallbackType; diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 7b64fb19c..1c9dbe8ce 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -31,12 +31,11 @@ namespace mrover { cv::resize(imageView, sizedImage, cv::Size(640, 640)); cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGRA2BGR); - cv::Mat finalMat; - sizedImage.convertTo(finalMat, CV_32FC3); + sizedImage.convertTo(sizedImage, CV_32FC3); //cv::Mat blob = cv::dnn::blobFromImage(sizedImage); //std::cout << finalMat.elemSize() * 640 * 640 << std::endl; //std::cout << finalMat.data << " awdasda" << std::endl; - inferenceWrapper.doDetections(finalMat); + mInferenceWrapper.doDetections(sizedImage); } /* void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { From 025af0030c25b2abc9021e92654af95ee608b11f Mon Sep 17 00:00:00 2001 From: qhdwight Date: Fri, 17 Nov 2023 14:41:49 -0500 Subject: [PATCH 081/197] Clean up includes, refactor allocation of bindings, make inference function take cv mats --- src/perception/object_detector/Inference.cpp | 5 +- src/perception/object_detector/Inference.cu | 62 ++++++------------- src/perception/object_detector/inference.cuh | 41 +----------- .../object_detector/inferenceWrapper.cu | 26 +++----- .../object_detector/inferenceWrapper.hpp | 14 ++--- src/perception/object_detector/ioHelper.cu | 9 +-- src/perception/object_detector/ioHelper.cuh | 11 ++-- .../object_detector/object_detector.cpp | 17 +---- .../object_detector/object_detector.hpp | 3 +- .../object_detector.processing.cpp | 21 +------ src/perception/object_detector/pch.hpp | 7 +++ 11 files changed, 56 insertions(+), 160 deletions(-) diff --git a/src/perception/object_detector/Inference.cpp b/src/perception/object_detector/Inference.cpp index 0c35a5cf1..c9b6a6fa3 100644 --- a/src/perception/object_detector/Inference.cpp +++ b/src/perception/object_detector/Inference.cpp @@ -1,10 +1,7 @@ -#include -#include - +#include "pch.hpp" namespace mrover { - /* // Initializes enginePtr with built engine diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index 6a508bef3..eb1707bcf 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -1,3 +1,5 @@ +#include "inference.cuh" + #include #include #include @@ -6,21 +8,6 @@ #include "ioHelper.cuh" -#include -#include -#include -#include -#include -#include -#include - -#include "inference.cuh" - -#include -#include -#include -#include - using namespace nvinfer1; /** @@ -136,37 +123,29 @@ namespace mrover { // Create Execution Context. mContext.reset(mEngine->createExecutionContext()); - Dims dims_i{mEngine->getBindingDimensions(0)}; - Dims4 inputDims{BATCH_SIZE, dims_i.d[1], dims_i.d[2], dims_i.d[3]}; - mContext->setBindingDimensions(0, inputDims); + mContext->setBindingDimensions(0, mEngine->getBindingDimensions(0)); } void Inference::doDetections(const cv::Mat& img) { //Do the forward pass on the network - ROS_INFO("HI"); - launchInference(img.data, mOutputTensor.data); - std::cout << (mOutputTensor.data) << " bruh " << std::endl; + launchInference(img, mOutputTensor); + ROS_INFO_STREAM(static_cast(mOutputTensor.data)); //return Parser(outputTensor).parseTensor(); } - void Inference::launchInference(void* input, void* output) { - assert(input); - // assert(output); + void Inference::launchInference(cv::Mat const& input, cv::Mat const& output) { + assert(!input.empty()); + assert(!output.empty()); + assert(input.isContinuous()); + assert(output.isContinuous()); assert(mContext); assert(mStream); int inputId = getBindingInputIndex(mContext.get()); - //Copy data to GPU memory - std::cout << input << std::endl; - std::cout << "ptr " << mBindings[inputId] << " size " << mInputDimensions.d[0] * mInputDimensions.d[1] * mInputDimensions.d[2] * sizeof(float) << std::endl; - cudaMemcpy(mBindings[inputId], input, mInputDimensions.d[0] * mInputDimensions.d[1] * mInputDimensions.d[2] * sizeof(float), cudaMemcpyHostToDevice); - - //Queue the async engine process + cudaMemcpy(mBindings[inputId], input.data, input.total() * input.elemSize(), cudaMemcpyHostToDevice); mContext->executeV2(mBindings.data()); - - //Copy data to CPU memory - cudaMemcpy(output, mBindings[1 - inputId], mOutputDimensions.d[0] * mOutputDimensions.d[1] * mOutputDimensions.d[2] * sizeof(float), cudaMemcpyDeviceToHost); + cudaMemcpy(output.data, mBindings[1 - inputId], output.total() * output.elemSize(), cudaMemcpyDeviceToHost); } @@ -180,18 +159,17 @@ namespace mrover { for (int i = 0; i < mEngine->getNbIOTensors(); i++) { const char* tensorName = mEngine->getIOTensorName(i); - Dims dims{mEngine->getTensorShape(tensorName)}; - - std::size_t size = accumulate(dims.d + 1, dims.d + dims.nbDims, BATCH_SIZE, std::multiplies<>()); - std::vector sizes{dims.d[1], dims.d[2], dims.d[3]}; + auto [rank, extents] = mEngine->getTensorShape(tensorName); - // Create CUDA buffer for Tensor. - cudaMalloc(&mBindings[i], BATCH_SIZE * size * sizeof(float)); + // Multiply sizeof(float) by the product of the extents + // This is essentially: element count * size of each element + std::size_t size = std::reduce(extents, extents + rank, sizeof(float), std::multiplies<>()); + // Create GPU memory for TensorRT to operate on + cudaMalloc(mBindings.data() + i, size); } - mInputDimensions = Dims3(mModelInputShape.width, mModelInputShape.height, 3); //3 Is for the 3 RGB pixels - int outputDims[] = {1, 84, 8400}; - mOutputTensor = cv::Mat{3, outputDims, CV_32F}; + // TODO(quintin): Avoid hardcoding this + mOutputTensor = cv::Mat::zeros(84, 8400, CV_MAKE_TYPE(CV_32F, 1)); } int Inference::getBindingInputIndex(IExecutionContext* context) { diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index 3cde6b160..18a33a9bb 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -1,43 +1,13 @@ // Cpp native #pragma once -#include -#include -#include - -#include - -#include -#include -#include - +#include "pch.hpp" -//Tensor-RT Specific -#include "cudaWrapper.cuh" #include -#include - +#include "cudaWrapper.cuh" #include "ioHelper.cuh" -// OpenCV / DNN / Inference -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -//MROVER MADE FILES -#include "object_detector.hpp" -#include "pch.hpp" - using nvinfer1::ICudaEngine; using nvinfer1::IExecutionContext; @@ -47,8 +17,6 @@ using nvinfer1::IExecutionContext; namespace mrover { class Inference { - constexpr static int BATCH_SIZE = 1; - nvinfer1::Logger mLogger; //Ptr to the engine @@ -60,9 +28,6 @@ namespace mrover { //Input, output and reference tensors cv::Mat mInputTensor; cv::Mat mOutputTensor; - //Num Entries - nvinfer1::Dims3 mInputDimensions; - nvinfer1::Dims3 mOutputDimensions; //Cuda Stream std::optional mStream; @@ -87,7 +52,7 @@ namespace mrover { //Creates a ptr to the engine ICudaEngine* createCudaEngine(std::string const& onnxModelPath); - void launchInference(void* input, void* output); + void launchInference(cv::Mat const& input, cv::Mat const& output); void prepTensors(); diff --git a/src/perception/object_detector/inferenceWrapper.cu b/src/perception/object_detector/inferenceWrapper.cu index 80f857281..910c336bb 100644 --- a/src/perception/object_detector/inferenceWrapper.cu +++ b/src/perception/object_detector/inferenceWrapper.cu @@ -1,22 +1,8 @@ -#include -#include -#include -#include -#include - -#include "ioHelper.cuh" +#include "inferenceWrapper.hpp" -#include -#include -#include -#include +#include #include "inference.cuh" -#include "inferenceWrapper.hpp" - -#include -#include -#include using namespace nvinfer1; @@ -36,12 +22,14 @@ using namespace nvinfer1; * Modifies stream, outputTensor */ namespace mrover { + //Initialize the unique_ptr to the inference class InferenceWrapper::InferenceWrapper(std::string onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string classesTxtFile = "") { - inferencePtr.reset(new Inference(onnxModelPath, modelInputShape, classesTxtFile)); + mInference.reset(new Inference(onnxModelPath, modelInputShape, classesTxtFile)); } - void InferenceWrapper::doDetections(cv::Mat& img) { - inferencePtr->doDetections(img); + void InferenceWrapper::doDetections(const cv::Mat& img) { + mInference->doDetections(img); } + } // namespace mrover diff --git a/src/perception/object_detector/inferenceWrapper.hpp b/src/perception/object_detector/inferenceWrapper.hpp index 45b081874..ccd920b2a 100644 --- a/src/perception/object_detector/inferenceWrapper.hpp +++ b/src/perception/object_detector/inferenceWrapper.hpp @@ -1,25 +1,21 @@ #pragma once + #include "pch.hpp" -#include -#include -#include namespace mrover { class Inference; class InferenceWrapper { - private: - //The pointer to the inference class - std::shared_ptr inferencePtr; + std::shared_ptr mInference; public: InferenceWrapper() = default; ~InferenceWrapper() = default; - //The constructor for the inferencewrapper InferenceWrapper(std::string onnxModelPath, cv::Size modelInputShape, std::string classesTxtFile); - void doDetections(cv::Mat& img); + void doDetections(cv::Mat const& img); }; -} // namespace mrover \ No newline at end of file + +} // namespace mrover diff --git a/src/perception/object_detector/ioHelper.cu b/src/perception/object_detector/ioHelper.cu index 4529dff31..46e39ad29 100644 --- a/src/perception/object_detector/ioHelper.cu +++ b/src/perception/object_detector/ioHelper.cu @@ -1,9 +1,4 @@ #include "ioHelper.cuh" -#include -#include -#include -#include -#include using namespace std; @@ -19,7 +14,7 @@ namespace nvinfer1 { return path.substr(baseId, path.rfind('.') - baseId); } - ostream& operator<<(ostream& o, const nvinfer1::ILogger::Severity severity) { + ostream& operator<<(ostream& o, const ILogger::Severity severity) { switch (severity) { case ILogger::Severity::kINTERNAL_ERROR: o << "INTERNAL_ERROR"; @@ -33,6 +28,8 @@ namespace nvinfer1 { case ILogger::Severity::kINFO: o << "INFO"; break; + default: + break; } return o; } diff --git a/src/perception/object_detector/ioHelper.cuh b/src/perception/object_detector/ioHelper.cuh index e80316c28..a9d296cc4 100644 --- a/src/perception/object_detector/ioHelper.cuh +++ b/src/perception/object_detector/ioHelper.cuh @@ -1,9 +1,8 @@ -#ifndef __IO_HELPER_H__ -#define __IO_HELPER_H__ +#pragma once -#include -#include -#include +#include "pch.hpp" + +#include namespace nvinfer1 { @@ -32,5 +31,3 @@ namespace nvinfer1 { std::string readBuffer(std::string const& path); } // namespace nvinfer1 - -#endif /*__IO_HELPER_H__*/ \ No newline at end of file diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 52e9f6c2e..aa144754b 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -1,25 +1,13 @@ #include "object_detector.hpp" -#include "inferenceWrapper.hpp" -//#include "inference.h" - -#include -//#include - -#include -#include -#include -#include -#include -#include -#include +#include "inferenceWrapper.hpp" namespace mrover { void ObjectDetectorNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mInferenceWrapper = InferenceWrapper("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); + mInferenceWrapper = InferenceWrapper("./yolov8s.onnx", cv::Size(640, 640), ""); //inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //read ONNX file into this mNet, YOLOV8, second smallest one @@ -48,7 +36,6 @@ int main(int argc, char** argv) { return EXIT_SUCCESS; } - #ifdef MROVER_IS_NODELET #include PLUGINLIB_EXPORT_CLASS(mrover::ObjectDetectorNodelet, nodelet::Nodelet) diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index a736dfdc0..47931ec17 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -1,5 +1,6 @@ -#include "inferenceWrapper.hpp" #include "pch.hpp" + +#include "inferenceWrapper.hpp" #include #include diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 1c9dbe8ce..fc07a33d3 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -1,32 +1,15 @@ #include "object_detector.hpp" -#include "inferenceWrapper.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "inferenceWrapper.hpp" namespace mrover { - void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { assert(msg); assert(msg->height > 0); assert(msg->width > 0); - cv::Mat imageView{static_cast(msg->height), static_cast(msg->width), CV_8UC4, const_cast(msg->data.data())}; + cv::Mat imageView{static_cast(msg->height), static_cast(msg->width), CV_8UC4, const_cast(msg->data.data())}; cv::Mat sizedImage; cv::resize(imageView, sizedImage, cv::Size(640, 640)); diff --git a/src/perception/object_detector/pch.hpp b/src/perception/object_detector/pch.hpp index 93bb96dc7..76660d837 100644 --- a/src/perception/object_detector/pch.hpp +++ b/src/perception/object_detector/pch.hpp @@ -1,13 +1,19 @@ #pragma once #include +#include #include #include #include #include +#include +#include +#include #include +#include #include #include +#include #include #include #include @@ -22,6 +28,7 @@ #include #include #include +#include #include #include #include From 432c22e7a0a28e4b67db4967b1570f1633350109 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Fri, 17 Nov 2023 21:08:54 -0500 Subject: [PATCH 082/197] Trying --- src/perception/object_detector/Inference.cu | 6 + src/perception/object_detector/inference.cuh | 2 + .../object_detector/inferenceWrapper.cu | 5 + .../object_detector/inferenceWrapper.hpp | 2 + .../object_detector/object_detector.cpp | 2 +- .../object_detector/object_detector.hpp | 3 + .../object_detector.processing.cpp | 169 +++++++++++++++++- 7 files changed, 184 insertions(+), 5 deletions(-) diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/Inference.cu index eb1707bcf..1908feef2 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/Inference.cu @@ -5,6 +5,7 @@ #include #include #include +#include #include "ioHelper.cuh" @@ -133,6 +134,11 @@ namespace mrover { //return Parser(outputTensor).parseTensor(); } + cv::Mat Inference::getOutputTensor() { + return mOutputTensor; + } + + void Inference::launchInference(cv::Mat const& input, cv::Mat const& output) { assert(!input.empty()); assert(!output.empty()); diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index 18a33a9bb..52e455864 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -60,6 +60,8 @@ namespace mrover { public: void doDetections(const cv::Mat& img); + + cv::Mat getOutputTensor(); }; } // namespace mrover diff --git a/src/perception/object_detector/inferenceWrapper.cu b/src/perception/object_detector/inferenceWrapper.cu index 910c336bb..032396e7e 100644 --- a/src/perception/object_detector/inferenceWrapper.cu +++ b/src/perception/object_detector/inferenceWrapper.cu @@ -1,6 +1,7 @@ #include "inferenceWrapper.hpp" #include +#include #include "inference.cuh" @@ -32,4 +33,8 @@ namespace mrover { mInference->doDetections(img); } + cv::Mat InferenceWrapper::getOutputTensor() { + return mInference->getOutputTensor(); + } + } // namespace mrover diff --git a/src/perception/object_detector/inferenceWrapper.hpp b/src/perception/object_detector/inferenceWrapper.hpp index ccd920b2a..2d0c09b09 100644 --- a/src/perception/object_detector/inferenceWrapper.hpp +++ b/src/perception/object_detector/inferenceWrapper.hpp @@ -16,6 +16,8 @@ namespace mrover { InferenceWrapper(std::string onnxModelPath, cv::Size modelInputShape, std::string classesTxtFile); void doDetections(cv::Mat const& img); + + cv::Mat getOutputTensor(); }; } // namespace mrover diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index aa144754b..33cdc85e7 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -7,7 +7,7 @@ namespace mrover { void ObjectDetectorNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mInferenceWrapper = InferenceWrapper("./yolov8s.onnx", cv::Size(640, 640), ""); + mInferenceWrapper = InferenceWrapper("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //read ONNX file into this mNet, YOLOV8, second smallest one diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 47931ec17..5b51a841a 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -20,6 +20,9 @@ namespace mrover { constexpr static int IMG_WIDTH = 1280; constexpr static int IMG_HEIGHT = 720; + std::vector classes{"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; + + ros::NodeHandle mNh, mPnh; //Inference inference; diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index fc07a33d3..f0aa9da7a 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -1,6 +1,9 @@ #include "object_detector.hpp" #include "inferenceWrapper.hpp" +#include +#include +#include namespace mrover { @@ -14,13 +17,171 @@ namespace mrover { cv::resize(imageView, sizedImage, cv::Size(640, 640)); cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGRA2BGR); + cv::Mat tempImage = sizedImage.clone(); sizedImage.convertTo(sizedImage, CV_32FC3); //cv::Mat blob = cv::dnn::blobFromImage(sizedImage); //std::cout << finalMat.elemSize() * 640 * 640 << std::endl; //std::cout << finalMat.data << " awdasda" << std::endl; mInferenceWrapper.doDetections(sizedImage); + + cv::Mat output = mInferenceWrapper.getOutputTensor(); + + int rows = output.rows; + int dimensions = output.cols; + + bool yolov8 = false; + // yolov5 has an output of shape (batchSize, 25200, 85) (Num classes + box[x,y,w,h] + confidence[c]) + // yolov8 has an output of shape (batchSize, 84, 8400) (Num classes + box[x,y,w,h]) + if (dimensions > rows) // Check if the shape[2] is more than shape[1] (yolov8) + { + yolov8 = true; + rows = output.cols; + dimensions = output.rows; + + output = output.reshape(1, dimensions); + cv::transpose(output, output); + } + float* data = (float*) output.data; + + //Model Information + float modelInputCols = 640; + float modelInputRows = 640; + float modelShapeWidth = 640; + float modelShapeHeight = 640; + + float modelConfidenceThresholdl = 0.25; + float modelScoreThreshold = 0.45; + float modelNMSThreshold = 0.50; + + + float x_factor = modelInputCols / modelShapeWidth; + float y_factor = modelInputRows / modelShapeHeight; + + std::vector class_ids; + std::vector confidences; + std::vector boxes; + + for (int i = 0; i < rows; ++i) { + if (yolov8) { + float* classes_scores = data + 4; + + cv::Mat scores(1, classes.size(), CV_32FC1, classes_scores); + cv::Point class_id; + double maxClassScore; + + minMaxLoc(scores, 0, &maxClassScore, 0, &class_id); + + if (maxClassScore > modelScoreThreshold) { + confidences.push_back(maxClassScore); + class_ids.push_back(class_id.x); + + float x = data[0]; + float y = data[1]; + float w = data[2]; + float h = data[3]; + + int left = int((x - 0.5 * w) * x_factor); + int top = int((y - 0.5 * h) * y_factor); + + int width = int(w * x_factor); + int height = int(h * y_factor); + + boxes.push_back(cv::Rect(left, top, width, height)); + } + } else // yolov5 + { + float confidence = data[4]; + + if (confidence >= modelConfidenceThresholdl) { + float* classes_scores = data + 5; + + cv::Mat scores(1, classes.size(), CV_32FC1, classes_scores); + cv::Point class_id; + double max_class_score; + + minMaxLoc(scores, 0, &max_class_score, 0, &class_id); + + if (max_class_score > modelScoreThreshold) { + confidences.push_back(confidence); + class_ids.push_back(class_id.x); + + float x = data[0]; + float y = data[1]; + float w = data[2]; + float h = data[3]; + + int left = int((x - 0.5 * w) * x_factor); + int top = int((y - 0.5 * h) * y_factor); + + int width = int(w * x_factor); + int height = int(h * y_factor); + + boxes.push_back(cv::Rect(left, top, width, height)); + } + } + } + + data += dimensions; + } + + std::vector nms_result; + cv::dnn::NMSBoxes(boxes, confidences, modelScoreThreshold, modelNMSThreshold, nms_result); + + std::vector detections{}; + for (unsigned long i = 0; i < nms_result.size(); ++i) { + int idx = nms_result[i]; + + Detection result; + result.class_id = class_ids[idx]; + result.confidence = confidences[idx]; + + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution dis(100, 255); + result.color = cv::Scalar(dis(gen), + dis(gen), + dis(gen)); + + result.className = classes[result.class_id]; + result.box = boxes[idx]; + + detections.push_back(result); + } + /* + cv::rectangle(tempImage, detections[0].box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); + + //Put the text on the image + cv::Point text_position(80, 80); + int font_size = 1; + cv::Scalar font_Color(0, 0, 0); + int font_weight = 2; + putText(tempImage, detections[0].className, text_position, cv::FONT_HERSHEY_COMPLEX, font_size, font_Color, font_weight); //Putting the text in the matrix// + */ + + //Show the image + cv::imshow("obj detector", tempImage); + cv::waitKey(1); + //Print the type of objected detected + + std::cout << detections[0].class_id + << " name" << std::endl; } - /* +} // namespace mrover +/* +cv::rectangle(sizedImg, box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); + + //Put the text on the image + cv::Point text_position(80, 80); + int font_size = 1; + cv::Scalar font_Color(0, 0, 0); + int font_weight = 2; + putText(sizedImg, msgData.object_type, text_position, cv::FONT_HERSHEY_COMPLEX, font_size, font_Color, font_weight); //Putting the text in the matrix// + + //Show the image + cv::imshow("obj detector", sizedImg); + cv::waitKey(1); + //Print the type of objected detected + ROS_INFO(firstDetection.className.c_str()); void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { //Ensure a valid message was received assert(msg); @@ -135,8 +296,8 @@ namespace mrover { // Publish to } */ - //Look at yolov8 documentation for the output matrix - /* +//Look at yolov8 documentation for the output matrix +/* * TODO(percep/obj-detector): * 0. Google "OpenCV DNN example." View a couple of tutorials. Most will be in Python but the C++ API is similar. * 1. Use cv::dnn::blobFromImage to convert the image to a blob @@ -145,4 +306,4 @@ namespace mrover { * 4. Publish the bounding boxes */ -} // namespace mrover +// namespace mrover From bc072bcfdbd759d25477e3657eb0bd41d8e01086 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 18 Nov 2023 13:24:21 -0500 Subject: [PATCH 083/197] Cleanup file names, use ROS as the logger --- .../{cudaWrapper.cuh => cuda_raii.cuh} | 0 .../{Inference.cu => inference.cu} | 2 +- src/perception/object_detector/inference.cuh | 4 +- .../object_detector/inferenceWrapper.cu | 40 ------------- .../object_detector/inferenceWrapper.hpp | 23 -------- src/perception/object_detector/ioHelper.cu | 57 ------------------- src/perception/object_detector/ioHelper.cuh | 33 ----------- src/perception/object_detector/logger.cu | 25 ++++++++ src/perception/object_detector/logger.cuh | 21 +++++++ .../object_detector/object_detector.cpp | 2 +- .../object_detector/object_detector.hpp | 2 +- .../object_detector.processing.cpp | 2 +- 12 files changed, 52 insertions(+), 159 deletions(-) rename src/perception/object_detector/{cudaWrapper.cuh => cuda_raii.cuh} (100%) rename src/perception/object_detector/{Inference.cu => inference.cu} (99%) delete mode 100644 src/perception/object_detector/inferenceWrapper.cu delete mode 100644 src/perception/object_detector/inferenceWrapper.hpp delete mode 100644 src/perception/object_detector/ioHelper.cu delete mode 100644 src/perception/object_detector/ioHelper.cuh create mode 100644 src/perception/object_detector/logger.cu create mode 100644 src/perception/object_detector/logger.cuh diff --git a/src/perception/object_detector/cudaWrapper.cuh b/src/perception/object_detector/cuda_raii.cuh similarity index 100% rename from src/perception/object_detector/cudaWrapper.cuh rename to src/perception/object_detector/cuda_raii.cuh diff --git a/src/perception/object_detector/Inference.cu b/src/perception/object_detector/inference.cu similarity index 99% rename from src/perception/object_detector/Inference.cu rename to src/perception/object_detector/inference.cu index 1908feef2..d4bee4b68 100644 --- a/src/perception/object_detector/Inference.cu +++ b/src/perception/object_detector/inference.cu @@ -7,7 +7,7 @@ #include #include -#include "ioHelper.cuh" +#include "logger.cuh" using namespace nvinfer1; diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index 52e455864..a1a91330f 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -5,8 +5,8 @@ #include -#include "cudaWrapper.cuh" -#include "ioHelper.cuh" +#include "cuda_raii.cuh" +#include "logger.cuh" using nvinfer1::ICudaEngine; using nvinfer1::IExecutionContext; diff --git a/src/perception/object_detector/inferenceWrapper.cu b/src/perception/object_detector/inferenceWrapper.cu deleted file mode 100644 index 032396e7e..000000000 --- a/src/perception/object_detector/inferenceWrapper.cu +++ /dev/null @@ -1,40 +0,0 @@ -#include "inferenceWrapper.hpp" - -#include -#include - -#include "inference.cuh" - -using namespace nvinfer1; - -/** -* Example Code: @link https://github.com/NVIDIA-developer-blog/code-samples/blob/master/posts/TensorRT-introduction/simpleOnnx_1.cpp -* IExecutionContest @link https://docs.nvidia.com/deeplearning/tensorrt/api/c_api/classnvinfer1_1_1_i_execution_context.html -* ------------------------------------------------------ -* For additional context see @link https://www.edge-ai-vision.com/2020/04/speeding-up-deep-learning-inference-using-tensorrt/ -*/ - - -/** -* cudaMemcpys CPU memory in inputTensor to GPU based on bindings -* Queues that tensor to be passed through model -* cudaMemcpys the result back to CPU memory -* Requires bindings, inputTensor, stream -* Modifies stream, outputTensor -*/ -namespace mrover { - - //Initialize the unique_ptr to the inference class - InferenceWrapper::InferenceWrapper(std::string onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string classesTxtFile = "") { - mInference.reset(new Inference(onnxModelPath, modelInputShape, classesTxtFile)); - } - - void InferenceWrapper::doDetections(const cv::Mat& img) { - mInference->doDetections(img); - } - - cv::Mat InferenceWrapper::getOutputTensor() { - return mInference->getOutputTensor(); - } - -} // namespace mrover diff --git a/src/perception/object_detector/inferenceWrapper.hpp b/src/perception/object_detector/inferenceWrapper.hpp deleted file mode 100644 index 2d0c09b09..000000000 --- a/src/perception/object_detector/inferenceWrapper.hpp +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once - -#include "pch.hpp" - -namespace mrover { - class Inference; - - class InferenceWrapper { - std::shared_ptr mInference; - - public: - InferenceWrapper() = default; - - ~InferenceWrapper() = default; - - InferenceWrapper(std::string onnxModelPath, cv::Size modelInputShape, std::string classesTxtFile); - - void doDetections(cv::Mat const& img); - - cv::Mat getOutputTensor(); - }; - -} // namespace mrover diff --git a/src/perception/object_detector/ioHelper.cu b/src/perception/object_detector/ioHelper.cu deleted file mode 100644 index 46e39ad29..000000000 --- a/src/perception/object_detector/ioHelper.cu +++ /dev/null @@ -1,57 +0,0 @@ -#include "ioHelper.cuh" - -using namespace std; - -namespace nvinfer1 { - - string getBasename(string const& path) { -#ifdef _WIN32 - constexpr char SEPARATOR = '\\'; -#else - constexpr char SEPARATOR = '/'; -#endif - int baseId = path.rfind(SEPARATOR) + 1; - return path.substr(baseId, path.rfind('.') - baseId); - } - - ostream& operator<<(ostream& o, const ILogger::Severity severity) { - switch (severity) { - case ILogger::Severity::kINTERNAL_ERROR: - o << "INTERNAL_ERROR"; - break; - case ILogger::Severity::kERROR: - o << "ERROR"; - break; - case ILogger::Severity::kWARNING: - o << "WARNING"; - break; - case ILogger::Severity::kINFO: - o << "INFO"; - break; - default: - break; - } - return o; - } - - void writeBuffer(void* buffer, size_t size, string const& path) { - ofstream stream(path.c_str(), ios::binary); - - if (stream) - stream.write(static_cast(buffer), size); - } - - // Returns empty string iff can't read the file - string readBuffer(string const& path) { - string buffer; - ifstream stream(path.c_str(), ios::binary); - - if (stream) { - stream >> noskipws; - copy(istream_iterator(stream), istream_iterator(), back_inserter(buffer)); - } - - return buffer; - } - -} // namespace nvinfer1 \ No newline at end of file diff --git a/src/perception/object_detector/ioHelper.cuh b/src/perception/object_detector/ioHelper.cuh deleted file mode 100644 index a9d296cc4..000000000 --- a/src/perception/object_detector/ioHelper.cuh +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -#include "pch.hpp" - -#include - -namespace nvinfer1 { - - std::ostream& operator<<(std::ostream& o, ILogger::Severity severity); - - class Logger : public nvinfer1::ILogger { - public: - void log(Severity severity, const char* msg) noexcept { - std::cerr << severity << ": " << msg << std::endl; - } - }; - - template - struct Destroy { - void operator()(T* t) const { - t->destroy(); - } - }; - - std::string getBasename(std::string const& path); - - size_t readTensor(std::vector const& tensorProtoPaths, std::vector& buffer); - - void writeBuffer(void* buffer, size_t size, std::string const& path); - - std::string readBuffer(std::string const& path); - -} // namespace nvinfer1 diff --git a/src/perception/object_detector/logger.cu b/src/perception/object_detector/logger.cu new file mode 100644 index 000000000..622bb8786 --- /dev/null +++ b/src/perception/object_detector/logger.cu @@ -0,0 +1,25 @@ +#include "logger.cuh" + +namespace nvinfer1 { + + auto Logger::log(Severity severity, char const* msg) noexcept -> void { + switch (severity) { + case Severity::kINTERNAL_ERROR: + ROS_FATAL_STREAM(msg); + break; + case Severity::kERROR: + ROS_ERROR_STREAM(msg); + break; + case Severity::kWARNING: + ROS_WARN_STREAM(msg); + break; + case Severity::kINFO: + ROS_INFO_STREAM(msg); + break; + case Severity::kVERBOSE: + ROS_DEBUG_STREAM(msg); + break; + } + } + +} // namespace nvinfer1 diff --git a/src/perception/object_detector/logger.cuh b/src/perception/object_detector/logger.cuh new file mode 100644 index 000000000..a051dd287 --- /dev/null +++ b/src/perception/object_detector/logger.cuh @@ -0,0 +1,21 @@ +#pragma once + +#include "pch.hpp" + +#include + +namespace nvinfer1 { + + class Logger final : public ILogger { + public: + auto log(Severity severity, char const* msg) noexcept -> void; + }; + + template + struct Destroy { + auto operator()(T* t) const -> void { + t->destroy(); + } + }; + +} // namespace nvinfer1 diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 33cdc85e7..162ff3bfb 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -1,6 +1,6 @@ #include "object_detector.hpp" -#include "inferenceWrapper.hpp" +#include "inference_wrapper.hpp" namespace mrover { diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 5b51a841a..238ba01b9 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -1,6 +1,6 @@ #include "pch.hpp" -#include "inferenceWrapper.hpp" +#include "inference_wrapper.hpp" #include #include diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index f0aa9da7a..d65681b04 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -1,6 +1,6 @@ #include "object_detector.hpp" -#include "inferenceWrapper.hpp" +#include "inference_wrapper.hpp" #include #include #include From f996b2d1ba959187005edfb6af99d330df3950fb Mon Sep 17 00:00:00 2001 From: Emerson-Ramey Date: Sun, 19 Nov 2023 06:48:34 +0000 Subject: [PATCH 084/197] initial files --- src/navigation/approach_object.py | 36 +++++++++++++++ src/navigation/approach_post.py | 32 ++++++++++++- src/navigation/approach_target_base.py | 63 ++++++++++++++++++++++++++ src/navigation/long_range.py | 37 +++++++++++++++ 4 files changed, 167 insertions(+), 1 deletion(-) create mode 100644 src/navigation/approach_object.py create mode 100644 src/navigation/approach_target_base.py create mode 100644 src/navigation/long_range.py diff --git a/src/navigation/approach_object.py b/src/navigation/approach_object.py new file mode 100644 index 000000000..a53eaca46 --- /dev/null +++ b/src/navigation/approach_object.py @@ -0,0 +1,36 @@ +import tf2_ros + +from util.ros_utils import get_rosparam +from util.state_lib.state import State +from typing import Optional + +from navigation import state +from navigation.approach_target_base import ApproachTargetBaseState + + +class ApproachObjectState(ApproachTargetBaseState): + """ + State for when we see an object (rubber mallet or water bottle). + We are only using the ZED camera. + Transitions: + -If arrived at target: DoneState + -Did not arrive at target: ApproachObjectState + -Arrived at the waypoint where the fiducial should be but have not seen it yet: SearchState + -Stuck? + """ + + def on_enter(self, context): + pass + + def on_exit(self, context): + pass + + def get_target_pos(self, context) -> Optional[int]: + object_pos = None # TODO: replace + return object_pos + + def determine_next(self, context, finished: bool) -> State: + if finished: + return state.DoneState() + else: + return self diff --git a/src/navigation/approach_post.py b/src/navigation/approach_post.py index b3ebfb676..a4c8d45c7 100644 --- a/src/navigation/approach_post.py +++ b/src/navigation/approach_post.py @@ -2,8 +2,38 @@ from util.ros_utils import get_rosparam from util.state_lib.state import State +from typing import Optional -from navigation import search, waypoint +from navigation import search, state, waypoint +from navigation.approach_target_base import ApproachTargetBaseState + + +class NewApproachPostState(ApproachTargetBaseState): + """ + State for when the tag is seen in the ZED camera. + Transitions: + -If arrived at target: DoneState + -Did not arrive at target: ApproachPostState + -Arrived at the waypoint where the fiducial should be but have not seen it yet: SearchState + -Stuck? + """ + + def on_enter(self, context): + pass + + def on_exit(self, context): + pass + + def get_target_pos(self, context) -> Optional[int]: + # return fid_pos, either position or None + fid_pos = context.env.current_fid_pos() + return fid_pos + + def determine_next(self, context, finished: bool) -> State: + if finished: + return state.DoneState() + else: + return self class ApproachPostState(State): diff --git a/src/navigation/approach_target_base.py b/src/navigation/approach_target_base.py new file mode 100644 index 000000000..ab28f4b76 --- /dev/null +++ b/src/navigation/approach_target_base.py @@ -0,0 +1,63 @@ +import tf2_ros + +from util.ros_utils import get_rosparam +from util.state_lib.state import State +from abc import abstractmethod +from typing import Optional + +from navigation import search + + +class ApproachTargetBaseState(State): + STOP_THRESH = get_rosparam("single_fiducial/stop_thresh", 0.7) + FIDUCIAL_STOP_THRESHOLD = get_rosparam("single_fiducial/fiducial_stop_threshold", 1.75) + DRIVE_FWD_THRESH = get_rosparam("waypoint/drive_fwd_thresh", 0.34) # 20 degrees + + def on_enter(self, context) -> None: + pass + + def on_exit(self, context) -> None: + pass + + @abstractmethod + def get_target_pos(self, context) -> Optional[int]: + raise NotImplementedError + + @abstractmethod + def determine_next(self, context, finished: bool) -> State: + raise NotImplementedError + + def on_loop(self, context) -> State: + """ + Drive towards a target based on what gets returned from get_target_pos(). + Return to search if there is no target position. + :param context: rover context + :return: Next state + """ + target_pos = self.get_target_pos(context) + if target_pos is None: + return search.SearchState() + + try: + cmd_vel, arrived = context.rover.driver.get_drive_command( + target_pos, + context.rover.get_pose(in_odom_frame=True), + self.STOP_THRESH, + self.DRIVE_FWD_THRESH, + in_odom=context.use_odom, + ) + next_state = self.determine_next(arrived) + if arrived: + context.env.arrived_at_post = True # TODO: change post to target? + context.env.last_post_location = self.get_target_pos() # TODO: change post to target? + context.course.increment_waypoint() + else: + context.rover.send_drive_command(cmd_vel) + except ( + tf2_ros.LookupException, + tf2_ros.ConnectivityException, + tf2_ros.ExtrapolationException, + ): + pass + + return next_state diff --git a/src/navigation/long_range.py b/src/navigation/long_range.py new file mode 100644 index 000000000..fe8641d2e --- /dev/null +++ b/src/navigation/long_range.py @@ -0,0 +1,37 @@ +import tf2_ros + +from util.ros_utils import get_rosparam +from util.state_lib.state import State +from abc import abstractmethod +from typing import Optional + +from navigation import approach_post +from navigation.approach_target_base import ApproachTargetBaseState + + +class LongRangeState(ApproachTargetBaseState): + """ + State for when the tag is seen only in the long range camera. + Transitions: + -If tag seen in ZED: ApproachPostState + -Not seen in ZED and moved: LongRangeState + -Don't see the tag in long range camera: SearchState + -Stuck? + """ + + def on_enter(self, context): + pass + + def on_exit(self, context): + pass + + def get_target_pos(self, context) -> Optional[int]: + target_pos = None # TODO: replace + return target_pos + + def determine_next(self, context, finished: bool) -> State: + fid_pos = context.env.current_fid_pos() + if fid_pos is None: + return self + else: + return approach_post.ApproachPostState() From a75a76364bcc1abda74a2a6ced93f8a3a237a2dd Mon Sep 17 00:00:00 2001 From: jbrhm Date: Sun, 19 Nov 2023 11:33:23 -0500 Subject: [PATCH 085/197] Bruh moment doesn't compile moment :( --- .../object_detector/object_detector.processing.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index d65681b04..59e0093be 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -17,7 +17,10 @@ namespace mrover { cv::resize(imageView, sizedImage, cv::Size(640, 640)); cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGRA2BGR); - cv::Mat tempImage = sizedImage.clone(); + + cv::imshow("sizedImage", sizedImage); + cv::waitKey(0); + sizedImage.convertTo(sizedImage, CV_32FC3); //cv::Mat blob = cv::dnn::blobFromImage(sizedImage); //std::cout << finalMat.elemSize() * 640 * 640 << std::endl; @@ -159,8 +162,6 @@ namespace mrover { */ //Show the image - cv::imshow("obj detector", tempImage); - cv::waitKey(1); //Print the type of objected detected std::cout << detections[0].class_id From 8de6b3f08f306498bf197b61dc4a4f1c10644b9e Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 19 Nov 2023 12:06:40 -0500 Subject: [PATCH 086/197] Oops --- .../object_detector/inference_wrapper.cu | 40 +++++++++++++++++++ .../object_detector/inference_wrapper.hpp | 23 +++++++++++ 2 files changed, 63 insertions(+) create mode 100644 src/perception/object_detector/inference_wrapper.cu create mode 100644 src/perception/object_detector/inference_wrapper.hpp diff --git a/src/perception/object_detector/inference_wrapper.cu b/src/perception/object_detector/inference_wrapper.cu new file mode 100644 index 000000000..f01046e85 --- /dev/null +++ b/src/perception/object_detector/inference_wrapper.cu @@ -0,0 +1,40 @@ +#include "inference_wrapper.hpp" + +#include +#include + +#include "inference.cuh" + +using namespace nvinfer1; + +/** +* Example Code: @link https://github.com/NVIDIA-developer-blog/code-samples/blob/master/posts/TensorRT-introduction/simpleOnnx_1.cpp +* IExecutionContest @link https://docs.nvidia.com/deeplearning/tensorrt/api/c_api/classnvinfer1_1_1_i_execution_context.html +* ------------------------------------------------------ +* For additional context see @link https://www.edge-ai-vision.com/2020/04/speeding-up-deep-learning-inference-using-tensorrt/ +*/ + + +/** +* cudaMemcpys CPU memory in inputTensor to GPU based on bindings +* Queues that tensor to be passed through model +* cudaMemcpys the result back to CPU memory +* Requires bindings, inputTensor, stream +* Modifies stream, outputTensor +*/ +namespace mrover { + + //Initialize the unique_ptr to the inference class + InferenceWrapper::InferenceWrapper(std::string onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string classesTxtFile = "") { + mInference.reset(new Inference(onnxModelPath, modelInputShape, classesTxtFile)); + } + + void InferenceWrapper::doDetections(const cv::Mat& img) { + mInference->doDetections(img); + } + + cv::Mat InferenceWrapper::getOutputTensor() { + return mInference->getOutputTensor(); + } + +} // namespace mrover diff --git a/src/perception/object_detector/inference_wrapper.hpp b/src/perception/object_detector/inference_wrapper.hpp new file mode 100644 index 000000000..2d0c09b09 --- /dev/null +++ b/src/perception/object_detector/inference_wrapper.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include "pch.hpp" + +namespace mrover { + class Inference; + + class InferenceWrapper { + std::shared_ptr mInference; + + public: + InferenceWrapper() = default; + + ~InferenceWrapper() = default; + + InferenceWrapper(std::string onnxModelPath, cv::Size modelInputShape, std::string classesTxtFile); + + void doDetections(cv::Mat const& img); + + cv::Mat getOutputTensor(); + }; + +} // namespace mrover From e7f7e9e268992529149da7cd1ef5e211d3cdbdd3 Mon Sep 17 00:00:00 2001 From: emmalinhart Date: Thu, 30 Nov 2023 20:03:18 -0500 Subject: [PATCH 087/197] camera shouldn't crash --- .../long_range_tag_detector.processing.cpp | 21 +++++++++++-------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp index f42653e6c..117364047 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp @@ -24,11 +24,11 @@ namespace mrover { // 3. We only want to publish the tags if the topic has subscribers if (mPublishImages && mImgPub.getNumSubscribers()) { // Draw the tags on the image using OpenCV - // publishTagsOnImage(); + //publishTagsOnImage(); } //Publish all tags that meet threshold - // publishThresholdTags(); + //publishThresholdTags(); //PUBLISH TAGS } @@ -57,25 +57,29 @@ namespace mrover { void LongRangeTagDetectorNodelet::updateHitCounts() { //loop through all identified IDs - for (size_t i = 0; i < mImmediateIds.size(); i++) + for (size_t i = 0; i < mImmediateIds.size(); i++) { updateNewlyIdentifiedTags(i); + } //Now decrement all the hitcounts for tags that were not updated // Set updated status to false - - for (auto& mTag: mTags) { - LongRangeTagStruct& currentTag = mTag.second; - + for (auto it = mTags.begin(); it != mTags.end();) { + LongRangeTagStruct& currentTag = it->second; + std::cout << mTags.size() << "!!!" << std::endl; if (currentTag.updated) { currentTag.updated = false; + it++; } else { //Decrement weight of undetected tags currentTag.hitCount -= mTagDecrementWeight; //if the value has fallen belown the minimum, remove it if (currentTag.hitCount <= mTagRemoveWeight) { - mTags.erase(mTag.first); + std::cout << "erasing" << std::endl; + it = mTags.erase(it); + } else { + it++; } } } @@ -108,7 +112,6 @@ namespace mrover { //Key exist sin mTags lrt.hitCount = std::min(mTags[currentId].hitCount + mTagIncrementWeight, mMaxHitCount); } - mTags[currentId] = lrt; } From 4f17971f5f444c887b717e42cd466a42b11cf8d6 Mon Sep 17 00:00:00 2001 From: emmalinhart Date: Sun, 3 Dec 2023 08:55:17 -0500 Subject: [PATCH 088/197] updated tag bearing function --- .../long_range_tag_detector.processing.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp index 117364047..30f36cecb 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp @@ -59,14 +59,13 @@ namespace mrover { //loop through all identified IDs for (size_t i = 0; i < mImmediateIds.size(); i++) { updateNewlyIdentifiedTags(i); + std::cout << "bearing: " << getTagBearing(mImmediateCorners[i]) << "!!!" << std::endl; } - //Now decrement all the hitcounts for tags that were not updated // Set updated status to false for (auto it = mTags.begin(); it != mTags.end();) { LongRangeTagStruct& currentTag = it->second; - std::cout << mTags.size() << "!!!" << std::endl; if (currentTag.updated) { currentTag.updated = false; it++; @@ -76,7 +75,6 @@ namespace mrover { //if the value has fallen belown the minimum, remove it if (currentTag.hitCount <= mTagRemoveWeight) { - std::cout << "erasing" << std::endl; it = mTags.erase(it); } else { it++; @@ -157,9 +155,10 @@ namespace mrover { float LongRangeTagDetectorNodelet::getTagBearing(std::vector& tagCorners) const { //for HD720 resolution cv::Point2f center = getTagCenterPixels(tagCorners); - float width = 2 * center.x; - float angleOfFOV = 104; - float bearing = ((center.x - width / 2) / width) * angleOfFOV; + auto imageWidth = (float) mImgMsg.width; + std::cout << "width: " << imageWidth << std::endl; + float angleOfFOV = 101; + float bearing = ((center.x - (imageWidth / 2)) / imageWidth) * angleOfFOV; return bearing; } From 45ed4afe0d39707a209570132651e6f222ef63f5 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 7 Dec 2023 13:37:00 -0500 Subject: [PATCH 089/197] LETS GOO TENSORRT WORKING, PARSING WORKING, DEBUG SHOWING WORKING :)))))))))) --- .../object_detector/object_detector.cpp | 3 - .../object_detector.processing.cpp | 67 +++++++++++++------ 2 files changed, 47 insertions(+), 23 deletions(-) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 162ff3bfb..22962997e 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -17,9 +17,6 @@ namespace mrover { mImgSub = mNh.subscribe("/camera/left/image", 1, &ObjectDetectorNodelet::imageCallback, this); mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); //mDetectionData = mNh.advertise("/object_detector/detected_object", 1); - - // TODO(quintin): I don't think this is the constructor you want. You should set channels via the type (e.g. "CV_32FC2" for 2 float channels) - mImageBlob = cv::Mat{1, NUM_CHANNELS, IMG_WIDTH, IMG_HEIGHT, CV_32F}; } } // namespace mrover diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 59e0093be..961b638ae 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -18,14 +18,10 @@ namespace mrover { cv::resize(imageView, sizedImage, cv::Size(640, 640)); cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGRA2BGR); - cv::imshow("sizedImage", sizedImage); - cv::waitKey(0); - - sizedImage.convertTo(sizedImage, CV_32FC3); - //cv::Mat blob = cv::dnn::blobFromImage(sizedImage); + cv::dnn::blobFromImage(sizedImage.clone(), mImageBlob, 1.0 / 255.0, cv::Size{640, 640}, cv::Scalar(), true, false); //std::cout << finalMat.elemSize() * 640 * 640 << std::endl; //std::cout << finalMat.data << " awdasda" << std::endl; - mInferenceWrapper.doDetections(sizedImage); + mInferenceWrapper.doDetections(mImageBlob); cv::Mat output = mInferenceWrapper.getOutputTensor(); @@ -150,22 +146,53 @@ namespace mrover { detections.push_back(result); } - /* - cv::rectangle(tempImage, detections[0].box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); - - //Put the text on the image - cv::Point text_position(80, 80); - int font_size = 1; - cv::Scalar font_Color(0, 0, 0); - int font_weight = 2; - putText(tempImage, detections[0].className, text_position, cv::FONT_HERSHEY_COMPLEX, font_size, font_Color, font_weight); //Putting the text in the matrix// - */ + if(detections.size() != 0) { + for(int i = 0; i < detections.size(); i++) { + std::cout << detections[i].className + << i << std::endl; + + + cv::rectangle(sizedImage, detections[i].box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); - //Show the image - //Print the type of objected detected + //Put the text on the image + cv::Point text_position(80, 80 * (i+1)); + int font_size = 1; + cv::Scalar font_Color(0, 0, 0); + int font_weight = 2; + putText(sizedImage, detections[i].className, text_position, cv::FONT_HERSHEY_COMPLEX, font_size, font_Color, font_weight); //Putting the text in the matrix// + } + //Show the image + if (mDebugImgPub.getNumSubscribers() > 0 || true) { + ROS_INFO("Publishing Debug Img"); - std::cout << detections[0].class_id - << " name" << std::endl; + // Create sensor msg image + sensor_msgs::Image newDebugImageMessage; + + cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGR2BGRA); + + newDebugImageMessage.height = sizedImage.rows; + newDebugImageMessage.width = sizedImage.cols; + + newDebugImageMessage.encoding = sensor_msgs::image_encodings::BGRA8; + + //Calculate the step for the imgMsg + newDebugImageMessage.step = sizedImage.channels() * sizedImage.cols; + newDebugImageMessage.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; + + // auto* bgrGpuPtr = sizedImage.getPtr(sl::MEM::GPU); + auto imgPtr = sizedImage.data; + + size_t size = newDebugImageMessage.step * newDebugImageMessage.height; + newDebugImageMessage.data.resize(size); + + memcpy(newDebugImageMessage.data.data(), imgPtr, size); + + mDebugImgPub.publish(newDebugImageMessage); + } + //Print the type of objected detected + + + } } } // namespace mrover /* From b2027cfd28ed93ea3a68d888a7115527737f8e89 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 7 Dec 2023 13:37:45 -0500 Subject: [PATCH 090/197] Publish Nav Data and Optimized --- src/perception/object_detector/inference.cu | 6 ++-- .../object_detector.processing.cpp | 35 +++++++++++++++---- 2 files changed, 31 insertions(+), 10 deletions(-) diff --git a/src/perception/object_detector/inference.cu b/src/perception/object_detector/inference.cu index d4bee4b68..18aa65f3b 100644 --- a/src/perception/object_detector/inference.cu +++ b/src/perception/object_detector/inference.cu @@ -89,13 +89,13 @@ namespace mrover { //Create runtime engine IRuntime* runtime = createInferRuntime(mLogger); - std::filesystem::path enginePath("./tensorrt-engine.engine"); + std::filesystem::path enginePath("./tensorrt-engine-best.engine"); //Check if engine file exists if (exists(enginePath)) { // TODO: error checking //Load engine from file - std::ifstream inputFileStream("./tensorrt-engine.engine", std::ios::binary); + std::ifstream inputFileStream("./tensorrt-engine-best.engine", std::ios::binary); std::stringstream engineBuffer; engineBuffer << inputFileStream.rdbuf(); @@ -112,7 +112,7 @@ namespace mrover { //Save Engine to File auto trtModelStream = tempEng->serialize(); - std::ofstream outputFileStream("./tensorrt-engine.engine", std::ios::binary); + std::ofstream outputFileStream("./tensorrt-engine-best.engine", std::ios::binary); outputFileStream.write(static_cast(trtModelStream->data()), trtModelStream->size()); outputFileStream.close(); diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 961b638ae..4396ea97c 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -18,7 +18,7 @@ namespace mrover { cv::resize(imageView, sizedImage, cv::Size(640, 640)); cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGRA2BGR); - cv::dnn::blobFromImage(sizedImage.clone(), mImageBlob, 1.0 / 255.0, cv::Size{640, 640}, cv::Scalar(), true, false); + cv::dnn::blobFromImage(sizedImage, mImageBlob, 1.0 / 255.0, cv::Size{640, 640}, cv::Scalar(), true, false); //std::cout << finalMat.elemSize() * 640 * 640 << std::endl; //std::cout << finalMat.data << " awdasda" << std::endl; mInferenceWrapper.doDetections(mImageBlob); @@ -146,16 +146,39 @@ namespace mrover { detections.push_back(result); } - if(detections.size() != 0) { - for(int i = 0; i < detections.size(); i++) { + if (!detections.empty()) { + Detection firstDetection = detections[0]; + + float classConfidence = 0.0; + cv::Rect box = firstDetection.box; + + DetectedObject msgData; + msgData.object_type = firstDetection.className; + msgData.detection_confidence = classConfidence; + + msgData.width = static_cast(box.width); + msgData.height = static_cast(box.height); + + //Get the heading + float objectHeading; + float zedFOV = 54; //54 @ 720; 42 @ 1080 + float fovPerPixel = (float) zedFOV / (float) (msg->width); + float xCenter = (float) box.x + ((float) box.width) / 2 - ((float) msg->width) / 2; + objectHeading = xCenter * fovPerPixel; + msgData.bearing = objectHeading; + + //publish the data to NAV + mDetectionData.publish(msgData); + + for (int i = 0; i < detections.size(); i++) { std::cout << detections[i].className - << i << std::endl; + << i << std::endl; cv::rectangle(sizedImage, detections[i].box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); //Put the text on the image - cv::Point text_position(80, 80 * (i+1)); + cv::Point text_position(80, 80 * (i + 1)); int font_size = 1; cv::Scalar font_Color(0, 0, 0); int font_weight = 2; @@ -190,8 +213,6 @@ namespace mrover { mDebugImgPub.publish(newDebugImageMessage); } //Print the type of objected detected - - } } } // namespace mrover From 63f2acea4dca5939ca9971753792c24d70bafb63 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 7 Dec 2023 13:38:16 -0500 Subject: [PATCH 091/197] Parsing Dimensions + Names :)) --- src/perception/object_detector/inference.cu | 23 +++++++++++++++---- .../object_detector/object_detector.cpp | 2 +- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/perception/object_detector/inference.cu b/src/perception/object_detector/inference.cu index 18aa65f3b..87b9727ee 100644 --- a/src/perception/object_detector/inference.cu +++ b/src/perception/object_detector/inference.cu @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -28,6 +29,9 @@ using namespace nvinfer1; */ namespace mrover { + constexpr static std::string_view INPUT_BINDING_NAME = "images"; + constexpr static std::string_view OUTPUT_BINDING_NAME = "output0"; + //Constructor // : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} Inference::Inference(std::string const& onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string const& classesTxtFile = "") { @@ -37,17 +41,18 @@ namespace mrover { mLogger.log(ILogger::Severity::kINFO, "Created CUDA Engine"); - // TODO: these are deprecated - assert(mEngine->getNbBindings() == 2); - assert(mEngine->bindingIsInput(0) ^ mEngine->bindingIsInput(1)); + //Check some assumptions + if(mEngine->getNbBindings() != 2) throw std::runtime_error("Invalid Binding Count"); + if(mEngine->getTensorIOMode(INPUT_BINDING_NAME.data()) != TensorIOMode::kINPUT) throw std::runtime_error("Expected Input Binding 0 Is An Input"); + if(mEngine->getTensorIOMode(OUTPUT_BINDING_NAME.data()) != TensorIOMode::kOUTPUT) throw std::runtime_error("Expected Input Binding Input To Be 1"); mStream.emplace(); mLogger.log(ILogger::Severity::kINFO, "Created CUDA stream"); - prepTensors(); - setUpContext(); + + prepTensors(); } // Initializes enginePtr with built engine @@ -175,6 +180,14 @@ namespace mrover { } // TODO(quintin): Avoid hardcoding this + assert(mContext); + Dims inputTensorDims = mEngine->getTensorShape(OUTPUT_BINDING_NAME.data()); + for (int i = 0; i < inputTensorDims.nbDims; i++) { + char message[512]; + std::snprintf(message, sizeof(message), "size %d %d", i, inputTensorDims.d[i]); + mLogger.log(nvinfer1::ILogger::Severity::kINFO, message); + } + mOutputTensor = cv::Mat::zeros(84, 8400, CV_MAKE_TYPE(CV_32F, 1)); } diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 22962997e..2551cb77b 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -7,7 +7,7 @@ namespace mrover { void ObjectDetectorNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mInferenceWrapper = InferenceWrapper("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); + mInferenceWrapper = InferenceWrapper("//home//jabra//Desktop//Rover//best.onnx", cv::Size(640, 640), ""); //inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //read ONNX file into this mNet, YOLOV8, second smallest one From 85be0790bd35bcfe5a96f633c40ddb7486a9a7a9 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 7 Dec 2023 13:38:49 -0500 Subject: [PATCH 092/197] Not finished Tidying --- src/perception/object_detector/Inference.cpp | 51 ------------------- src/perception/object_detector/cuda_raii.cuh | 4 +- src/perception/object_detector/inference.cu | 44 +++++++--------- src/perception/object_detector/inference.cuh | 14 ++--- .../object_detector/inference_wrapper.cu | 14 ++--- .../object_detector/inference_wrapper.hpp | 6 +-- src/perception/object_detector/logger.cuh | 4 +- .../object_detector/object_detector.cpp | 6 +-- .../object_detector/object_detector.hpp | 2 +- .../object_detector.processing.cpp | 48 ++++++++--------- 10 files changed, 62 insertions(+), 131 deletions(-) delete mode 100644 src/perception/object_detector/Inference.cpp diff --git a/src/perception/object_detector/Inference.cpp b/src/perception/object_detector/Inference.cpp deleted file mode 100644 index c9b6a6fa3..000000000 --- a/src/perception/object_detector/Inference.cpp +++ /dev/null @@ -1,51 +0,0 @@ -#include "pch.hpp" - -namespace mrover { - - /* - -// Initializes enginePtr with built engine -void Inference::createCudaEngine(std::string& onnxModelPath) { - // See link sfor additional context - const auto explicitBatch = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); - std::unique_ptr> builder{nvinfer1::createInferBuilder(logger)}; - std::unique_ptr> network{builder->createNetworkV2(explicitBatch)}; - std::unique_ptr> parser{nvonnxparser::createParser(*network, logger)}; - std::unique_ptr> config{builder->createBuilderConfig()}; - - //Parse the onnx from file - if (!parser->parseFromFile(onnxModelPath.c_str(), static_cast(ILogger::Severity::kINFO))) { - std::cout << "ERROR: could not parse input engine." << std::endl; - } - - config->setMemoryPoolLimit(MemoryPoolType::kWORKSPACE, 1 << 30); - - auto profile = builder->createOptimizationProfile(); - profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMIN, Dims4{1, 3, 256, 256}); - profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kOPT, Dims4{1, 3, 256, 256}); - profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMAX, Dims4{32, 3, 256, 256}); - config->addOptimizationProfile(profile); - - this->enginePtr = builder->buildSerializedNetwork(*network, *config); -} - -std::vector Inference::doDetections(cv::Mat& img) { - //Do the forward pass on the network - launchInference(img.data, this->outputTensor.data, ); - - return Parser(this->outputTensor).parseTensor(); -} -*/ - - /* -void Inference::setUpContext() { - // Create Execution Context. - this->context.reset(this->enginePtr->createExecutionContext()); - - Dims dims_i{this->enginePtr->getBindingDimensions(0)}; - Dims4 inputDims{Inference::BATCH_SIZE, dims_i.d[1], dims_i.d[2], dims_i.d[3]}; - context->setBindingDimensions(0, inputDims); -} -*/ - -} // namespace mrover \ No newline at end of file diff --git a/src/perception/object_detector/cuda_raii.cuh b/src/perception/object_detector/cuda_raii.cuh index 9ffeacc7d..eda9ff60a 100644 --- a/src/perception/object_detector/cuda_raii.cuh +++ b/src/perception/object_detector/cuda_raii.cuh @@ -43,7 +43,7 @@ namespace cudawrapper { check(cudaStreamCreate(&mStream)); } - operator cudaStream_t() { + operator cudaStream_t() const { return mStream; } @@ -61,7 +61,7 @@ namespace cudawrapper { check(cudaEventCreate(&mEvent)); } - operator cudaEvent_t() { + operator cudaEvent_t() const { return mEvent; } diff --git a/src/perception/object_detector/inference.cu b/src/perception/object_detector/inference.cu index 87b9727ee..df8a15cf5 100644 --- a/src/perception/object_detector/inference.cu +++ b/src/perception/object_detector/inference.cu @@ -10,17 +10,7 @@ #include "logger.cuh" -using namespace nvinfer1; - -/** -* Example Code: @link https://github.com/NVIDIA-developer-blog/code-samples/blob/master/posts/TensorRT-introduction/simpleOnnx_1.cpp -* IExecutionContest @link https://docs.nvidia.com/deeplearning/tensorrt/api/c_api/classnvinfer1_1_1_i_execution_context.html -* ------------------------------------------------------ -* For additional context see @link https://www.edge-ai-vision.com/2020/04/speeding-up-deep-learning-inference-using-tensorrt/ -*/ - - -/** +using namespace nvinfer1;/** * cudaMemcpys CPU memory in inputTensor to GPU based on bindings * Queues that tensor to be passed through model * cudaMemcpys the result back to CPU memory @@ -29,8 +19,8 @@ using namespace nvinfer1; */ namespace mrover { - constexpr static std::string_view INPUT_BINDING_NAME = "images"; - constexpr static std::string_view OUTPUT_BINDING_NAME = "output0"; + constexpr static const char * INPUT_BINDING_NAME = "images"; + constexpr static const char * OUTPUT_BINDING_NAME = "output0"; //Constructor // : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} @@ -42,9 +32,9 @@ namespace mrover { mLogger.log(ILogger::Severity::kINFO, "Created CUDA Engine"); //Check some assumptions - if(mEngine->getNbBindings() != 2) throw std::runtime_error("Invalid Binding Count"); - if(mEngine->getTensorIOMode(INPUT_BINDING_NAME.data()) != TensorIOMode::kINPUT) throw std::runtime_error("Expected Input Binding 0 Is An Input"); - if(mEngine->getTensorIOMode(OUTPUT_BINDING_NAME.data()) != TensorIOMode::kOUTPUT) throw std::runtime_error("Expected Input Binding Input To Be 1"); + if(mEngine->getNbIOTensors() != 2) throw std::runtime_error("Invalid Binding Count"); + if(mEngine->getTensorIOMode(INPUT_BINDING_NAME) != TensorIOMode::kINPUT) throw std::runtime_error("Expected Input Binding 0 Is An Input"); + if(mEngine->getTensorIOMode(OUTPUT_BINDING_NAME) != TensorIOMode::kOUTPUT) throw std::runtime_error("Expected Input Binding Input To Be 1"); mStream.emplace(); @@ -106,7 +96,7 @@ namespace mrover { engineBuffer << inputFileStream.rdbuf(); std::string enginePlan = engineBuffer.str(); // TODO: deprecated - return runtime->deserializeCudaEngine(enginePlan.data(), enginePlan.size(), nullptr); + return runtime->deserializeCudaEngine(enginePlan.data(), enginePlan.size()); } else { IHostMemory* serializedEngine = builder->buildSerializedNetwork(*network, *config); if (!serializedEngine) throw std::runtime_error("Failed to serialize engine"); @@ -129,10 +119,10 @@ namespace mrover { // Create Execution Context. mContext.reset(mEngine->createExecutionContext()); - mContext->setBindingDimensions(0, mEngine->getBindingDimensions(0)); + mContext->setInputShape(INPUT_BINDING_NAME, mEngine->getTensorShape(INPUT_BINDING_NAME)); } - void Inference::doDetections(const cv::Mat& img) { + void Inference::doDetections(const cv::Mat& img) const{ //Do the forward pass on the network launchInference(img, mOutputTensor); ROS_INFO_STREAM(static_cast(mOutputTensor.data)); @@ -144,7 +134,7 @@ namespace mrover { } - void Inference::launchInference(cv::Mat const& input, cv::Mat const& output) { + void Inference::launchInference(cv::Mat const& input, cv::Mat const& output) const { assert(!input.empty()); assert(!output.empty()); assert(input.isContinuous()); @@ -152,7 +142,7 @@ namespace mrover { assert(mContext); assert(mStream); - int inputId = getBindingInputIndex(mContext.get()); + const int inputId = getBindingInputIndex(mContext.get()); cudaMemcpy(mBindings[inputId], input.data, input.total() * input.elemSize(), cudaMemcpyHostToDevice); mContext->executeV2(mBindings.data()); @@ -174,24 +164,24 @@ namespace mrover { // Multiply sizeof(float) by the product of the extents // This is essentially: element count * size of each element - std::size_t size = std::reduce(extents, extents + rank, sizeof(float), std::multiplies<>()); + const std::size_t size = std::reduce(extents, extents + rank, sizeof(float), std::multiplies<>()); // Create GPU memory for TensorRT to operate on cudaMalloc(mBindings.data() + i, size); } // TODO(quintin): Avoid hardcoding this assert(mContext); - Dims inputTensorDims = mEngine->getTensorShape(OUTPUT_BINDING_NAME.data()); - for (int i = 0; i < inputTensorDims.nbDims; i++) { + const Dims outputTensorDims = mEngine->getTensorShape(OUTPUT_BINDING_NAME); + for (int i = 0; i < outputTensorDims.nbDims; i++) { char message[512]; - std::snprintf(message, sizeof(message), "size %d %d", i, inputTensorDims.d[i]); + std::snprintf(message, sizeof(message), "size %d %d", i, outputTensorDims.d[i]); mLogger.log(nvinfer1::ILogger::Severity::kINFO, message); } - mOutputTensor = cv::Mat::zeros(84, 8400, CV_MAKE_TYPE(CV_32F, 1)); + mOutputTensor = cv::Mat::zeros(outputTensorDims.d[1], outputTensorDims.d[2], CV_MAKE_TYPE(CV_32F, 1)); } - int Inference::getBindingInputIndex(IExecutionContext* context) { + int Inference::getBindingInputIndex(const IExecutionContext* context) { return context->getEngine().getTensorIOMode(context->getEngine().getIOTensorName(0)) != TensorIOMode::kINPUT; // 0 (false) if bindingIsInput(0), 1 (true) otherwise } } // namespace mrover diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index a1a91330f..adb63bd6e 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -20,30 +20,30 @@ namespace mrover { nvinfer1::Logger mLogger; //Ptr to the engine - std::unique_ptr> mEngine; + std::unique_ptr> mEngine{}; //Ptr to the context - std::unique_ptr> mContext; + std::unique_ptr> mContext{}; //Input, output and reference tensors cv::Mat mInputTensor; cv::Mat mOutputTensor; //Cuda Stream - std::optional mStream; + std::optional mStream{}; //Bindings std::array mBindings{}; //ONNX Model Path - std::string mOnnxModelPath; + std::string mOnnxModelPath{}; //Size of Model cv::Size mModelInputShape; cv::Size mModelOutputShape; //STATIC FUNCTIONS - static int getBindingInputIndex(IExecutionContext* context); + static int getBindingInputIndex(const IExecutionContext* context); public: Inference(std::string const& onnxModelPath, cv::Size modelInputShape, std::string const& classesTxtFile); @@ -52,14 +52,14 @@ namespace mrover { //Creates a ptr to the engine ICudaEngine* createCudaEngine(std::string const& onnxModelPath); - void launchInference(cv::Mat const& input, cv::Mat const& output); + void launchInference(cv::Mat const& input, cv::Mat const& output) const; void prepTensors(); void setUpContext(); public: - void doDetections(const cv::Mat& img); + void doDetections(const cv::Mat& img) const; cv::Mat getOutputTensor(); }; diff --git a/src/perception/object_detector/inference_wrapper.cu b/src/perception/object_detector/inference_wrapper.cu index f01046e85..24de09db6 100644 --- a/src/perception/object_detector/inference_wrapper.cu +++ b/src/perception/object_detector/inference_wrapper.cu @@ -7,12 +7,6 @@ using namespace nvinfer1; -/** -* Example Code: @link https://github.com/NVIDIA-developer-blog/code-samples/blob/master/posts/TensorRT-introduction/simpleOnnx_1.cpp -* IExecutionContest @link https://docs.nvidia.com/deeplearning/tensorrt/api/c_api/classnvinfer1_1_1_i_execution_context.html -* ------------------------------------------------------ -* For additional context see @link https://www.edge-ai-vision.com/2020/04/speeding-up-deep-learning-inference-using-tensorrt/ -*/ /** @@ -25,15 +19,15 @@ using namespace nvinfer1; namespace mrover { //Initialize the unique_ptr to the inference class - InferenceWrapper::InferenceWrapper(std::string onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string classesTxtFile = "") { - mInference.reset(new Inference(onnxModelPath, modelInputShape, classesTxtFile)); + InferenceWrapper::InferenceWrapper(std::string onnxModelPath, cv::Size const modelInputShape = {640, 640}, std::string classesTxtFile = "") : mInference({}) { + mInference.reset(new Inference(std::move(onnxModelPath), modelInputShape, std::move(classesTxtFile))); } - void InferenceWrapper::doDetections(const cv::Mat& img) { + void InferenceWrapper::doDetections(const cv::Mat& img) const { mInference->doDetections(img); } - cv::Mat InferenceWrapper::getOutputTensor() { + cv::Mat InferenceWrapper::getOutputTensor() const { return mInference->getOutputTensor(); } diff --git a/src/perception/object_detector/inference_wrapper.hpp b/src/perception/object_detector/inference_wrapper.hpp index 2d0c09b09..3c19dc1d6 100644 --- a/src/perception/object_detector/inference_wrapper.hpp +++ b/src/perception/object_detector/inference_wrapper.hpp @@ -13,11 +13,11 @@ namespace mrover { ~InferenceWrapper() = default; - InferenceWrapper(std::string onnxModelPath, cv::Size modelInputShape, std::string classesTxtFile); + InferenceWrapper(std::string onnxModelPath, cv::Size const modelInputShape, std::string classesTxtFile); - void doDetections(cv::Mat const& img); + void doDetections(cv::Mat const& img) const; - cv::Mat getOutputTensor(); + cv::Mat getOutputTensor() const; }; } // namespace mrover diff --git a/src/perception/object_detector/logger.cuh b/src/perception/object_detector/logger.cuh index a051dd287..29db705c0 100644 --- a/src/perception/object_detector/logger.cuh +++ b/src/perception/object_detector/logger.cuh @@ -1,14 +1,12 @@ #pragma once -#include "pch.hpp" - #include namespace nvinfer1 { class Logger final : public ILogger { public: - auto log(Severity severity, char const* msg) noexcept -> void; + void log(Severity severity, char const* msg) noexcept override; }; template diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 2551cb77b..46420d580 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -1,5 +1,5 @@ #include "object_detector.hpp" - +#include "pch.hpp" #include "inference_wrapper.hpp" namespace mrover { @@ -7,7 +7,7 @@ namespace mrover { void ObjectDetectorNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mInferenceWrapper = InferenceWrapper("//home//jabra//Desktop//Rover//best.onnx", cv::Size(640, 640), ""); + mInferenceWrapper = InferenceWrapper(std::string("//home//jabra//Desktop//Rover//best.onnx"), cv::Size(640, 640), ""); //inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //read ONNX file into this mNet, YOLOV8, second smallest one @@ -16,7 +16,7 @@ namespace mrover { mImgSub = mNh.subscribe("/camera/left/image", 1, &ObjectDetectorNodelet::imageCallback, this); mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); - //mDetectionData = mNh.advertise("/object_detector/detected_object", 1); + mDetectionData = mNh.advertise("/object_detector/detected_object", 1); } } // namespace mrover diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 238ba01b9..45ffb74f8 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -20,7 +20,7 @@ namespace mrover { constexpr static int IMG_WIDTH = 1280; constexpr static int IMG_HEIGHT = 720; - std::vector classes{"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; + std::vector classes{"Hammer"};//{"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; ros::NodeHandle mNh, mPnh; diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 4396ea97c..468407a07 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -19,8 +19,7 @@ namespace mrover { cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGRA2BGR); cv::dnn::blobFromImage(sizedImage, mImageBlob, 1.0 / 255.0, cv::Size{640, 640}, cv::Scalar(), true, false); - //std::cout << finalMat.elemSize() * 640 * 640 << std::endl; - //std::cout << finalMat.data << " awdasda" << std::endl; + mInferenceWrapper.doDetections(mImageBlob); cv::Mat output = mInferenceWrapper.getOutputTensor(); @@ -48,8 +47,8 @@ namespace mrover { float modelShapeWidth = 640; float modelShapeHeight = 640; - float modelConfidenceThresholdl = 0.25; - float modelScoreThreshold = 0.45; + float modelConfidenceThresholdl = 0.9; + float modelScoreThreshold = 0.90; float modelNMSThreshold = 0.50; @@ -170,7 +169,7 @@ namespace mrover { //publish the data to NAV mDetectionData.publish(msgData); - for (int i = 0; i < detections.size(); i++) { + for (size_t i = 0; i < detections.size(); i++) { std::cout << detections[i].className << i << std::endl; @@ -185,34 +184,35 @@ namespace mrover { putText(sizedImage, detections[i].className, text_position, cv::FONT_HERSHEY_COMPLEX, font_size, font_Color, font_weight); //Putting the text in the matrix// } //Show the image - if (mDebugImgPub.getNumSubscribers() > 0 || true) { - ROS_INFO("Publishing Debug Img"); - // Create sensor msg image - sensor_msgs::Image newDebugImageMessage; + //Print the type of objected detected + } + if (mDebugImgPub.getNumSubscribers() > 0 || true) { + ROS_INFO("Publishing Debug Img"); - cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGR2BGRA); + // Create sensor msg image + sensor_msgs::Image newDebugImageMessage; - newDebugImageMessage.height = sizedImage.rows; - newDebugImageMessage.width = sizedImage.cols; + cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGR2BGRA); - newDebugImageMessage.encoding = sensor_msgs::image_encodings::BGRA8; + newDebugImageMessage.height = sizedImage.rows; + newDebugImageMessage.width = sizedImage.cols; - //Calculate the step for the imgMsg - newDebugImageMessage.step = sizedImage.channels() * sizedImage.cols; - newDebugImageMessage.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; + newDebugImageMessage.encoding = sensor_msgs::image_encodings::BGRA8; - // auto* bgrGpuPtr = sizedImage.getPtr(sl::MEM::GPU); - auto imgPtr = sizedImage.data; + //Calculate the step for the imgMsg + newDebugImageMessage.step = sizedImage.channels() * sizedImage.cols; + newDebugImageMessage.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; - size_t size = newDebugImageMessage.step * newDebugImageMessage.height; - newDebugImageMessage.data.resize(size); + // auto* bgrGpuPtr = sizedImage.getPtr(sl::MEM::GPU); + auto imgPtr = sizedImage.data; - memcpy(newDebugImageMessage.data.data(), imgPtr, size); + size_t size = newDebugImageMessage.step * newDebugImageMessage.height; + newDebugImageMessage.data.resize(size); - mDebugImgPub.publish(newDebugImageMessage); - } - //Print the type of objected detected + memcpy(newDebugImageMessage.data.data(), imgPtr, size); + + mDebugImgPub.publish(newDebugImageMessage); } } } // namespace mrover From 57adaced7b77ad743be0b1423a1b19fa0e96683f Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 7 Dec 2023 13:38:56 -0500 Subject: [PATCH 093/197] Cleaned Code --- src/perception/object_detector/logger.cu | 1 + .../object_detector/object_detector.hpp | 2 +- .../object_detector.processing.cpp | 49 ++++++----- src/perception/object_detector/parser.cpp.txt | 81 ------------------- src/perception/object_detector/parser.hpp | 41 ---------- 5 files changed, 26 insertions(+), 148 deletions(-) delete mode 100644 src/perception/object_detector/parser.cpp.txt delete mode 100644 src/perception/object_detector/parser.hpp diff --git a/src/perception/object_detector/logger.cu b/src/perception/object_detector/logger.cu index 622bb8786..2abd2ebf8 100644 --- a/src/perception/object_detector/logger.cu +++ b/src/perception/object_detector/logger.cu @@ -1,4 +1,5 @@ #include "logger.cuh" +#include "pch.hpp" namespace nvinfer1 { diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 45ffb74f8..31a465802 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -1,4 +1,4 @@ -#include "pch.hpp" +#pragma once #include "inference_wrapper.hpp" #include diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 468407a07..c0cc21a35 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -39,7 +39,7 @@ namespace mrover { output = output.reshape(1, dimensions); cv::transpose(output, output); } - float* data = (float*) output.data; + auto data = reinterpret_cast(output.data); //Model Information float modelInputCols = 640; @@ -47,7 +47,6 @@ namespace mrover { float modelShapeWidth = 640; float modelShapeHeight = 640; - float modelConfidenceThresholdl = 0.9; float modelScoreThreshold = 0.90; float modelNMSThreshold = 0.50; @@ -60,17 +59,18 @@ namespace mrover { std::vector boxes; for (int i = 0; i < rows; ++i) { + if (yolov8) { float* classes_scores = data + 4; - cv::Mat scores(1, classes.size(), CV_32FC1, classes_scores); + cv::Mat scores(1, static_cast(classes.size()), CV_32FC1, classes_scores); cv::Point class_id; double maxClassScore; - minMaxLoc(scores, 0, &maxClassScore, 0, &class_id); + minMaxLoc(scores, nullptr, &maxClassScore, nullptr, &class_id); if (maxClassScore > modelScoreThreshold) { - confidences.push_back(maxClassScore); + confidences.push_back(static_cast(maxClassScore)); class_ids.push_back(class_id.x); float x = data[0]; @@ -78,26 +78,27 @@ namespace mrover { float w = data[2]; float h = data[3]; - int left = int((x - 0.5 * w) * x_factor); - int top = int((y - 0.5 * h) * y_factor); + int left = static_cast((x - 0.5 * w) * x_factor); + int top = static_cast((y - 0.5 * h) * y_factor); - int width = int(w * x_factor); - int height = int(h * y_factor); + int width = static_cast(w * x_factor); + int height = static_cast(h * y_factor); - boxes.push_back(cv::Rect(left, top, width, height)); + boxes.emplace_back(left, top, width, height); } } else // yolov5 { - float confidence = data[4]; - if (confidence >= modelConfidenceThresholdl) { + float modelConfidenceThresholdl = 0.9; + + if (float confidence = data[4]; confidence >= modelConfidenceThresholdl) { float* classes_scores = data + 5; - cv::Mat scores(1, classes.size(), CV_32FC1, classes_scores); + cv::Mat scores(1, static_cast(classes.size()), CV_32FC1, classes_scores); cv::Point class_id; double max_class_score; - minMaxLoc(scores, 0, &max_class_score, 0, &class_id); + minMaxLoc(scores, nullptr, &max_class_score, nullptr, &class_id); if (max_class_score > modelScoreThreshold) { confidences.push_back(confidence); @@ -108,13 +109,13 @@ namespace mrover { float w = data[2]; float h = data[3]; - int left = int((x - 0.5 * w) * x_factor); - int top = int((y - 0.5 * h) * y_factor); + int left = static_cast((x - 0.5 * w) * x_factor); + int top = static_cast((y - 0.5 * h) * y_factor); - int width = int(w * x_factor); - int height = int(h * y_factor); + int width = static_cast(w * x_factor); + int height = static_cast(h * y_factor); - boxes.push_back(cv::Rect(left, top, width, height)); + boxes.emplace_back(left, top, width, height); } } } @@ -126,9 +127,7 @@ namespace mrover { cv::dnn::NMSBoxes(boxes, confidences, modelScoreThreshold, modelNMSThreshold, nms_result); std::vector detections{}; - for (unsigned long i = 0; i < nms_result.size(); ++i) { - int idx = nms_result[i]; - + for(int idx : nms_result) { Detection result; result.class_id = class_ids[idx]; result.confidence = confidences[idx]; @@ -161,8 +160,8 @@ namespace mrover { //Get the heading float objectHeading; float zedFOV = 54; //54 @ 720; 42 @ 1080 - float fovPerPixel = (float) zedFOV / (float) (msg->width); - float xCenter = (float) box.x + ((float) box.width) / 2 - ((float) msg->width) / 2; + float fovPerPixel = (float) zedFOV / static_cast(msg->width); + float xCenter = static_cast(box.x) + (static_cast(box.width)) / 2 - (static_cast(msg->width)) / 2; objectHeading = xCenter * fovPerPixel; msgData.bearing = objectHeading; @@ -177,7 +176,7 @@ namespace mrover { cv::rectangle(sizedImage, detections[i].box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); //Put the text on the image - cv::Point text_position(80, 80 * (i + 1)); + cv::Point text_position(80, static_cast(80 * (i + 1))); int font_size = 1; cv::Scalar font_Color(0, 0, 0); int font_weight = 2; diff --git a/src/perception/object_detector/parser.cpp.txt b/src/perception/object_detector/parser.cpp.txt deleted file mode 100644 index 18129d2b4..000000000 --- a/src/perception/object_detector/parser.cpp.txt +++ /dev/null @@ -1,81 +0,0 @@ -#include "parser.hpp" -namespace mrover { - - - Parser::Parser(std::vector& tensor) { - this->tensor = tensor; - } - - std::vector Parser::parseTensor(cv::Mat& outputTensor) { - int cols = 8400; //Number of outputs in model - int dimension = 84; //80 + 4 - int numClasses = 80; //The number of possible classes - - //Output vector of detections - std::vector detections; - - //Intermediary vectors of data - std::vector class_ids; - std::vector confidences; - std::vector boxes; - - for (int x = 0; x < cols; x++) { - int ptr = x; - - //Create box - float l = tensor[ptr]; - ptr += cols; - float h = tensor[ptr]; - ptr += cols; - float width = tensor[ptr]; - ptr += cols; - float height = tensor[ptr]; - - int left = int((l - 0.5 * width)); - int top = int((h - 0.5 * height)); - - - boxes.push_back(cv::Rect(left, top, static_cast(width), static_cast(height))); - - //Find class id and confidence - int maxIndex = 0; - float maxConfidence = 0; - - for (int i = 0; i < numClasses; i++) { - ptr += cols; - if (tensor[ptr] > maxConfidence) { - maxConfidence = tensor[ptr]; - maxIndex = i; - } - } - - confidences.push_back(maxConfidence); - class_ids.push_back(maxIndex); - } - - std::vector nms_result; - cv::dnn::NMSBoxes(boxes, confidences, modelScoreThreshold, modelNMSThreshold, nms_result); - - for (unsigned long i = 0; i < nms_result.size(); ++i) { - int idx = nms_result[i]; - - Detection result; - result.class_id = class_ids[idx]; - result.confidence = confidences[idx]; - - std::random_device rd; - std::mt19937 gen(rd()); - std::uniform_int_distribution dis(100, 255); - result.color = cv::Scalar(dis(gen), - dis(gen), - dis(gen)); - - result.className = classes[result.class_id]; - result.box = boxes[idx]; - - detections.push_back(result); - } - - return detections; - } -} // namespace mrover \ No newline at end of file diff --git a/src/perception/object_detector/parser.hpp b/src/perception/object_detector/parser.hpp deleted file mode 100644 index 0ea8d74cc..000000000 --- a/src/perception/object_detector/parser.hpp +++ /dev/null @@ -1,41 +0,0 @@ -#include "object_detector.hpp" -#ifndef PARSER_H -#define PARSER_H - -//OPEN CV -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -//STD -#include - -namespace mrover { - - - class Parser { - private: - std::vector tensor; - - //MAT VALS - int width; - - //NET PARAMS - float modelScoreThreshold = 0.5; - float modelNMSThreshold = 0.50; - std::vector classes{"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; - - public: - Parser(std::vector& tensor); - - std::vector parseTensor(cv::Mat& outputTensor); - }; -} // namespace mrover -#endif \ No newline at end of file From 7dbe7ff50ed6e0fb513eb42d9d666ad54e22d795 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 7 Dec 2023 13:39:11 -0500 Subject: [PATCH 094/197] Parsing point cloud 2 image --- CMakeLists.txt | 2 +- .../object_detector/object_detector.cpp | 2 +- .../object_detector/object_detector.hpp | 12 +- .../object_detector.processing.cpp | 168 +++--------------- src/perception/object_detector/pch.hpp | 15 +- 5 files changed, 40 insertions(+), 159 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 754b23f47..569fe91f6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -216,7 +216,7 @@ mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_a if (ZED_FOUND) mrover_add_nodelet(object_detector src/perception/object_detector/*.c* src/perception/object_detector src/perception/object_detector/pch.hpp) - mrover_nodelet_link_libraries(object_detector PRIVATE opencv_core opencv_dnn opencv_imgproc lie nvinfer nvonnxparser) + mrover_nodelet_link_libraries(object_detector PRIVATE opencv_core opencv_dnn opencv_imgproc lie nvinfer nvonnxparser tbb) # Temporary mrover_nodelet_link_libraries(object_detector PRIVATE opencv_imgcodecs) mrover_nodelet_link_libraries(object_detector PRIVATE opencv_highgui) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 46420d580..0991ad80b 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -14,7 +14,7 @@ namespace mrover { // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases // TODO(percep/obj-detectr): make this configurable - mImgSub = mNh.subscribe("/camera/left/image", 1, &ObjectDetectorNodelet::imageCallback, this); + mImgSub = mNh.subscribe("/camera/left/points", 1, &ObjectDetectorNodelet::imageCallback, this); mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); mDetectionData = mNh.advertise("/object_detector/detected_object", 1); } diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 31a465802..4044a9807 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -16,25 +16,24 @@ namespace mrover { class ObjectDetectorNodelet : public nodelet::Nodelet { + cv::Mat mImg; + constexpr static int NUM_CHANNELS = 3; constexpr static int IMG_WIDTH = 1280; constexpr static int IMG_HEIGHT = 720; std::vector classes{"Hammer"};//{"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; - ros::NodeHandle mNh, mPnh; //Inference inference; InferenceWrapper mInferenceWrapper; // Publishers - ros::Publisher mDebugImgPub; ros::Publisher mDetectionData; // Subscribers - ros::Subscriber mImgSub; // Preallocated cv::Mats @@ -44,23 +43,18 @@ namespace mrover { dynamic_reconfigure::Server::CallbackType mCallbackType; // Internal state - cv::dnn::Net mNet; // Debug - LoopProfiler mProfiler{"Object Detector"}; void onInit() override; - //DetectedObject convertToObjMsg(Detection& detection); - - public: ObjectDetectorNodelet() = default; ~ObjectDetectorNodelet() override = default; - void imageCallback(sensor_msgs::ImageConstPtr const& msg); + void imageCallback(sensor_msgs::PointCloud2ConstPtr const& msg); }; } // namespace mrover \ No newline at end of file diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index c0cc21a35..3db483015 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -4,18 +4,32 @@ #include #include #include +#include "../point.hpp" + namespace mrover { - void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { + void ObjectDetectorNodelet::imageCallback(sensor_msgs::PointCloud2ConstPtr const& msg) { assert(msg); assert(msg->height > 0); assert(msg->width > 0); + if (static_cast(msg->height) != mImg.rows || static_cast(msg->width) != mImg.cols) { + NODELET_INFO("Image size changed from [%d %d] to [%u %u]", mImg.cols, mImg.rows, msg->width, msg->height); + mImg = cv::Mat{static_cast(msg->height), static_cast(msg->width), CV_8UC4, cv::Scalar{0, 0, 0, 0}}; + } + auto* pixelPtr = reinterpret_cast(mImg.data); + auto* pointPtr = reinterpret_cast(msg->data.data()); + std::for_each(std::execution::par_unseq, pixelPtr, pixelPtr + mImg.total(), [&](cv::Vec4b& pixel) { + size_t const i = &pixel - pixelPtr; + pixel[0] = pointPtr[i].b; + pixel[1] = pointPtr[i].g; + pixel[2] = pointPtr[i].r; + pixel[3] = pointPtr[i].a; + }); - cv::Mat imageView{static_cast(msg->height), static_cast(msg->width), CV_8UC4, const_cast(msg->data.data())}; cv::Mat sizedImage; - cv::resize(imageView, sizedImage, cv::Size(640, 640)); + cv::resize(mImg, sizedImage, cv::Size(640, 640)); cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGRA2BGR); cv::dnn::blobFromImage(sizedImage, mImageBlob, 1.0 / 255.0, cv::Size{640, 640}, cv::Scalar(), true, false); @@ -147,7 +161,8 @@ namespace mrover { if (!detections.empty()) { Detection firstDetection = detections[0]; - float classConfidence = 0.0; + float classConfidence = firstDetection.confidence; + cv::Rect box = firstDetection.box; DetectedObject msgData; @@ -160,8 +175,8 @@ namespace mrover { //Get the heading float objectHeading; float zedFOV = 54; //54 @ 720; 42 @ 1080 - float fovPerPixel = (float) zedFOV / static_cast(msg->width); - float xCenter = static_cast(box.x) + (static_cast(box.width)) / 2 - (static_cast(msg->width)) / 2; + float fovPerPixel = (float) zedFOV / static_cast(modelShapeWidth); + float xCenter = static_cast(box.x) + (static_cast(box.width) / 2) - (static_cast(modelShapeWidth) / 2); objectHeading = xCenter * fovPerPixel; msgData.bearing = objectHeading; @@ -203,7 +218,6 @@ namespace mrover { newDebugImageMessage.step = sizedImage.channels() * sizedImage.cols; newDebugImageMessage.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; - // auto* bgrGpuPtr = sizedImage.getPtr(sl::MEM::GPU); auto imgPtr = sizedImage.data; size_t size = newDebugImageMessage.step * newDebugImageMessage.height; @@ -215,143 +229,3 @@ namespace mrover { } } } // namespace mrover -/* -cv::rectangle(sizedImg, box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); - - //Put the text on the image - cv::Point text_position(80, 80); - int font_size = 1; - cv::Scalar font_Color(0, 0, 0); - int font_weight = 2; - putText(sizedImg, msgData.object_type, text_position, cv::FONT_HERSHEY_COMPLEX, font_size, font_Color, font_weight); //Putting the text in the matrix// - - //Show the image - cv::imshow("obj detector", sizedImg); - cv::waitKey(1); - //Print the type of objected detected - ROS_INFO(firstDetection.className.c_str()); - void ObjectDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { - //Ensure a valid message was received - assert(msg); - - //Get a CV::Mat image view - cv::Mat imageView{static_cast(msg->height), static_cast(msg->width), CV_8UC3, const_cast(msg->data.data())}; - - cv::dnn::blobFromImage(imageView, this->imageBlob); - - //Run Image Detections - //Return type of Detection struct - std::vector detections = this->inference.doDetections(imageView); - - //just an array of DetectedObject - DetectedObjects detectionsMessage; - - detectionsMessage.num_detections = static_cast(detections.size()); - - //Convert Vector of Detections to Output Message - for (Detection& detection: detections) { - DetectedObject objMsg = convertToObjMsg(detection); - detectionsMessage.push_back(objMsg); - } - - // detectionsMessage.header.seq = mSeqNum; - detectionsMessage.header.stamp = ros::Time::now(); - // detectionsMessage.header.frame_id = "frame" - - this->publisher; - } - - assert(msg); - assert(msg->height > 0); - assert(msg->width > 0); - - cv::Mat sizedImg{static_cast(msg->height), static_cast(msg->width), CV_8UC4, const_cast(msg->data.data())}; - - cv::cvtColor(sizedImg, sizedImg, cv::COLOR_BGRA2BGR); - - std::vector detections = inference.runInference(sizedImg); - - - if (!detections.empty()) { - Detection firstDetection = detections[0]; - - float classConfidence = 0.0; - cv::Rect box = firstDetection.box; - - DetectedObject msgData; - msgData.object_type = firstDetection.className; - msgData.detection_confidence = classConfidence; - - msgData.xBoxPixel = static_cast(box.x); - msgData.yBoxPixel = static_cast(box.y); - msgData.width = static_cast(box.width); - msgData.height = static_cast(box.height); - - //Get the heading - - float objectHeading; - float zedFOV = 54; //54 @ 720; 42 @ 1080 - float fovPerPixel = (float) zedFOV / (float) (msg->width); - float xCenter = (float) box.x + ((float) box.width) / 2 - ((float) msg->width) / 2; - objectHeading = xCenter * fovPerPixel; - msgData.heading = objectHeading; - - - //Put the rectangle on the image - cv::rectangle(sizedImg, box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); - - //Put the text on the image - cv::Point text_position(80, 80); - int font_size = 1; - cv::Scalar font_Color(0, 0, 0); - int font_weight = 2; - putText(sizedImg, msgData.object_type, text_position, cv::FONT_HERSHEY_COMPLEX, font_size, font_Color, font_weight); //Putting the text in the matrix// - - - //Print the type of objected detected - // ROS_INFO_STREAM(firstDetection.className.c_str()); - - //Publish Dectection Data - mDetectionData.publish(msgData); - - if (mDebugImgPub.getNumSubscribers() > 0 || true) { - // Create sensor msg image - sensor_msgs::Image newDebugImageMessage; - - //Change the mat to from bgr to bgra - cv::cvtColor(sizedImg, sizedImg, cv::COLOR_BGR2BGRA); - - - newDebugImageMessage.height = sizedImg.rows; - newDebugImageMessage.width = sizedImg.cols; - - newDebugImageMessage.encoding = sensor_msgs::image_encodings::BGRA8; - - //Calculate the step for the imgMsg - newDebugImageMessage.step = sizedImg.channels() * sizedImg.cols; - newDebugImageMessage.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; - - // auto* bgrGpuPtr = sizedImage.getPtr(sl::MEM::GPU); - auto imgPtr = sizedImg.data; - - size_t size = msg->step * msg->height; - newDebugImageMessage.data.resize(size); - - memcpy(newDebugImageMessage.data.data(), imgPtr, size); - - mDebugImgPub.publish(newDebugImageMessage); - } - // Publish to - } - */ -//Look at yolov8 documentation for the output matrix -/* - * TODO(percep/obj-detector): - * 0. Google "OpenCV DNN example." View a couple of tutorials. Most will be in Python but the C++ API is similar. - * 1. Use cv::dnn::blobFromImage to convert the image to a blob - * 2. Use mNet.forward to run the network - * 3. Parse the output of the network to get the bounding boxes - * 4. Publish the bounding boxes - */ - -// namespace mrover diff --git a/src/perception/object_detector/pch.hpp b/src/perception/object_detector/pch.hpp index 76660d837..d8002572e 100644 --- a/src/perception/object_detector/pch.hpp +++ b/src/perception/object_detector/pch.hpp @@ -1,6 +1,6 @@ #pragma once -#include +#include //TODO: clean up :) #include #include #include @@ -17,6 +17,17 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include @@ -42,3 +53,5 @@ #include #include +#include +#include From aa1e69301743061e8d21625537c142b75e12cde9 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 7 Dec 2023 13:44:44 -0500 Subject: [PATCH 095/197] Update LFS files to track NN files --- .gitattributes | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/.gitattributes b/.gitattributes index 070d9837a..5be6800f8 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1,6 +1,11 @@ text=auto +# Image files *.png filter=lfs diff=lfs merge=lfs -text +# Model files *.dae filter=lfs diff=lfs merge=lfs -text -*.blend filter=lfs diff=lfs merge=lfs -text *.stl filter=lfs diff=lfs merge=lfs -text - +*.blend filter=lfs diff=lfs merge=lfs -text +# Neural network files +*.pt filter=lfs diff=lfs merge=lfs -text +*.onnx filter=lfs diff=lfs merge=lfs -text +*.engine filter=lfs diff=lfs merge=lfs -text From e3f5a9bf87b580d52f55a65f82f17d1f3c15a32c Mon Sep 17 00:00:00 2001 From: jbrhm Date: Tue, 12 Dec 2023 16:15:12 -0500 Subject: [PATCH 096/197] Post engine fiasco Changes --- src/perception/object_detector/inference.cu | 1 - .../object_detector/object_detector.cpp | 4 ++ .../object_detector/object_detector.hpp | 9 ++++ .../object_detector.processing.cpp | 50 ++++++++++++++++++- 4 files changed, 62 insertions(+), 2 deletions(-) diff --git a/src/perception/object_detector/inference.cu b/src/perception/object_detector/inference.cu index df8a15cf5..ec0ef95d7 100644 --- a/src/perception/object_detector/inference.cu +++ b/src/perception/object_detector/inference.cu @@ -125,7 +125,6 @@ namespace mrover { void Inference::doDetections(const cv::Mat& img) const{ //Do the forward pass on the network launchInference(img, mOutputTensor); - ROS_INFO_STREAM(static_cast(mOutputTensor.data)); //return Parser(outputTensor).parseTensor(); } diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 0991ad80b..76479c73d 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -17,6 +17,10 @@ namespace mrover { mImgSub = mNh.subscribe("/camera/left/points", 1, &ObjectDetectorNodelet::imageCallback, this); mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); mDetectionData = mNh.advertise("/object_detector/detected_object", 1); + mNh.param("world_frame", mMapFrameId, "map"); + + //Create the Reference Frames + mNh.param("camera_frame", mCameraFrameId, "zed2i_left_camera_frame"); } } // namespace mrover diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 4044a9807..a282e5c0f 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -48,6 +48,13 @@ namespace mrover { // Debug LoopProfiler mProfiler{"Object Detector"}; + //TF Member Variables + tf2_ros::Buffer mTfBuffer; + tf2_ros::TransformListener mTfListener{mTfBuffer}; + tf2_ros::TransformBroadcaster mTfBroadcaster; + std::string mCameraFrameId; + std::string mMapFrameId; + void onInit() override; public: @@ -56,5 +63,7 @@ namespace mrover { ~ObjectDetectorNodelet() override = default; void imageCallback(sensor_msgs::PointCloud2ConstPtr const& msg); + + std::optional getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) const; }; } // namespace mrover \ No newline at end of file diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 3db483015..8df5a4810 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -172,6 +172,37 @@ namespace mrover { msgData.width = static_cast(box.width); msgData.height = static_cast(box.height); + std::pair center(box.x + box.width/2, box.y + box.height/2); + + ROS_INFO("center image coordinates x y: %d %d", center.first, center.second); + + if (std::optional objectLocation = getObjectInCamFromPixel(msg, center.first, center.second); objectLocation) { + + ROS_INFO("x y z position %f %f %f", objectLocation->position().x(), objectLocation->position().y(), objectLocation->position().z()); + + // Publish tag to immediate + try{ + std::string immediateFrameId = "detectedobject"; + SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, objectLocation.value()); + + SE3 tagInsideCamera = SE3::fromTfTree(mTfBuffer, mMapFrameId, immediateFrameId); + + SE3::pushToTfTree(mTfBroadcaster, "detectedobjectFR", mMapFrameId, tagInsideCamera); + } catch (tf2::ExtrapolationException const&) { + NODELET_WARN("Old data for immediate tag"); + } catch (tf2::LookupException const&) { + NODELET_WARN("Expected transform for immediate tag"); + } catch (tf::ConnectivityException const&) { + NODELET_WARN("Expected connection to odom frame. Is visual odometry running?"); + } catch (tf::LookupException const&) { + NODELET_WARN("LOOK UP NOT FOUND"); + } + + } + + msgData.width = static_cast(box.width); + msgData.height = static_cast(box.height); + //Get the heading float objectHeading; float zedFOV = 54; //54 @ 720; 42 @ 1080 @@ -187,8 +218,9 @@ namespace mrover { std::cout << detections[i].className << i << std::endl; - + cv::Rect tempRect(center.first, center.second, 10, 10); cv::rectangle(sizedImage, detections[i].box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); + cv::rectangle(sizedImage, tempRect, cv::Scalar(0, 0, 0), 3, cv::LINE_8, 0); //Put the text on the image cv::Point text_position(80, static_cast(80 * (i + 1))); @@ -228,4 +260,20 @@ namespace mrover { mDebugImgPub.publish(newDebugImageMessage); } } + std::optional ObjectDetectorNodelet::getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) const { + assert(cloudPtr); + + if (u >= cloudPtr->width || v >= cloudPtr->height) { + NODELET_WARN("Tag center out of bounds: [%zu %zu]", u, v); + return std::nullopt; + } + + Point point = reinterpret_cast(cloudPtr->data.data())[u + v * cloudPtr->width]; + if (!std::isfinite(point.x) || !std::isfinite(point.y) || !std::isfinite(point.z)) { + NODELET_WARN("Tag center point not finite: [%f %f %f]", point.x, point.y, point.z); + return std::nullopt; + } + + return std::make_optional(R3{point.x, point.y, point.z}, SO3{}); + } } // namespace mrover From 9eb1d6ad745f8784096104abedba9181159dcf91 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Thu, 4 Jan 2024 20:23:06 -0500 Subject: [PATCH 097/197] fix build --- src/perception/tag_detector/pch.hpp | 1 + starter_project/autonomy/AutonomyStarterProject.cmake | 2 +- starter_project/autonomy/src/perception.hpp | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/perception/tag_detector/pch.hpp b/src/perception/tag_detector/pch.hpp index b1d493369..850c65245 100644 --- a/src/perception/tag_detector/pch.hpp +++ b/src/perception/tag_detector/pch.hpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include diff --git a/starter_project/autonomy/AutonomyStarterProject.cmake b/starter_project/autonomy/AutonomyStarterProject.cmake index 76c8c1b08..13e0d3bc8 100644 --- a/starter_project/autonomy/AutonomyStarterProject.cmake +++ b/starter_project/autonomy/AutonomyStarterProject.cmake @@ -18,7 +18,7 @@ add_executable(starter_project_perception ${STARTER_PROJECT_PERCEPTION_SOURCES}) # Ensure that our project builds after message generation add_dependencies(starter_project_perception ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # Link needed libraries -target_link_libraries(starter_project_perception ${catkin_LIBRARIES} opencv_core opencv_objdetect) +target_link_libraries(starter_project_perception ${catkin_LIBRARIES} opencv_core opencv_objdetect opencv_aruco) # Include needed directories target_include_directories(starter_project_perception PUBLIC ${catkin_INCLUDE_DIRS}) diff --git a/starter_project/autonomy/src/perception.hpp b/starter_project/autonomy/src/perception.hpp index 881277103..362606918 100644 --- a/starter_project/autonomy/src/perception.hpp +++ b/starter_project/autonomy/src/perception.hpp @@ -9,6 +9,7 @@ // OpenCV Headers, cv namespace #include +#include #include // ROS Headers, ros namespace From 52d57636b729c9a0202aa14439b73cbcee65a8ca Mon Sep 17 00:00:00 2001 From: Emerson-Ramey Date: Tue, 9 Jan 2024 06:34:42 +0000 Subject: [PATCH 098/197] outline changes for new states --- src/navigation/approach_object.py | 2 +- src/navigation/approach_post.py | 8 ++++---- src/navigation/context.py | 32 +++++++++++++++++++++---------- src/navigation/long_range.py | 9 +++++++-- src/navigation/search.py | 14 ++++++++++++-- src/navigation/waypoint.py | 12 ++++++++++-- 6 files changed, 56 insertions(+), 21 deletions(-) diff --git a/src/navigation/approach_object.py b/src/navigation/approach_object.py index a53eaca46..400d627bc 100644 --- a/src/navigation/approach_object.py +++ b/src/navigation/approach_object.py @@ -26,7 +26,7 @@ def on_exit(self, context): pass def get_target_pos(self, context) -> Optional[int]: - object_pos = None # TODO: replace + object_pos = context.env.current_target_pos() return object_pos def determine_next(self, context, finished: bool) -> State: diff --git a/src/navigation/approach_post.py b/src/navigation/approach_post.py index a4c8d45c7..e4aa1ec6b 100644 --- a/src/navigation/approach_post.py +++ b/src/navigation/approach_post.py @@ -26,7 +26,7 @@ def on_exit(self, context): def get_target_pos(self, context) -> Optional[int]: # return fid_pos, either position or None - fid_pos = context.env.current_fid_pos() + fid_pos = context.env.current_target_pos() return fid_pos def determine_next(self, context, finished: bool) -> State: @@ -54,7 +54,7 @@ def on_loop(self, context) -> State: :param ud: State machine user data :return: Next state """ - fid_pos = context.env.current_fid_pos() + fid_pos = context.env.current_target_pos() if fid_pos is None: return search.SearchState() @@ -68,9 +68,9 @@ def on_loop(self, context) -> State: ) if arrived: context.env.arrived_at_post = True - context.env.last_post_location = context.env.current_fid_pos(odom_override=False) + context.env.last_post_location = context.env.current_target_pos(odom_override=False) context.course.increment_waypoint() - return waypoint.WaypointState() + return waypoint.WaypointState() # we want to go to done state now context.rover.send_drive_command(cmd_vel) except ( tf2_ros.LookupException, diff --git a/src/navigation/context.py b/src/navigation/context.py index 24468dd70..abcab5884 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -64,8 +64,9 @@ class Environment: NO_FIDUCIAL: ClassVar[int] = -1 arrived_at_post: bool = False last_post_location: Optional[np.ndarray] = None - - def get_fid_pos(self, fid_id: int, in_odom_frame: bool = True) -> Optional[np.ndarray]: + # TODO add dictionary for long range tag id : (time received, our hit counter, bearing) + + def get_target_pos(self, id: str, in_odom_frame: bool = True) -> Optional[np.ndarray]: """ Retrieves the pose of the given fiducial ID from the TF tree in the odom frame if in_odom_frame is True otherwise in the world frame @@ -73,12 +74,12 @@ def get_fid_pos(self, fid_id: int, in_odom_frame: bool = True) -> Optional[np.nd """ try: parent_frame = self.ctx.odom_frame if in_odom_frame else self.ctx.world_frame - fid_pose, time = SE3.from_tf_time( - self.ctx.tf_buffer, parent_frame=parent_frame, child_frame=f"fiducial{fid_id}" + target_pose, time = SE3.from_tf_time( + self.ctx.tf_buffer, parent_frame=parent_frame, child_frame=f"{id}" ) now = rospy.Time.now() if now.to_sec() - time.to_sec() >= TAG_EXPIRATION_TIME_SECONDS: - print(f"TAG EXPIRED {fid_id}!") + print(f"EXPIRED {id}!") return None except ( tf2_ros.LookupException, @@ -86,11 +87,11 @@ def get_fid_pos(self, fid_id: int, in_odom_frame: bool = True) -> Optional[np.nd tf2_ros.ExtrapolationException, ) as e: return None - return fid_pose.position - - def current_fid_pos(self, odom_override: bool = True) -> Optional[np.ndarray]: + return target_pose.position + + def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray]: """ - Retrieves the position of the current fiducial (and we are looking for it) + Retrieves the position of the current fiducial or object (and we are looking for it) :param: odom_override if false will force it to be in the map frame """ assert self.ctx.course @@ -100,7 +101,15 @@ def current_fid_pos(self, odom_override: bool = True) -> Optional[np.ndarray]: print("CURRENT WAYPOINT IS NONE") return None - return self.get_fid_pos(current_waypoint.tag_id, in_odom) + if current_waypoint.type == WaypointType.POST: + return self.get_target_pos(f"fiducial{current_waypoint.tag_id}", in_odom) + elif current_waypoint.type == WaypointType.MALLET: + return self.get_target_pos("mallet", in_odom) + elif current_waypoint.type == WaypointType.WATER_BOTTLE: + return self.get_target_pos("water_bottle", in_odom) + else: + print("CURRENT WAYPOINT IS NOT A POST OR OBJECT") + return None @dataclass @@ -202,6 +211,7 @@ class Context: vis_publisher: rospy.Publisher course_listener: rospy.Subscriber stuck_listener: rospy.Subscriber + # TODO create listener for long range cam # Use these as the primary interfaces in states course: Optional[Course] @@ -242,3 +252,5 @@ def recv_enable_auton(self, req: mrover.srv.PublishEnableAutonRequest) -> mrover def stuck_callback(self, msg: Bool): self.rover.stuck = msg.data + + # TODO create callback for long range listener diff --git a/src/navigation/long_range.py b/src/navigation/long_range.py index fe8641d2e..76c0d4949 100644 --- a/src/navigation/long_range.py +++ b/src/navigation/long_range.py @@ -26,12 +26,17 @@ def on_exit(self, context): pass def get_target_pos(self, context) -> Optional[int]: - target_pos = None # TODO: replace + target_pos = None # TODO: get bearing from most recent message for current tag_id and create a “target” location to drive to by setting a point a certain look ahead distance far away from the rover (20-30 meters) once aligned with the bearing return target_pos def determine_next(self, context, finished: bool) -> State: - fid_pos = context.env.current_fid_pos() + # if we see the tag in the ZED transition to approach post + fid_pos = context.env.current_target_pos() if fid_pos is None: + # if we haven’t seen the tag in the long range cam in the last 3 seconds, go back to search + # TODO if time.now - last message received for tag_id > 3: + # return search.SearchState() + # otherwise continue in long range state return self else: return approach_post.ApproachPostState() diff --git a/src/navigation/search.py b/src/navigation/search.py index 2b0d8ee3b..76aec55e4 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -94,6 +94,7 @@ class SearchState(State): def on_enter(self, context) -> None: search_center = context.course.current_waypoint() if not self.is_recovering: + # TODO give different parameters to spiral_traj based on if search_center.type is POST or RUBBER_MALLET (water bottle has diff search state) self.traj = SearchTrajectory.spiral_traj( context.rover.get_pose().position[0:2], self.SPIRAL_COVERAGE_RADIUS, @@ -134,6 +135,15 @@ def on_loop(self, context) -> State: ) context.rover.send_drive_command(cmd_vel) - if context.env.current_fid_pos() is not None and context.course.look_for_post(): - return approach_post.ApproachPostState() + # TODO get current waypoint + # if current waypoint type is POST + if context.env.current_target_pos() is not None and context.course.look_for_post(): # indent this if statement + return approach_post.ApproachPostState() # if we see the tag in the ZED, go to ApproachPostState + # if we see the tag in the long range camera, go to LongRangeState + # if tag id has hit count > 3: + # return long_range.LongRangeState() + + # elif current waypoint type is RUBBER MALLET + # if context.env.current_target_pos() is not None and context.course.look_for_object(): + # return approach_object.ApproachObjectState() # if we see the object return self diff --git a/src/navigation/waypoint.py b/src/navigation/waypoint.py index 0a964dc48..8aa3398c2 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -35,8 +35,14 @@ def on_loop(self, context) -> State: return post_backup.PostBackupState() if context.course.look_for_post(): - if context.env.current_fid_pos() is not None: + if context.env.current_target_pos() is not None: return approach_post.ApproachPostState() + # TODO: + # elif tag id has hit count > 3: + # return long_range.LongRangeState() + # elif context.course.look_for_object(): + # if context.env.current_target_pos() is not None: + # return approach_object.ApproachObjectState() # Attempt to find the waypoint in the TF tree and drive to it try: @@ -51,9 +57,11 @@ def on_loop(self, context) -> State: if not context.course.look_for_post(): # We finished a regular waypoint, go onto the next one context.course.increment_waypoint() - else: + else: # TODO: elif looking for post or rubber mallet # We finished a waypoint associated with a tag id, but we have not seen it yet. return search.SearchState() + # TODO elif looking for water bottle: + # return obstacle_avoidance.ObstacleAvoidanceSearchState() if context.rover.stuck: context.rover.previous_state = self From 023d79d4d63f50edc996f51ad7919fb0b6cd480a Mon Sep 17 00:00:00 2001 From: John Date: Fri, 12 Jan 2024 14:32:48 -0500 Subject: [PATCH 099/197] New Ubuntu --- launch/zed_test.launch | 4 ++-- src/perception/object_detector/object_detector.cpp | 2 +- src/perception/tag_detector/pch.hpp | 1 + starter_project/autonomy/AutonomyStarterProject.cmake | 2 +- starter_project/autonomy/src/perception.cpp | 3 +++ starter_project/autonomy/src/perception.hpp | 1 + 6 files changed, 9 insertions(+), 4 deletions(-) diff --git a/launch/zed_test.launch b/launch/zed_test.launch index 81c98f59d..88158f753 100644 --- a/launch/zed_test.launch +++ b/launch/zed_test.launch @@ -30,10 +30,10 @@ args="load mrover/TagDetectorNodelet perception_nodelet_manager" output="screen"> - - + --> diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 76479c73d..bb3758d83 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -7,7 +7,7 @@ namespace mrover { void ObjectDetectorNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mInferenceWrapper = InferenceWrapper(std::string("//home//jabra//Desktop//Rover//best.onnx"), cv::Size(640, 640), ""); + mInferenceWrapper = InferenceWrapper(std::string("//home//john//Desktop//Rover//best.onnx"), cv::Size(640, 640), ""); //inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); //read ONNX file into this mNet, YOLOV8, second smallest one diff --git a/src/perception/tag_detector/pch.hpp b/src/perception/tag_detector/pch.hpp index b1d493369..850c65245 100644 --- a/src/perception/tag_detector/pch.hpp +++ b/src/perception/tag_detector/pch.hpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include diff --git a/starter_project/autonomy/AutonomyStarterProject.cmake b/starter_project/autonomy/AutonomyStarterProject.cmake index 76c8c1b08..13e0d3bc8 100644 --- a/starter_project/autonomy/AutonomyStarterProject.cmake +++ b/starter_project/autonomy/AutonomyStarterProject.cmake @@ -18,7 +18,7 @@ add_executable(starter_project_perception ${STARTER_PROJECT_PERCEPTION_SOURCES}) # Ensure that our project builds after message generation add_dependencies(starter_project_perception ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # Link needed libraries -target_link_libraries(starter_project_perception ${catkin_LIBRARIES} opencv_core opencv_objdetect) +target_link_libraries(starter_project_perception ${catkin_LIBRARIES} opencv_core opencv_objdetect opencv_aruco) # Include needed directories target_include_directories(starter_project_perception PUBLIC ${catkin_INCLUDE_DIRS}) diff --git a/starter_project/autonomy/src/perception.cpp b/starter_project/autonomy/src/perception.cpp index 0340865d9..3a677c6cd 100644 --- a/starter_project/autonomy/src/perception.cpp +++ b/starter_project/autonomy/src/perception.cpp @@ -3,6 +3,9 @@ // ROS Headers, ros namespace #include +#include +#include + int main(int argc, char** argv) { ros::init(argc, argv, "starter_project_perception"); // Our node name (See: http://wiki.ros.org/Nodes) diff --git a/starter_project/autonomy/src/perception.hpp b/starter_project/autonomy/src/perception.hpp index 881277103..2100f3914 100644 --- a/starter_project/autonomy/src/perception.hpp +++ b/starter_project/autonomy/src/perception.hpp @@ -10,6 +10,7 @@ // OpenCV Headers, cv namespace #include #include +#include // ROS Headers, ros namespace #include From 72160d945bcf8fbcd698f7863cf1cc1760fb22c9 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 12 Jan 2024 17:37:36 -0500 Subject: [PATCH 100/197] Rescaled Pixel Grab --- launch/zed_test.launch | 4 ++-- .../object_detector/object_detector.processing.cpp | 8 +++----- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/launch/zed_test.launch b/launch/zed_test.launch index 88158f753..1181c3820 100644 --- a/launch/zed_test.launch +++ b/launch/zed_test.launch @@ -2,7 +2,7 @@ - + @@ -36,5 +36,5 @@ --> + args="1 0 0 0 0 0 base_link 100"/> diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 8df5a4810..b311afd08 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -174,10 +174,9 @@ namespace mrover { std::pair center(box.x + box.width/2, box.y + box.height/2); - ROS_INFO("center image coordinates x y: %d %d", center.first, center.second); - - if (std::optional objectLocation = getObjectInCamFromPixel(msg, center.first, center.second); objectLocation) { + ROS_INFO("center image coordinates x y: %d %d", center.first * static_cast(msg->width) / sizedImage.cols, center.second * static_cast(msg->height) / sizedImage.rows); + if (std::optional objectLocation = getObjectInCamFromPixel(msg, center.first * static_cast(msg->width) / sizedImage.cols, center.second * static_cast(msg->height) / sizedImage.rows); objectLocation) { ROS_INFO("x y z position %f %f %f", objectLocation->position().x(), objectLocation->position().y(), objectLocation->position().z()); // Publish tag to immediate @@ -197,7 +196,6 @@ namespace mrover { } catch (tf::LookupException const&) { NODELET_WARN("LOOK UP NOT FOUND"); } - } msgData.width = static_cast(box.width); @@ -268,7 +266,7 @@ namespace mrover { return std::nullopt; } - Point point = reinterpret_cast(cloudPtr->data.data())[u + v * cloudPtr->width]; + Point point = reinterpret_cast(cloudPtr->data.data())[u + v * cloudPtr->width ]; if (!std::isfinite(point.x) || !std::isfinite(point.y) || !std::isfinite(point.z)) { NODELET_WARN("Tag center point not finite: [%f %f %f]", point.x, point.y, point.z); return std::nullopt; From 2324c79608b4e8de703f42f35136be9953d60c73 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 14 Jan 2024 10:56:20 -0500 Subject: [PATCH 101/197] commented and builds --- src/perception/object_detector/cuda_raii.cuh | 76 ------------ src/perception/object_detector/inference.cu | 52 ++++---- src/perception/object_detector/inference.cuh | 19 ++- .../object_detector/inference_wrapper.cu | 5 +- .../object_detector/inference_wrapper.hpp | 6 + .../object_detector/object_detector.cpp | 11 +- .../object_detector/object_detector.hpp | 16 ++- .../object_detector.processing.cpp | 114 +++++++++--------- src/perception/object_detector/pch.hpp | 2 - 9 files changed, 118 insertions(+), 183 deletions(-) delete mode 100644 src/perception/object_detector/cuda_raii.cuh diff --git a/src/perception/object_detector/cuda_raii.cuh b/src/perception/object_detector/cuda_raii.cuh deleted file mode 100644 index eda9ff60a..000000000 --- a/src/perception/object_detector/cuda_raii.cuh +++ /dev/null @@ -1,76 +0,0 @@ -/* Copyright (c) 1993-2018, NVIDIA CORPORATION. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of NVIDIA CORPORATION nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY - * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR - * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY - * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -#pragma once - -#include - -inline void check(cudaError_t result) { - if (result == cudaSuccess) return; - - throw std::runtime_error{std::string{"CUDA Error:"} + cudaGetErrorString(result)}; -} - -namespace cudawrapper { - - class CudaStream { - public: - CudaStream() { - check(cudaStreamCreate(&mStream)); - } - - operator cudaStream_t() const { - return mStream; - } - - ~CudaStream() { - cudaStreamDestroy(mStream); - } - - private: - cudaStream_t mStream; - }; - - class CudaEvent { - public: - CudaEvent() { - check(cudaEventCreate(&mEvent)); - } - - operator cudaEvent_t() const { - return mEvent; - } - - ~CudaEvent() { - cudaEventDestroy(mEvent); - } - - private: - cudaEvent_t mEvent; - }; - -} // namespace cudawrapper diff --git a/src/perception/object_detector/inference.cu b/src/perception/object_detector/inference.cu index ec0ef95d7..800249478 100644 --- a/src/perception/object_detector/inference.cu +++ b/src/perception/object_detector/inference.cu @@ -19,85 +19,78 @@ using namespace nvinfer1;/** */ namespace mrover { + //Create names for common model names constexpr static const char * INPUT_BINDING_NAME = "images"; constexpr static const char * OUTPUT_BINDING_NAME = "output0"; - //Constructor - // : logger{}, inputTensor{}, outputTensor{}, referenceTensor{}, stream{} Inference::Inference(std::string const& onnxModelPath, cv::Size modelInputShape = {640, 640}, std::string const& classesTxtFile = "") { - + //Create the engine object from either the file or from onnx file mEngine = std::unique_ptr>{createCudaEngine(onnxModelPath)}; if (!mEngine) throw std::runtime_error("Failed to create CUDA engine"); + //Log the Engine was created mLogger.log(ILogger::Severity::kINFO, "Created CUDA Engine"); - //Check some assumptions + //Check some assumptions about the model if(mEngine->getNbIOTensors() != 2) throw std::runtime_error("Invalid Binding Count"); if(mEngine->getTensorIOMode(INPUT_BINDING_NAME) != TensorIOMode::kINPUT) throw std::runtime_error("Expected Input Binding 0 Is An Input"); if(mEngine->getTensorIOMode(OUTPUT_BINDING_NAME) != TensorIOMode::kOUTPUT) throw std::runtime_error("Expected Input Binding Input To Be 1"); - mStream.emplace(); - - mLogger.log(ILogger::Severity::kINFO, "Created CUDA stream"); - + //Create the execution context setUpContext(); + //Init the io tensors prepTensors(); } - // Initializes enginePtr with built engine ICudaEngine* Inference::createCudaEngine(std::string const& onnxModelPath) { // See link sfor additional context constexpr auto explicitBatch = 1U << static_cast(NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + //Init logger std::unique_ptr> builder{createInferBuilder(mLogger)}; if (!builder) throw std::runtime_error("Failed to create Infer Builder"); mLogger.log(ILogger::Severity::kINFO, "Created Infer Builder"); + //Init Network std::unique_ptr> network{builder->createNetworkV2(explicitBatch)}; if (!network) throw std::runtime_error("Failed to create Network Definition"); mLogger.log(ILogger::Severity::kINFO, "Created Network Definition"); + //Init the onnx file parser std::unique_ptr> parser{nvonnxparser::createParser(*network, mLogger)}; if (!parser) throw std::runtime_error("Failed to create ONNX Parser"); mLogger.log(ILogger::Severity::kINFO, "Created ONNX Parser"); + //Init the builder std::unique_ptr> config{builder->createBuilderConfig()}; if (!config) throw std::runtime_error("Failed to create Builder Config"); mLogger.log(ILogger::Severity::kINFO, "Created Builder Config"); - // TODO: Not needed if we already have the engine file //Parse the onnx from file if (!parser->parseFromFile(onnxModelPath.c_str(), static_cast(ILogger::Severity::kINFO))) { throw std::runtime_error("Failed to parse ONNX file"); } - config->setMemoryPoolLimit(MemoryPoolType::kWORKSPACE, 1 << 30); - - // auto profile = builder->createOptimizationProfile(); - // profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMIN, Dims4{1, 3, 256, 256}); - // profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kOPT, Dims4{1, 3, 256, 256}); - // profile->setDimensions(network->getInput(0)->getName(), OptProfileSelector::kMAX, Dims4{32, 3, 256, 256}); - - // config->addOptimizationProfile(profile); - //Create runtime engine IRuntime* runtime = createInferRuntime(mLogger); + //Engine file path std::filesystem::path enginePath("./tensorrt-engine-best.engine"); //Check if engine file exists if (exists(enginePath)) { - // TODO: error checking //Load engine from file std::ifstream inputFileStream("./tensorrt-engine-best.engine", std::ios::binary); std::stringstream engineBuffer; + //Stream in the engine file to the buffer engineBuffer << inputFileStream.rdbuf(); std::string enginePlan = engineBuffer.str(); - // TODO: deprecated + //Deserialize the Cuda Engine file from the buffer return runtime->deserializeCudaEngine(enginePlan.data(), enginePlan.size()); } else { + //Create the Engine from onnx file IHostMemory* serializedEngine = builder->buildSerializedNetwork(*network, *config); if (!serializedEngine) throw std::runtime_error("Failed to serialize engine"); @@ -119,32 +112,39 @@ namespace mrover { // Create Execution Context. mContext.reset(mEngine->createExecutionContext()); + //Set up the input tensor sizing mContext->setInputShape(INPUT_BINDING_NAME, mEngine->getTensorShape(INPUT_BINDING_NAME)); } void Inference::doDetections(const cv::Mat& img) const{ //Do the forward pass on the network launchInference(img, mOutputTensor); - //return Parser(outputTensor).parseTensor(); } cv::Mat Inference::getOutputTensor() { + //Returns the output tensor return mOutputTensor; } void Inference::launchInference(cv::Mat const& input, cv::Mat const& output) const { + //Assert these items have been initialized assert(!input.empty()); assert(!output.empty()); assert(input.isContinuous()); assert(output.isContinuous()); assert(mContext); - assert(mStream); + //Get the binding id for the input tensor const int inputId = getBindingInputIndex(mContext.get()); + //Memcpy the input tensor from the host to the gpu cudaMemcpy(mBindings[inputId], input.data, input.total() * input.elemSize(), cudaMemcpyHostToDevice); + + //Execute the model on the gpu mContext->executeV2(mBindings.data()); + + //Memcpy the output tensor from the gpu to the host cudaMemcpy(output.data, mBindings[1 - inputId], output.total() * output.elemSize(), cudaMemcpyDeviceToHost); } @@ -155,10 +155,9 @@ namespace mrover { * Modifies bindings, inputTensor, and outputTensor */ void Inference::prepTensors() { - + //Assign the properties to the input and output tensors for (int i = 0; i < mEngine->getNbIOTensors(); i++) { const char* tensorName = mEngine->getIOTensorName(i); - auto [rank, extents] = mEngine->getTensorShape(tensorName); // Multiply sizeof(float) by the product of the extents @@ -168,8 +167,8 @@ namespace mrover { cudaMalloc(mBindings.data() + i, size); } - // TODO(quintin): Avoid hardcoding this assert(mContext); + //Create an appropriately sized output tensor const Dims outputTensorDims = mEngine->getTensorShape(OUTPUT_BINDING_NAME); for (int i = 0; i < outputTensorDims.nbDims; i++) { char message[512]; @@ -181,6 +180,7 @@ namespace mrover { } int Inference::getBindingInputIndex(const IExecutionContext* context) { + //Returns the id for the input tensor return context->getEngine().getTensorIOMode(context->getEngine().getIOTensorName(0)) != TensorIOMode::kINPUT; // 0 (false) if bindingIsInput(0), 1 (true) otherwise } } // namespace mrover diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index adb63bd6e..627d3501c 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -5,18 +5,15 @@ #include -#include "cuda_raii.cuh" #include "logger.cuh" using nvinfer1::ICudaEngine; using nvinfer1::IExecutionContext; -// ROS Mesage -> CV View Matrix -> cv::blobFromImage -> cv::Mat (input) -// Preallocated CV::Mat of floats of correct size -> transpose into CV::Mat (also preallocated - namespace mrover { class Inference { + //Init Logger nvinfer1::Logger mLogger; //Ptr to the engine @@ -29,9 +26,6 @@ namespace mrover { cv::Mat mInputTensor; cv::Mat mOutputTensor; - //Cuda Stream - std::optional mStream{}; - //Bindings std::array mBindings{}; @@ -45,22 +39,27 @@ namespace mrover { //STATIC FUNCTIONS static int getBindingInputIndex(const IExecutionContext* context); - public: - Inference(std::string const& onnxModelPath, cv::Size modelInputShape, std::string const& classesTxtFile); - private: //Creates a ptr to the engine ICudaEngine* createCudaEngine(std::string const& onnxModelPath); + //Launch the model execution onto the GPU void launchInference(cv::Mat const& input, cv::Mat const& output) const; + //Init the tensors void prepTensors(); + //Init the execution context void setUpContext(); public: + //Inference Constructor + Inference(std::string const& onnxModelPath, cv::Size modelInputShape, std::string const& classesTxtFile); + + //Forward Pass of the model void doDetections(const cv::Mat& img) const; + //Get the output tensor with in a YOLO v8 style data structure cv::Mat getOutputTensor(); }; diff --git a/src/perception/object_detector/inference_wrapper.cu b/src/perception/object_detector/inference_wrapper.cu index 24de09db6..8afc4eeac 100644 --- a/src/perception/object_detector/inference_wrapper.cu +++ b/src/perception/object_detector/inference_wrapper.cu @@ -18,16 +18,19 @@ using namespace nvinfer1; */ namespace mrover { - //Initialize the unique_ptr to the inference class InferenceWrapper::InferenceWrapper(std::string onnxModelPath, cv::Size const modelInputShape = {640, 640}, std::string classesTxtFile = "") : mInference({}) { + //Initialize the unique_ptr to the inference class mInference.reset(new Inference(std::move(onnxModelPath), modelInputShape, std::move(classesTxtFile))); } + void InferenceWrapper::doDetections(const cv::Mat& img) const { + //Execute the forward pass on the inference object mInference->doDetections(img); } cv::Mat InferenceWrapper::getOutputTensor() const { + //Get the output tensor from the inference object return mInference->getOutputTensor(); } diff --git a/src/perception/object_detector/inference_wrapper.hpp b/src/perception/object_detector/inference_wrapper.hpp index 3c19dc1d6..b00e552f7 100644 --- a/src/perception/object_detector/inference_wrapper.hpp +++ b/src/perception/object_detector/inference_wrapper.hpp @@ -6,17 +6,23 @@ namespace mrover { class Inference; class InferenceWrapper { + //Ptr to the inference std::shared_ptr mInference; public: + //Default Constructor for the Wrapper InferenceWrapper() = default; + //Deconstructor for the Wrapper ~InferenceWrapper() = default; + //Inference Wrapper Constructor InferenceWrapper(std::string onnxModelPath, cv::Size const modelInputShape, std::string classesTxtFile); + //Forward Pass on the model void doDetections(cv::Mat const& img) const; + //Retrieve the output tensor from the previous forward pass cv::Mat getOutputTensor() const; }; diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index bb3758d83..0b8e8f690 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -5,22 +5,21 @@ namespace mrover { void ObjectDetectorNodelet::onInit() { + //Get the Handlers mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mInferenceWrapper = InferenceWrapper(std::string("//home//john//Desktop//Rover//best.onnx"), cv::Size(640, 640), ""); - //inference = Inference("//home//jabra//Desktop//Rover//yolov8s.onnx", cv::Size(640, 640), ""); - //read ONNX file into this mNet, YOLOV8, second smallest one - // Note(quintin): I downloaded this pt (PyTorch) model file from: https://github.com/ultralytics/assets/releases - // TODO(percep/obj-detectr): make this configurable + //Create the inference wrapper + mInferenceWrapper = InferenceWrapper(std::string("//home//john//Desktop//Rover//best.onnx"), cv::Size(640, 640), ""); + //Create the publishers and subscribers for the detected image and the debug image mImgSub = mNh.subscribe("/camera/left/points", 1, &ObjectDetectorNodelet::imageCallback, this); mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); mDetectionData = mNh.advertise("/object_detector/detected_object", 1); - mNh.param("world_frame", mMapFrameId, "map"); //Create the Reference Frames mNh.param("camera_frame", mCameraFrameId, "zed2i_left_camera_frame"); + mNh.param("world_frame", mMapFrameId, "map"); } } // namespace mrover diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index a282e5c0f..d005f4d91 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -3,27 +3,32 @@ #include "inference_wrapper.hpp" #include #include +#include "pch.hpp" namespace mrover { + //Data type for detection struct Detection { int class_id{0}; std::string className; float confidence{0.0}; - cv::Scalar color{}; cv::Rect box{}; }; class ObjectDetectorNodelet : public nodelet::Nodelet { + //Mat for the image from the point cloud cv::Mat mImg; + //Image information constexpr static int NUM_CHANNELS = 3; constexpr static int IMG_WIDTH = 1280; constexpr static int IMG_HEIGHT = 720; + //List of class names std::vector classes{"Hammer"};//{"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; + //ROS Handlers ros::NodeHandle mNh, mPnh; //Inference inference; @@ -55,15 +60,20 @@ namespace mrover { std::string mCameraFrameId; std::string mMapFrameId; + //Init method void onInit() override; + //Function to get SE3 from the point cloud + std::optional getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) const; + public: + //Node constructor ObjectDetectorNodelet() = default; + //Node Deconstructor ~ObjectDetectorNodelet() override = default; + //Callback for when ZED publishes data void imageCallback(sensor_msgs::PointCloud2ConstPtr const& msg); - - std::optional getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) const; }; } // namespace mrover \ No newline at end of file diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index b311afd08..2e8cf0874 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -5,18 +5,24 @@ #include #include #include "../point.hpp" +#include namespace mrover { void ObjectDetectorNodelet::imageCallback(sensor_msgs::PointCloud2ConstPtr const& msg) { + //Does the msg exist with a height and width assert(msg); assert(msg->height > 0); assert(msg->width > 0); + + //Adjust the picture size to be in line with the expected img size form the Point Cloud if (static_cast(msg->height) != mImg.rows || static_cast(msg->width) != mImg.cols) { NODELET_INFO("Image size changed from [%d %d] to [%u %u]", mImg.cols, mImg.rows, msg->width, msg->height); mImg = cv::Mat{static_cast(msg->height), static_cast(msg->width), CV_8UC4, cv::Scalar{0, 0, 0, 0}}; } + + //Convert the pointcloud data into rgba image auto* pixelPtr = reinterpret_cast(mImg.data); auto* pointPtr = reinterpret_cast(msg->data.data()); std::for_each(std::execution::par_unseq, pixelPtr, pixelPtr + mImg.total(), [&](cv::Vec4b& pixel) { @@ -27,20 +33,23 @@ namespace mrover { pixel[3] = pointPtr[i].a; }); + //Resize the image and change it from BGRA to BGR cv::Mat sizedImage; - cv::resize(mImg, sizedImage, cv::Size(640, 640)); cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGRA2BGR); + //Create the blob from the resized image cv::dnn::blobFromImage(sizedImage, mImageBlob, 1.0 / 255.0, cv::Size{640, 640}, cv::Scalar(), true, false); + //Run the blob through the model mInferenceWrapper.doDetections(mImageBlob); + //Retrieve the output from the model cv::Mat output = mInferenceWrapper.getOutputTensor(); + //Get model specific information int rows = output.rows; int dimensions = output.cols; - bool yolov8 = false; // yolov5 has an output of shape (batchSize, 25200, 85) (Num classes + box[x,y,w,h] + confidence[c]) // yolov8 has an output of shape (batchSize, 84, 8400) (Num classes + box[x,y,w,h]) @@ -53,6 +62,7 @@ namespace mrover { output = output.reshape(1, dimensions); cv::transpose(output, output); } + //Reinterpret data from the output to be in a usable form auto data = reinterpret_cast(output.data); //Model Information @@ -61,124 +71,107 @@ namespace mrover { float modelShapeWidth = 640; float modelShapeHeight = 640; + //Set model thresholds float modelScoreThreshold = 0.90; float modelNMSThreshold = 0.50; - + //Get x and y scale factors float x_factor = modelInputCols / modelShapeWidth; float y_factor = modelInputRows / modelShapeHeight; + //Init storage containers std::vector class_ids; std::vector confidences; std::vector boxes; + //Each of the possibilities do interpret the data for (int i = 0; i < rows; ++i) { - + //This should always be true if (yolov8) { + //This is because the first 4 points are box[x,y,w,h] float* classes_scores = data + 4; + //Create a mat to store all of the class scores cv::Mat scores(1, static_cast(classes.size()), CV_32FC1, classes_scores); cv::Point class_id; double maxClassScore; + //Find the max class score for the associated row minMaxLoc(scores, nullptr, &maxClassScore, nullptr, &class_id); + //Determine if the class is an acceptable confidence level else disregard if (maxClassScore > modelScoreThreshold) { + //Push back data points into storage containers confidences.push_back(static_cast(maxClassScore)); class_ids.push_back(class_id.x); + //Get the bounding box data float x = data[0]; float y = data[1]; float w = data[2]; float h = data[3]; + //Cast the corners into integers to be used on pixels int left = static_cast((x - 0.5 * w) * x_factor); int top = static_cast((y - 0.5 * h) * y_factor); - int width = static_cast(w * x_factor); int height = static_cast(h * y_factor); + //Push abck the box into storage boxes.emplace_back(left, top, width, height); } - } else // yolov5 - { - - float modelConfidenceThresholdl = 0.9; - - if (float confidence = data[4]; confidence >= modelConfidenceThresholdl) { - float* classes_scores = data + 5; - - cv::Mat scores(1, static_cast(classes.size()), CV_32FC1, classes_scores); - cv::Point class_id; - double max_class_score; - - minMaxLoc(scores, nullptr, &max_class_score, nullptr, &class_id); - - if (max_class_score > modelScoreThreshold) { - confidences.push_back(confidence); - class_ids.push_back(class_id.x); - - float x = data[0]; - float y = data[1]; - float w = data[2]; - float h = data[3]; - - int left = static_cast((x - 0.5 * w) * x_factor); - int top = static_cast((y - 0.5 * h) * y_factor); - - int width = static_cast(w * x_factor); - int height = static_cast(h * y_factor); - - boxes.emplace_back(left, top, width, height); - } - } + } else { //YOLO V5 + throw std::runtime_error("Something is wrong with interpretation"); } data += dimensions; } + //Coalesce the boxes into a smaller number of distinct boxes std::vector nms_result; cv::dnn::NMSBoxes(boxes, confidences, modelScoreThreshold, modelNMSThreshold, nms_result); + //Storage for the detection from the model std::vector detections{}; for(int idx : nms_result) { + //Init the detection Detection result; + + //Fill in the id and confidence for the class result.class_id = class_ids[idx]; result.confidence = confidences[idx]; - std::random_device rd; - std::mt19937 gen(rd()); - std::uniform_int_distribution dis(100, 255); - result.color = cv::Scalar(dis(gen), - dis(gen), - dis(gen)); - + //Fill in the class name and box information result.className = classes[result.class_id]; result.box = boxes[idx]; + //Push back the detection into the for storagevector detections.push_back(result); } + + //If there are detections locate them in 3D if (!detections.empty()) { + //Get the first detection to locate in 3D Detection firstDetection = detections[0]; + //Get the associated confidence with the object float classConfidence = firstDetection.confidence; + //Get the associated box with the object cv::Rect box = firstDetection.box; + //Fill out the msgData information to be published to the topic DetectedObject msgData; msgData.object_type = firstDetection.className; msgData.detection_confidence = classConfidence; - msgData.width = static_cast(box.width); msgData.height = static_cast(box.height); + //Calculate the center of the box std::pair center(box.x + box.width/2, box.y + box.height/2); - - ROS_INFO("center image coordinates x y: %d %d", center.first * static_cast(msg->width) / sizedImage.cols, center.second * static_cast(msg->height) / sizedImage.rows); - + + //Get the object's position in 3D from the point cloud if (std::optional objectLocation = getObjectInCamFromPixel(msg, center.first * static_cast(msg->width) / sizedImage.cols, center.second * static_cast(msg->height) / sizedImage.rows); objectLocation) { - ROS_INFO("x y z position %f %f %f", objectLocation->position().x(), objectLocation->position().y(), objectLocation->position().z()); - // Publish tag to immediate try{ std::string immediateFrameId = "detectedobject"; @@ -198,9 +191,6 @@ namespace mrover { } } - msgData.width = static_cast(box.width); - msgData.height = static_cast(box.height); - //Get the heading float objectHeading; float zedFOV = 54; //54 @ 720; 42 @ 1080 @@ -209,9 +199,10 @@ namespace mrover { objectHeading = xCenter * fovPerPixel; msgData.bearing = objectHeading; - //publish the data to NAV + //Publish the data to the topic mDetectionData.publish(msgData); + //Draw the detected object's bounding boxes on the image for each of the objects detected for (size_t i = 0; i < detections.size(); i++) { std::cout << detections[i].className << i << std::endl; @@ -227,37 +218,42 @@ namespace mrover { int font_weight = 2; putText(sizedImage, detections[i].className, text_position, cv::FONT_HERSHEY_COMPLEX, font_size, font_Color, font_weight); //Putting the text in the matrix// } - //Show the image - - //Print the type of objected detected } if (mDebugImgPub.getNumSubscribers() > 0 || true) { ROS_INFO("Publishing Debug Img"); - // Create sensor msg image - sensor_msgs::Image newDebugImageMessage; + // Init sensor msg image + sensor_msgs::Image newDebugImageMessage;//I chose regular msg not ptr so it can be used outside of this process + //Convert the image back to BGRA for ROS cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGR2BGRA); + //Fill in the msg image size newDebugImageMessage.height = sizedImage.rows; newDebugImageMessage.width = sizedImage.cols; + //Fill in the encoding type newDebugImageMessage.encoding = sensor_msgs::image_encodings::BGRA8; //Calculate the step for the imgMsg newDebugImageMessage.step = sizedImage.channels() * sizedImage.cols; newDebugImageMessage.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; + //Pointer to the the image data auto imgPtr = sizedImage.data; + //Calculate the image size size_t size = newDebugImageMessage.step * newDebugImageMessage.height; newDebugImageMessage.data.resize(size); + //Copy the data to the image memcpy(newDebugImageMessage.data.data(), imgPtr, size); + //Publish the image to the topic mDebugImgPub.publish(newDebugImageMessage); } } + std::optional ObjectDetectorNodelet::getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) const { assert(cloudPtr); diff --git a/src/perception/object_detector/pch.hpp b/src/perception/object_detector/pch.hpp index d8002572e..3417b5a3b 100644 --- a/src/perception/object_detector/pch.hpp +++ b/src/perception/object_detector/pch.hpp @@ -47,9 +47,7 @@ #include #include - #include -//#include #include #include From 6f2674f475a75c31d69c69ef7d6b184870ea1f8f Mon Sep 17 00:00:00 2001 From: John Date: Sun, 14 Jan 2024 11:14:48 -0500 Subject: [PATCH 102/197] Tested --- src/perception/object_detector/object_detector.processing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 2e8cf0874..6721dc6db 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -121,7 +121,7 @@ namespace mrover { boxes.emplace_back(left, top, width, height); } } else { //YOLO V5 - throw std::runtime_error("Something is wrong with interpretation"); + throw std::runtime_error("Something is wrong with interpretation"); } data += dimensions; From 7ec79579eec547e7a3333273622153a6df52a0dc Mon Sep 17 00:00:00 2001 From: John Date: Tue, 16 Jan 2024 19:52:59 -0500 Subject: [PATCH 103/197] Immediate and Permanent Thresholding on DetectedObject --- .../object_detector/object_detector.cpp | 13 +++++++++ .../object_detector/object_detector.hpp | 9 +++++++ .../object_detector.processing.cpp | 27 ++++++++++++++----- 3 files changed, 43 insertions(+), 6 deletions(-) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 0b8e8f690..79dc10e36 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -9,6 +9,8 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); + + //Create the inference wrapper mInferenceWrapper = InferenceWrapper(std::string("//home//john//Desktop//Rover//best.onnx"), cv::Size(640, 640), ""); @@ -17,9 +19,20 @@ namespace mrover { mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); mDetectionData = mNh.advertise("/object_detector/detected_object", 1); + //READ ROS PARAMETERS + //Create the Reference Frames mNh.param("camera_frame", mCameraFrameId, "zed2i_left_camera_frame"); mNh.param("world_frame", mMapFrameId, "map"); + + //Read ROS Params + mNh.param("obj_increment_weight", mObjIncrementWeight, 1); + mNh.param("obj_decrement_weight", mObjDecrementWeight, 5); + mNh.param("obj_decrement_threshold", mObjHitThreshold, 100); + mNh.param("obj_max_hitcount", mObjMaxHitcount, 200); + + //Initialize Object Hit Cout to Zeros + mHitCount = 0; } } // namespace mrover diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index d005f4d91..c327cf71c 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -60,6 +60,15 @@ namespace mrover { std::string mCameraFrameId; std::string mMapFrameId; + //Hit counter + int mHitCount; + + //Constants after initialization + int mObjIncrementWeight; + int mObjDecrementWeight; + int mObjHitThreshold; + int mObjMaxHitcount; + //Init method void onInit() override; diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 6721dc6db..8d1a3f430 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -6,6 +6,7 @@ #include #include "../point.hpp" #include +#include namespace mrover { @@ -72,7 +73,7 @@ namespace mrover { float modelShapeHeight = 640; //Set model thresholds - float modelScoreThreshold = 0.90; + float modelScoreThreshold = 0.95; float modelNMSThreshold = 0.50; //Get x and y scale factors @@ -109,7 +110,7 @@ namespace mrover { float x = data[0]; float y = data[1]; float w = data[2]; - float h = data[3]; + float h = data[3]; //Cast the corners into integers to be used on pixels int left = static_cast((x - 0.5 * w) * x_factor); @@ -172,14 +173,21 @@ namespace mrover { //Get the object's position in 3D from the point cloud if (std::optional objectLocation = getObjectInCamFromPixel(msg, center.first * static_cast(msg->width) / sizedImage.cols, center.second * static_cast(msg->height) / sizedImage.rows); objectLocation) { - // Publish tag to immediate try{ - std::string immediateFrameId = "detectedobject"; + //Publish Immediate + std::string immediateFrameId = "immediateDetectedObject"; SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, objectLocation.value()); - SE3 tagInsideCamera = SE3::fromTfTree(mTfBuffer, mMapFrameId, immediateFrameId); + //Since the object is seen we need to decrement + mHitCount = std::min(mObjMaxHitcount, mHitCount + mObjIncrementWeight); + + //Only publish to permament if we are confident in the object + if(mHitCount > mObjHitThreshold){ + std::string permanentFrameId = "detectedObject"; + SE3::pushToTfTree(mTfBroadcaster, permanentFrameId, mCameraFrameId, objectLocation.value()); + } - SE3::pushToTfTree(mTfBroadcaster, "detectedobjectFR", mMapFrameId, tagInsideCamera); + SE3 tagInsideCamera = SE3::fromTfTree(mTfBuffer, mMapFrameId, immediateFrameId); } catch (tf2::ExtrapolationException const&) { NODELET_WARN("Old data for immediate tag"); } catch (tf2::LookupException const&) { @@ -218,7 +226,14 @@ namespace mrover { int font_weight = 2; putText(sizedImage, detections[i].className, text_position, cv::FONT_HERSHEY_COMPLEX, font_size, font_Color, font_weight); //Putting the text in the matrix// } + } else { + mHitCount = std::max(0, mHitCount - mObjDecrementWeight); + } + + //DEBUG TODO REMOVE + ROS_INFO("Hit Count: %zu", mHitCount); + if (mDebugImgPub.getNumSubscribers() > 0 || true) { ROS_INFO("Publishing Debug Img"); From fa6a6f0f2e0112cf1eedb9e09fbe9fc40466c1aa Mon Sep 17 00:00:00 2001 From: Marvin Jirapongsuwan Date: Thu, 18 Jan 2024 15:00:16 -0500 Subject: [PATCH 104/197] wrote mLongRangeTagsPub --- .../long_range_cam/long_range_tag_detector.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp index b77195acb..777b3ba92 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp @@ -1,4 +1,6 @@ #include "long_range_tag_detector.hpp" +#include "mrover/LongRangeTags.h" +#include namespace mrover { @@ -16,8 +18,11 @@ namespace mrover { mPnh.param("tag_decrement_weight", mTagDecrementWeight, 1); mImgPub = mNh.advertise("long_range_tag_detection", 1); + mLongRangeTagsPub = mNh.advertise("tags", 1); - mImgSub = mNh.subscribe("long_range_cam/image", 1, &LongRangeTagDetectorNodelet::imageCallback, this); + // topic: long_range_cam/image + //todo + mImgSub = mNh.subscribe(, 1, &LongRangeTagDetectorNodelet::imageCallback, this); mDictionary = cv::makePtr(cv::aruco::getPredefinedDictionary(dictionaryNumber)); mServiceEnableDetections = mNh.advertiseService("enable_detections", &LongRangeTagDetectorNodelet::enableDetectionsCallback, this); From e7226843d61b948ce661f622362bbd5e5f145424 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 18 Jan 2024 20:20:17 -0500 Subject: [PATCH 105/197] Working IRL --- launch/zed.launch | 2 +- launch/zed_test.launch | 12 +++++++++--- src/perception/object_detector/object_detector.cpp | 8 ++++---- 3 files changed, 14 insertions(+), 8 deletions(-) diff --git a/launch/zed.launch b/launch/zed.launch index 342ed8f10..80540e07a 100644 --- a/launch/zed.launch +++ b/launch/zed.launch @@ -6,7 +6,7 @@ - diff --git a/launch/zed_test.launch b/launch/zed_test.launch index 0232b23f7..ef352a45a 100644 --- a/launch/zed_test.launch +++ b/launch/zed_test.launch @@ -7,6 +7,13 @@ + + + + + + + @@ -21,14 +28,13 @@ args="manager" output="screen"/> - - - + --> ("world_frame", mMapFrameId, "map"); //Read ROS Params - mNh.param("obj_increment_weight", mObjIncrementWeight, 1); - mNh.param("obj_decrement_weight", mObjDecrementWeight, 5); - mNh.param("obj_decrement_threshold", mObjHitThreshold, 100); - mNh.param("obj_max_hitcount", mObjMaxHitcount, 200); + mNh.param("obj_increment_weight", mObjIncrementWeight, 2); + mNh.param("obj_decrement_weight", mObjDecrementWeight, 1); + mNh.param("obj_hitcount_threshold", mObjHitThreshold, 50); + mNh.param("obj_hitcount_max", mObjMaxHitcount, 60); //Initialize Object Hit Cout to Zeros mHitCount = 0; From ae8440079ea88083cffb234a556002e3daa09cbc Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 18 Jan 2024 21:11:28 -0500 Subject: [PATCH 106/197] Changes for obj detect working in sim --- launch/auton.launch | 5 +++ launch/new_sim.launch | 21 +++++----- src/perception/object_detector/inference.cu | 38 ++++++++++--------- .../object_detector/object_detector.cpp | 13 ++++--- .../object_detector.processing.cpp | 4 +- src/perception/object_detector/pch.hpp | 1 + src/simulator/urdf.camera.cpp | 2 +- 7 files changed, 49 insertions(+), 35 deletions(-) diff --git a/launch/auton.launch b/launch/auton.launch index a7916eb56..11f8e19f9 100644 --- a/launch/auton.launch +++ b/launch/auton.launch @@ -5,6 +5,7 @@ + + + + pkg="nodelet" type="nodelet" name="long_range_tag_detector" + args="load mrover/LongRangeTagDetectorNodelet nodelet_manager" output="screen" /> + \ No newline at end of file diff --git a/plugins/tag_detector_plugin.xml b/plugins/tag_detector_plugin.xml index bd6238412..babad0bed 100644 --- a/plugins/tag_detector_plugin.xml +++ b/plugins/tag_detector_plugin.xml @@ -1,4 +1,4 @@ - + diff --git a/src/perception/long_range_cam/main.cpp b/src/perception/long_range_cam/main.cpp new file mode 100644 index 000000000..0e5e833da --- /dev/null +++ b/src/perception/long_range_cam/main.cpp @@ -0,0 +1,23 @@ +#include "long_range_cam.hpp" + +#ifdef MROVER_IS_NODELET + +#include +PLUGINLIB_EXPORT_CLASS(mrover::LongRangeCamNodelet, nodelet::Nodelet) + +#else + +int main(int argc, char** argv) { + ros::init(argc, argv, "usb_camera"); + + // Start the ZED Nodelet + nodelet::Loader nodelet; + std::cout << ros::this_node::getName() << std::endl; + nodelet.load(ros::this_node::getName(), "mrover/LongRangeCamNodelet", ros::names::getRemappings(), {}); + + ros::spin(); + + return EXIT_SUCCESS; +} + +#endif \ No newline at end of file diff --git a/src/perception/long_range_cam/pch.hpp b/src/perception/long_range_cam/pch.hpp index d39a1b1aa..f0c16770d 100644 --- a/src/perception/long_range_cam/pch.hpp +++ b/src/perception/long_range_cam/pch.hpp @@ -13,8 +13,6 @@ #include #include -#include - #include #include #include diff --git a/src/perception/tag_detector/long_range_cam/main.cpp b/src/perception/tag_detector/long_range_cam/main.cpp new file mode 100644 index 000000000..d62371420 --- /dev/null +++ b/src/perception/tag_detector/long_range_cam/main.cpp @@ -0,0 +1,22 @@ +#include "long_range_tag_detector.hpp" + +#ifdef MROVER_IS_NODELET + +#include +PLUGINLIB_EXPORT_CLASS(mrover::LongRangeTagDetectorNodelet, nodelet::Nodelet) + +#else + +int main(int argc, char** argv) { + ros::init(argc, argv, "long_range_tag_detector"); + + // Start the long range cam Nodelet + nodelet::Loader nodelet; + nodelet.load(ros::this_node::getName(), "mrover/LongRangeTagDetectorNodelet", ros::names::getRemappings(), {}); + + ros::spin(); + + return EXIT_SUCCESS; +} + +#endif \ No newline at end of file diff --git a/src/perception/tag_detector/long_range_cam/pch.hpp b/src/perception/tag_detector/long_range_cam/pch.hpp index fad6365fd..afaebe976 100644 --- a/src/perception/tag_detector/long_range_cam/pch.hpp +++ b/src/perception/tag_detector/long_range_cam/pch.hpp @@ -12,8 +12,6 @@ #include #include -#include - #include #include #include diff --git a/src/perception/tag_detector/main.cpp b/src/perception/tag_detector/zed/main.cpp similarity index 100% rename from src/perception/tag_detector/main.cpp rename to src/perception/tag_detector/zed/main.cpp diff --git a/src/perception/tag_detector/zed/tag_detector.processing.cpp b/src/perception/tag_detector/zed/tag_detector.processing.cpp index 18e7fcdb9..a101d4fff 100644 --- a/src/perception/tag_detector/zed/tag_detector.processing.cpp +++ b/src/perception/tag_detector/zed/tag_detector.processing.cpp @@ -1,192 +1,192 @@ #include "tag_detector.hpp" -#include "../../point.hpp" +#include "point.hpp" namespace mrover { -/** + /** * @brief Retrieve the pose of the tag in camera space * @param msg 3D Point Cloud with points stored relative to the camera * @param u X Pixel Position * @param v Y Pixel Position */ -std::optional TagDetectorNodelet::getTagInCamFromPixel( - sensor_msgs::PointCloud2ConstPtr const &cloudPtr, size_t u, size_t v) { - assert(cloudPtr); - - if (u >= cloudPtr->width || v >= cloudPtr->height) { - NODELET_WARN("Tag center out of bounds: [%zu %zu]", u, v); - return std::nullopt; - } - - Point point = reinterpret_cast( - cloudPtr->data.data())[u + v * cloudPtr->width]; - - if (!std::isfinite(point.x) || !std::isfinite(point.y) || - !std::isfinite(point.z)) { - NODELET_WARN("Tag center point not finite: [%f %f %f]", point.x, point.y, - point.z); - return std::nullopt; - } - - return std::make_optional(R3{point.x, point.y, point.z}, SO3{}); -} + std::optional TagDetectorNodelet::getTagInCamFromPixel( + sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) { + assert(cloudPtr); + + if (u >= cloudPtr->width || v >= cloudPtr->height) { + NODELET_WARN("Tag center out of bounds: [%zu %zu]", u, v); + return std::nullopt; + } + + Point point = reinterpret_cast( + cloudPtr->data.data())[u + v * cloudPtr->width]; + + if (!std::isfinite(point.x) || !std::isfinite(point.y) || + !std::isfinite(point.z)) { + NODELET_WARN("Tag center point not finite: [%f %f %f]", point.x, point.y, + point.z); + return std::nullopt; + } + + return std::make_optional(R3{point.x, point.y, point.z}, SO3{}); + } -/** + /** * For each tag we have detected so far, fuse point cloud information. * This information is where it is in the world. * * @param msg Point cloud message */ -void TagDetectorNodelet::pointCloudCallback( - sensor_msgs::PointCloud2ConstPtr const &msg) { - assert(msg); - assert(msg->height > 0); - assert(msg->width > 0); - - if (!mEnableDetections) - return; - - mProfiler.beginLoop(); - - NODELET_DEBUG("Got point cloud %d", msg->header.seq); - - // OpenCV needs a dense BGR image |BGR|...| but out point cloud is - // |BGRAXYZ...|...| So we need to copy the data into the correct format - if (static_cast(msg->height) != mImg.rows || - static_cast(msg->width) != mImg.cols) { - NODELET_INFO("Image size changed from [%d %d] to [%u %u]", mImg.cols, - mImg.rows, msg->width, msg->height); - mImg = cv::Mat{static_cast(msg->height), static_cast(msg->width), - CV_8UC3, cv::Scalar{0, 0, 0}}; - } - auto *pixelPtr = reinterpret_cast(mImg.data); - auto *pointPtr = reinterpret_cast(msg->data.data()); - std::for_each(std::execution::par_unseq, pixelPtr, pixelPtr + mImg.total(), - [&](cv::Vec3b &pixel) { - size_t i = &pixel - pixelPtr; - pixel[0] = pointPtr[i].b; - pixel[1] = pointPtr[i].g; - pixel[2] = pointPtr[i].r; - }); - mProfiler.measureEvent("Convert"); - - // Call thresholding - publishThresholdedImage(); - mProfiler.measureEvent("Threshold"); - - // Detect the tag vertices in screen space and their respective ids - // {mImmediateCorneres, mImmediateIds} are the outputs from OpenCV - cv::aruco::detectMarkers(mImg, mDictionary, mImmediateCorners, mImmediateIds, - mDetectorParams); - NODELET_DEBUG("OpenCV detect size: %zu", mImmediateIds.size()); - mProfiler.measureEvent("OpenCV Detect"); - - // Update ID, image center, and increment hit count for all detected tags - for (size_t i = 0; i < mImmediateIds.size(); ++i) { - int id = mImmediateIds[i]; - Tag &tag = mTags[id]; - tag.hitCount = - std::clamp(tag.hitCount + mTagIncrementWeight, 0, mMaxHitCount); - tag.id = id; - tag.imageCenter = - std::reduce(mImmediateCorners[i].begin(), mImmediateCorners[i].end()) / - static_cast(mImmediateCorners[i].size()); - tag.tagInCam = getTagInCamFromPixel(msg, std::lround(tag.imageCenter.x), - std::lround(tag.imageCenter.y)); - - if (tag.tagInCam) { - // Publish tag to immediate - std::string immediateFrameId = - "immediateFiducial" + std::to_string(tag.id); - SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, - tag.tagInCam.value()); - } - } - - // Handle tags that were not seen this update - // Decrement their hit count and remove if they hit zero - auto it = mTags.begin(); - while (it != mTags.end()) { - auto &[id, tag] = *it; - if (std::ranges::find(mImmediateIds, id) == mImmediateIds.end()) { - tag.hitCount -= mTagDecrementWeight; - tag.tagInCam = std::nullopt; - if (tag.hitCount <= 0) { - it = mTags.erase(it); - continue; - } - } - ++it; - } - - // Publish all tags to the tf tree that have been seen enough times - for (auto const &[id, tag] : mTags) { - if (tag.hitCount >= mMinHitCountBeforePublish && tag.tagInCam) { - try { - std::string immediateFrameId = - "immediateFiducial" + std::to_string(tag.id); - // Publish tag to odom - std::string const &parentFrameId = - mUseOdom ? mOdomFrameId : mMapFrameId; - SE3 tagInParent = - SE3::fromTfTree(mTfBuffer, parentFrameId, immediateFrameId); - SE3::pushToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), - parentFrameId, tagInParent); - } catch (tf2::ExtrapolationException const &) { - NODELET_WARN("Old data for immediate tag"); - } catch (tf2::LookupException const &) { - NODELET_WARN("Expected transform for immediate tag"); - } catch (tf::ConnectivityException const &) { - NODELET_WARN( - "Expected connection to odom frame. Is visual odometry running?"); - } - } - } - - if (mPublishImages && mImgPub.getNumSubscribers()) { - - cv::aruco::drawDetectedMarkers(mImg, mImmediateCorners, mImmediateIds); - // Max number of tags the hit counter can display = 10; - if (!mTags.empty()) { - // TODO: remove some magic numbers in this block - int tagCount = 1; - auto tagBoxWidth = static_cast(mImg.cols / (mTags.size() * 2)); - for (auto &[id, tag] : mTags) { - cv::Scalar color{255, 0, 0}; - cv::Point pt{tagBoxWidth * tagCount, mImg.rows / 10}; - std::string text = - "id" + std::to_string(id) + ":" + std::to_string(tag.hitCount); - cv::putText(mImg, text, pt, cv::FONT_HERSHEY_COMPLEX, mImg.cols / 800.0, - color, mImg.cols / 300); - - ++tagCount; - } + void TagDetectorNodelet::pointCloudCallback( + sensor_msgs::PointCloud2ConstPtr const& msg) { + assert(msg); + assert(msg->height > 0); + assert(msg->width > 0); + + if (!mEnableDetections) + return; + + mProfiler.beginLoop(); + + NODELET_DEBUG("Got point cloud %d", msg->header.seq); + + // OpenCV needs a dense BGR image |BGR|...| but out point cloud is + // |BGRAXYZ...|...| So we need to copy the data into the correct format + if (static_cast(msg->height) != mImg.rows || + static_cast(msg->width) != mImg.cols) { + NODELET_INFO("Image size changed from [%d %d] to [%u %u]", mImg.cols, + mImg.rows, msg->width, msg->height); + mImg = cv::Mat{static_cast(msg->height), static_cast(msg->width), + CV_8UC3, cv::Scalar{0, 0, 0}}; + } + auto* pixelPtr = reinterpret_cast(mImg.data); + auto* pointPtr = reinterpret_cast(msg->data.data()); + std::for_each(std::execution::par_unseq, pixelPtr, pixelPtr + mImg.total(), + [&](cv::Vec3b& pixel) { + size_t i = &pixel - pixelPtr; + pixel[0] = pointPtr[i].b; + pixel[1] = pointPtr[i].g; + pixel[2] = pointPtr[i].r; + }); + mProfiler.measureEvent("Convert"); + + // Call thresholding + publishThresholdedImage(); + mProfiler.measureEvent("Threshold"); + + // Detect the tag vertices in screen space and their respective ids + // {mImmediateCorneres, mImmediateIds} are the outputs from OpenCV + cv::aruco::detectMarkers(mImg, mDictionary, mImmediateCorners, mImmediateIds, + mDetectorParams); + NODELET_DEBUG("OpenCV detect size: %zu", mImmediateIds.size()); + mProfiler.measureEvent("OpenCV Detect"); + + // Update ID, image center, and increment hit count for all detected tags + for (size_t i = 0; i < mImmediateIds.size(); ++i) { + int id = mImmediateIds[i]; + Tag& tag = mTags[id]; + tag.hitCount = + std::clamp(tag.hitCount + mTagIncrementWeight, 0, mMaxHitCount); + tag.id = id; + tag.imageCenter = + std::reduce(mImmediateCorners[i].begin(), mImmediateCorners[i].end()) / + static_cast(mImmediateCorners[i].size()); + tag.tagInCam = getTagInCamFromPixel(msg, std::lround(tag.imageCenter.x), + std::lround(tag.imageCenter.y)); + + if (tag.tagInCam) { + // Publish tag to immediate + std::string immediateFrameId = + "immediateFiducial" + std::to_string(tag.id); + SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, + tag.tagInCam.value()); + } + } + + // Handle tags that were not seen this update + // Decrement their hit count and remove if they hit zero + auto it = mTags.begin(); + while (it != mTags.end()) { + auto& [id, tag] = *it; + if (std::ranges::find(mImmediateIds, id) == mImmediateIds.end()) { + tag.hitCount -= mTagDecrementWeight; + tag.tagInCam = std::nullopt; + if (tag.hitCount <= 0) { + it = mTags.erase(it); + continue; + } + } + ++it; + } + + // Publish all tags to the tf tree that have been seen enough times + for (auto const& [id, tag]: mTags) { + if (tag.hitCount >= mMinHitCountBeforePublish && tag.tagInCam) { + try { + std::string immediateFrameId = + "immediateFiducial" + std::to_string(tag.id); + // Publish tag to odom + std::string const& parentFrameId = + mUseOdom ? mOdomFrameId : mMapFrameId; + SE3 tagInParent = + SE3::fromTfTree(mTfBuffer, parentFrameId, immediateFrameId); + SE3::pushToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), + parentFrameId, tagInParent); + } catch (tf2::ExtrapolationException const&) { + NODELET_WARN("Old data for immediate tag"); + } catch (tf2::LookupException const&) { + NODELET_WARN("Expected transform for immediate tag"); + } catch (tf2::ConnectivityException const&) { + NODELET_WARN( + "Expected connection to odom frame. Is visual odometry running?"); + } + } + } + + if (mPublishImages && mImgPub.getNumSubscribers()) { + + cv::aruco::drawDetectedMarkers(mImg, mImmediateCorners, mImmediateIds); + // Max number of tags the hit counter can display = 10; + if (!mTags.empty()) { + // TODO: remove some magic numbers in this block + int tagCount = 1; + auto tagBoxWidth = static_cast(mImg.cols / (mTags.size() * 2)); + for (auto& [id, tag]: mTags) { + cv::Scalar color{255, 0, 0}; + cv::Point pt{tagBoxWidth * tagCount, mImg.rows / 10}; + std::string text = + "id" + std::to_string(id) + ":" + std::to_string(tag.hitCount); + cv::putText(mImg, text, pt, cv::FONT_HERSHEY_COMPLEX, mImg.cols / 800.0, + color, mImg.cols / 300); + + ++tagCount; + } + } + mImgMsg.header.seq = mSeqNum; + mImgMsg.header.stamp = ros::Time::now(); + mImgMsg.header.frame_id = "zed2i_left_camera_frame"; + mImgMsg.height = mImg.rows; + mImgMsg.width = mImg.cols; + mImgMsg.encoding = sensor_msgs::image_encodings::BGR8; + mImgMsg.step = mImg.step; + mImgMsg.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; + size_t size = mImgMsg.step * mImgMsg.height; + mImgMsg.data.resize(size); + std::uninitialized_copy(std::execution::par_unseq, mImg.data, + mImg.data + size, mImgMsg.data.begin()); + mImgPub.publish(mImgMsg); + } + + size_t detectedCount = mImmediateIds.size(); + NODELET_INFO_COND(!mPrevDetectedCount.has_value() || + detectedCount != mPrevDetectedCount.value(), + "Detected %zu markers", detectedCount); + mPrevDetectedCount = detectedCount; + + mProfiler.measureEvent("Publish"); + + mSeqNum++; } - mImgMsg.header.seq = mSeqNum; - mImgMsg.header.stamp = ros::Time::now(); - mImgMsg.header.frame_id = "zed2i_left_camera_frame"; - mImgMsg.height = mImg.rows; - mImgMsg.width = mImg.cols; - mImgMsg.encoding = sensor_msgs::image_encodings::BGR8; - mImgMsg.step = mImg.step; - mImgMsg.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; - size_t size = mImgMsg.step * mImgMsg.height; - mImgMsg.data.resize(size); - std::uninitialized_copy(std::execution::par_unseq, mImg.data, - mImg.data + size, mImgMsg.data.begin()); - mImgPub.publish(mImgMsg); - } - - size_t detectedCount = mImmediateIds.size(); - NODELET_INFO_COND(!mPrevDetectedCount.has_value() || - detectedCount != mPrevDetectedCount.value(), - "Detected %zu markers", detectedCount); - mPrevDetectedCount = detectedCount; - - mProfiler.measureEvent("Publish"); - - mSeqNum++; -} } // namespace mrover From b9d69574904cb1957bb343208993a5294e9826a9 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Mon, 29 Jan 2024 21:28:22 -0500 Subject: [PATCH 139/197] remove mains from percep nodelets --- src/perception/long_range_cam/long_range_cam.cpp | 13 ------------- .../long_range_cam/long_range_tag_detector.cpp | 12 ------------ 2 files changed, 25 deletions(-) diff --git a/src/perception/long_range_cam/long_range_cam.cpp b/src/perception/long_range_cam/long_range_cam.cpp index 16388d0e3..783ffe52d 100644 --- a/src/perception/long_range_cam/long_range_cam.cpp +++ b/src/perception/long_range_cam/long_range_cam.cpp @@ -82,19 +82,6 @@ namespace mrover { } // namespace mrover -int main(int argc, char** argv) { - ros::init(argc, argv, "usb_camera"); - - // Start the ZED Nodelet - nodelet::Loader nodelet; - std::cout << ros::this_node::getName() << std::endl; - nodelet.load(ros::this_node::getName(), "mrover/LongRangeCamNodelet", ros::names::getRemappings(), {}); - - ros::spin(); - - return EXIT_SUCCESS; -} - #ifdef MROVER_IS_NODELET #include PLUGINLIB_EXPORT_CLASS(mrover::LongRangeCamNodelet, nodelet::Nodelet) diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp index e85126d8a..45f72b9ed 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp @@ -143,17 +143,5 @@ namespace mrover { } // namespace mrover -int main(int argc, char** argv) { - ros::init(argc, argv, "long_range_tag_detector"); - - // Start the long range cam Nodelet - nodelet::Loader nodelet; - nodelet.load(ros::this_node::getName(), "mrover/LongRangeTagDetectorNodelet", ros::names::getRemappings(), {}); - - ros::spin(); - - return EXIT_SUCCESS; -} - #include PLUGINLIB_EXPORT_CLASS(mrover::LongRangeTagDetectorNodelet, nodelet::Nodelet) From 34818a966550f12492042d43a48df7382f17bc42 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Tue, 30 Jan 2024 15:14:58 -0500 Subject: [PATCH 140/197] nav includes --- src/navigation/long_range.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/navigation/long_range.py b/src/navigation/long_range.py index 500511517..b1e132dfb 100644 --- a/src/navigation/long_range.py +++ b/src/navigation/long_range.py @@ -15,8 +15,8 @@ from util.state_lib.state import State from abc import abstractmethod import state -from state import ApproachTargetBaseState -from state import approach_post +from navigation.approach_target_base import ApproachTargetBaseState +import approach_post import numpy as np import math From 92379ab34a472131053b58940ab436aa2f193d39 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Tue, 30 Jan 2024 16:11:12 -0500 Subject: [PATCH 141/197] add transtions in navigation.py --- .catkin_tools/CATKIN_IGNORE | 0 .catkin_tools/README | 13 ----------- .catkin_tools/VERSION | 1 - .catkin_tools/profiles/default/build.yaml | 22 ------------------- .../catkin_tools_prebuild/package.xml | 10 --------- config/simulator/simulator.yaml | 18 +++++++-------- scripts/debug_course_publisher.py | 2 +- src/navigation/context.py | 3 +++ src/navigation/long_range.py | 18 ++++----------- src/navigation/navigation.py | 15 +++++++++++-- 10 files changed, 30 insertions(+), 72 deletions(-) delete mode 100644 .catkin_tools/CATKIN_IGNORE delete mode 100644 .catkin_tools/README delete mode 100644 .catkin_tools/VERSION delete mode 100644 .catkin_tools/profiles/default/build.yaml delete mode 100644 .catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml diff --git a/.catkin_tools/CATKIN_IGNORE b/.catkin_tools/CATKIN_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/.catkin_tools/README b/.catkin_tools/README deleted file mode 100644 index 4706f4756..000000000 --- a/.catkin_tools/README +++ /dev/null @@ -1,13 +0,0 @@ -# Catkin Tools Metadata - -This directory was generated by catkin_tools and it contains persistent -configuration information used by the `catkin` command and its sub-commands. - -Each subdirectory of the `profiles` directory contains a set of persistent -configuration options for separate profiles. The default profile is called -`default`. If another profile is desired, it can be described in the -`profiles.yaml` file in this directory. - -Please see the catkin_tools documentation before editing any files in this -directory. Most actions can be performed with the `catkin` command-line -program. diff --git a/.catkin_tools/VERSION b/.catkin_tools/VERSION deleted file mode 100644 index f76f91317..000000000 --- a/.catkin_tools/VERSION +++ /dev/null @@ -1 +0,0 @@ -0.9.2 \ No newline at end of file diff --git a/.catkin_tools/profiles/default/build.yaml b/.catkin_tools/profiles/default/build.yaml deleted file mode 100644 index 910ef4283..000000000 --- a/.catkin_tools/profiles/default/build.yaml +++ /dev/null @@ -1,22 +0,0 @@ -authors: [] -blacklist: [] -build_space: build -catkin_make_args: [] -cmake_args: [] -devel_layout: linked -devel_space: devel -extend_path: null -extends: null -install: false -install_space: install -isolate_install: false -jobs_args: [] -licenses: -- TODO -log_space: logs -maintainers: [] -make_args: [] -source_space: src -use_env_cache: false -use_internal_make_jobserver: true -whitelist: [] diff --git a/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml b/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml deleted file mode 100644 index 134c59ade..000000000 --- a/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml +++ /dev/null @@ -1,10 +0,0 @@ - - catkin_tools_prebuild - - This package is used to generate catkin setup files. - - 0.0.0 - BSD - jbohren - catkin - diff --git a/config/simulator/simulator.yaml b/config/simulator/simulator.yaml index acd6866b2..910c97198 100644 --- a/config/simulator/simulator.yaml +++ b/config/simulator/simulator.yaml @@ -2,39 +2,39 @@ objects: - type: urdf name: rover uri: package://mrover/urdf/rover/rover.urdf.xacro - translation: [ 0, 0, 0.5 ] + translation: [0, 0, 0.5] - type: urdf name: world uri: package://mrover/urdf/world/world.urdf.xacro - type: urdf name: tag_0 uri: package://mrover/urdf/world/tag_0.urdf.xacro - translation: [ -3, -3, 0.7 ] + translation: [5, 0, 0.7] - type: urdf name: tag_1 uri: package://mrover/urdf/world/tag_1.urdf.xacro - translation: [ 15, -14, 2.4 ] + translation: [15, -14, 2.4] - type: urdf name: hammer uri: package://mrover/urdf/world/hammer.urdf.xacro - translation: [ 8, 7, 0.6 ] + translation: [8, 7, 0.6] - type: urdf name: bottle uri: package://mrover/urdf/world/bottle.urdf.xacro - translation: [ -6.25, 10.5, 0.3 ] + translation: [-6.25, 10.5, 0.3] - type: urdf name: rock1 uri: package://mrover/urdf/world/rock.urdf.xacro - translation: [ 2, -2, 0.1 ] + translation: [2, -2, 0.1] - type: urdf name: rock2 uri: package://mrover/urdf/world/rock.urdf.xacro - translation: [ 7, -6, 0.2 ] + translation: [7, -6, 0.2] - type: urdf name: rock3 uri: package://mrover/urdf/world/rock.urdf.xacro - translation: [ 8, -10, 1 ] + translation: [8, -10, 1] - type: urdf name: rock4 uri: package://mrover/urdf/world/rock.urdf.xacro - translation: [ 9, -2, 0.2 ] \ No newline at end of file + translation: [9, -2, 0.2] diff --git a/scripts/debug_course_publisher.py b/scripts/debug_course_publisher.py index 3bfe991bc..5f1468ba0 100755 --- a/scripts/debug_course_publisher.py +++ b/scripts/debug_course_publisher.py @@ -26,7 +26,7 @@ # ) ( Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)), - SE3(position=np.array([-2, -2, 0])), + SE3(position=np.array([3, 0, 0])), ) # ( # Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)), diff --git a/src/navigation/context.py b/src/navigation/context.py index fa68db9c5..bc3fc19bf 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -151,6 +151,9 @@ def push_frame(self, tags: List[LongRangeTag]) -> None: self.__data[tag.id] = self.TagData(hit_count=1, tag=tag, time=rospy.get_time()) def get(self, tag_id: int) -> Optional[LongRangeTag]: + if len(self.__data) == 0: + return None + time_difference = rospy.get_time() - self.__data[tag_id].time if ( tag_id in self.__data diff --git a/src/navigation/long_range.py b/src/navigation/long_range.py index b1e132dfb..2f13bc4b0 100644 --- a/src/navigation/long_range.py +++ b/src/navigation/long_range.py @@ -1,24 +1,14 @@ import tf2_ros import rospy -from aenum import Enum, NoAlias -from geometry_msgs.msg import Twist + from util.ros_utils import get_rosparam -from context import Context from util.state_lib.state import State -from util.state_lib.state_machine import StateMachine from util.np_utils import normalized -from abc import ABC, abstractmethod -from state import DoneState -from search import SearchState -from util.state_lib.state_publisher_server import StatePublisher from typing import Optional -from util.state_lib.state import State -from abc import abstractmethod -import state + +from navigation import approach_post from navigation.approach_target_base import ApproachTargetBaseState -import approach_post import numpy as np -import math DIST_AHEAD = get_rosparam("long_range/distance_ahead", 20) @@ -68,6 +58,6 @@ def get_target_pos(self, context) -> Optional[int]: def determine_next(self, context, finished: bool) -> State: fid_pos = context.env.current_fid_pos() if fid_pos is None: - return state.LongRangeState() + return self else: return approach_post.ApproachPostState() diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py index 300d87c08..056ac5cfc 100755 --- a/src/navigation/navigation.py +++ b/src/navigation/navigation.py @@ -15,6 +15,7 @@ from navigation.state import DoneState, OffState, off_check from navigation.waypoint import WaypointState from navigation.approach_object import ApproachObjectState +from navigation.long_range import LongRangeState class Navigation(threading.Thread): @@ -32,10 +33,18 @@ def __init__(self, context: Context): self.state_machine.add_transitions(PostBackupState(), [WaypointState(), RecoveryState()]) self.state_machine.add_transitions( RecoveryState(), - [WaypointState(), SearchState(), PostBackupState(), ApproachPostState(), ApproachObjectState()], + [ + WaypointState(), + SearchState(), + PostBackupState(), + ApproachPostState(), + ApproachObjectState(), + LongRangeState(), + ], ) self.state_machine.add_transitions( - SearchState(), [ApproachPostState(), ApproachObjectState(), WaypointState(), RecoveryState()] + SearchState(), + [ApproachPostState(), ApproachObjectState(), LongRangeState(), WaypointState(), RecoveryState()], ) self.state_machine.add_transitions(DoneState(), [WaypointState()]) self.state_machine.add_transitions( @@ -44,12 +53,14 @@ def __init__(self, context: Context): PostBackupState(), ApproachPostState(), ApproachObjectState(), + LongRangeState(), SearchState(), RecoveryState(), DoneState(), ], ) self.state_machine.add_transitions(ApproachObjectState(), [DoneState(), SearchState()]) + self.state_machine.add_transitions(LongRangeState(), [ApproachPostState()]) self.state_machine.add_transitions(OffState(), [WaypointState(), DoneState()]) self.state_machine.configure_off_switch(OffState(), off_check) self.state_machine_server = StatePublisher(self.state_machine, "nav_structure", 1, "nav_state", 10) From dcae199e00e4e8413d9948209f5a0d496414116f Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Tue, 30 Jan 2024 16:16:42 -0500 Subject: [PATCH 142/197] fix image input to work in sim --- .../long_range_cam/long_range_tag_detector.cpp | 2 +- .../long_range_tag_detector.processing.cpp | 11 ++++++++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp index 45f72b9ed..ce825d003 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp @@ -23,7 +23,7 @@ namespace mrover { // topic: long_range_cam/image //todo - mImgSub = mNh.subscribe("long_range_cam/image", 1, &LongRangeTagDetectorNodelet::imageCallback, this); + mImgSub = mNh.subscribe("long_range_image", 1, &LongRangeTagDetectorNodelet::imageCallback, this); mDictionary = cv::makePtr(cv::aruco::getPredefinedDictionary(dictionaryNumber)); mServiceEnableDetections = mNh.advertiseService("enable_detections", &LongRangeTagDetectorNodelet::enableDetectionsCallback, this); diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp index e54a78150..6f76b94cc 100644 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp +++ b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp @@ -3,6 +3,8 @@ #include #include +#include + namespace mrover { /** @@ -25,7 +27,7 @@ namespace mrover { // 3. We only want to publish the tags if the topic has subscribers if (mPublishImages && mImgPub.getNumSubscribers()) { // Draw the tags on the image using OpenCV - //publishTagsOnImage(); + publishTagsOnImage(); } //Publish all tags that meet threshold @@ -40,8 +42,11 @@ namespace mrover { assert(msg); assert(msg->height > 0); assert(msg->width > 0); - //Store image message to mImgMsg member variable - cv::Mat cvImage = cv::Mat{static_cast(msg->height), static_cast(msg->width), CV_8UC3, const_cast(msg->data.data())}; + + // TODO: Modify when going back to real cam + cv::Mat cvImage_C4 = cv::Mat{static_cast(msg->height), static_cast(msg->width), CV_8UC4, const_cast(msg->data.data())}; + cv::Mat cvImage = cv::Mat(); + cv::cvtColor(cvImage_C4, cvImage, cv::COLOR_BGRA2BGR); //Store to mImg member variable - unsure if necessary but need it to persist cvImage.copyTo(mImg); From ada1a1f72dbced55b519a0b9d972c917cd799028 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 30 Jan 2024 17:53:32 -0500 Subject: [PATCH 143/197] Changes to zed Wrapper --- src/util/loop_profiler.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/util/loop_profiler.hpp b/src/util/loop_profiler.hpp index c549b9ece..0cb859615 100644 --- a/src/util/loop_profiler.hpp +++ b/src/util/loop_profiler.hpp @@ -33,6 +33,9 @@ class LoopProfiler { if (mTick % mPrintTick == 0) { Clock::duration averageLoopDuration{}; for (auto& [_, durations]: mEventReadings) { + if (durations.empty()) { + continue; + } averageLoopDuration += std::accumulate(durations.begin(), durations.end(), Clock::duration{}) / durations.size(); } // Print update time for the entire loop @@ -44,6 +47,9 @@ class LoopProfiler { << " (" << hz << " Hz)"); // Print update times for each loop event for (auto& [name, durations]: mEventReadings) { + if (durations.empty()) { + continue; + } Clock::duration averageEventDuration = std::accumulate(durations.begin(), durations.end(), Clock::duration{}) / durations.size(); auto averageEventMs = std::chrono::duration_cast(averageEventDuration); ROS_DEBUG_STREAM("\t" << name << ": " << averageEventMs.count() << "ms"); From d5da7aa6ec61448c119538454d6a8066b405f809 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Tue, 30 Jan 2024 19:56:25 -0500 Subject: [PATCH 144/197] merge new states --- CMakeLists.txt | 1 + config/DetectorParams.cfg | 92 +++++++++++++++++++++++++++++++++ config/TagDetectorParams.cfg | 2 +- config/simulator/simulator.yaml | 4 +- 4 files changed, 96 insertions(+), 3 deletions(-) create mode 100755 config/DetectorParams.cfg diff --git a/CMakeLists.txt b/CMakeLists.txt index c1e762a6b..9526099e9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,6 +62,7 @@ set(MROVER_MESSAGE_DEPENDENCIES ) set(MROVER_PARAMETERS + config/DetectorParams.cfg config/TagDetectorParams.cfg config/ObjectDetectorParams.cfg ) diff --git a/config/DetectorParams.cfg b/config/DetectorParams.cfg new file mode 100755 index 000000000..38165dc1e --- /dev/null +++ b/config/DetectorParams.cfg @@ -0,0 +1,92 @@ +#!/usr/bin/env python3 +PACKAGE = "mrover" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("adaptiveThreshConstant", double_t, 0, + "Constant for adaptive thresholding before finding contours", + 7, 0, 255) + +gen.add("adaptiveThreshWinSizeMin", int_t, 0, + "Minimum window size for adaptive thresholding before finding contours", + 3, 1, 100) + +gen.add("adaptiveThreshWinSizeMax", int_t, 0, + "Maximum window size for adaptive thresholding before finding contours", + 23, 1, 100) + +gen.add("adaptiveThreshWinSizeStep", int_t, 0, + "Increments from adaptiveThreshWinSizeMin to adaptiveThreshWinSizeMax during the thresholding", + 10, 1, 23) + +gen.add("cornerRefinementMaxIterations", int_t, 0, + "Maximum number of iterations for stop criteria of the corner refinement process", + 30, 1, 100) + +gen.add("cornerRefinementMinAccuracy", double_t, 0, + "Minimum error for the stop criteria of the corner refinement process", + 0.1, 0, 1) + +gen.add("cornerRefinementWinSize", int_t, 0, + "Window size for the corner refinement process (in pixels)", + 5, 1, 100) + +gen.add("doCornerRefinement", bool_t, 0, + "Whether to do subpixel corner refinement", + True) + +gen.add("cornerRefinementSubpix", bool_t, 0, + "Whether to do subpixel corner refinement (true) or contour (false)", + True) + +gen.add("errorCorrectionRate", double_t, 0, + "Error correction rate respect to the maximum error correction capability for each dictionary", + 0.6, 0, 1) + +gen.add("minCornerDistanceRate", double_t, 0, + "Minimum distance between corners for detected markers relative to its perimeter", + 0.05, 0, 1) + +gen.add("markerBorderBits", int_t, 0, + "Number of bits of the marker border, i.e. marker border width", + 1, 0, 100) + +gen.add("maxErroneousBitsInBorderRate", double_t, 0, + "Maximum number of accepted erroneous bits in the border (i.e. number of allowed white bits in the border)", + 0.35, 0, 1) + +gen.add("minDistanceToBorder", int_t, 0, + "Minimum distance of any corner to the image border for detected markers (in pixels)", + 3, 0, 100) + +gen.add("minMarkerDistanceRate", double_t, 0, + "minimum mean distance beetween two marker corners to be considered similar, so that the smaller one is removed. The rate is relative to the smaller perimeter of the two markers", + 0.05, 0, 1) + +gen.add("minMarkerPerimeterRate", double_t, 0, + "Determine minumum perimeter for marker contour to be detected. This is defined as a rate respect to the maximum dimension of the input image", + 0.03, 0, 1) + +gen.add("maxMarkerPerimeterRate", double_t, 0, + "Determine maximum perimeter for marker contour to be detected. This is defined as a rate respect to the maximum dimension of the input image", + 4.0, 0, 1) + +gen.add("minOtsuStdDev", double_t, 0, + "Minimun standard deviation in pixels values during the decodification step to apply Otsu thresholding (otherwise, all the bits are set to 0 or 1 depending on mean higher than 128 or not)", + 5.0, 0, 255) + +gen.add("perspectiveRemoveIgnoredMarginPerCell", double_t, 0, + "Width of the margin of pixels on each cell not considered for the determination of the cell bit. Represents the rate respect to the total size of the cell, i.e. perpectiveRemovePixelPerCell", + 0.13, 0, 1) + +gen.add("perspectiveRemovePixelPerCell", int_t, 0, + "Number of bits (per dimension) for each cell of the marker when removing the perspective", + 4, 1, 100) + +gen.add("polygonalApproxAccuracyRate", double_t, 0, + "Minimum accuracy during the polygonal approximation process to determine which contours are squares", + 0.08, 0, 1) + +exit(gen.generate(PACKAGE, "zed_tag_detector", "DetectorParams")) diff --git a/config/TagDetectorParams.cfg b/config/TagDetectorParams.cfg index 6c9692af7..390d6903a 100755 --- a/config/TagDetectorParams.cfg +++ b/config/TagDetectorParams.cfg @@ -89,4 +89,4 @@ gen.add("polygonalApproxAccuracyRate", double_t, 0, "Minimum accuracy during the polygonal approximation process to determine which contours are squares", 0.08, 0, 1) -exit(gen.generate(PACKAGE, "tag_detector", "TagDetectorParams")) +exit(gen.generate(PACKAGE, "long_range_tag_detector", "TagDetectorParams")) diff --git a/config/simulator/simulator.yaml b/config/simulator/simulator.yaml index 910c97198..79dc3c215 100644 --- a/config/simulator/simulator.yaml +++ b/config/simulator/simulator.yaml @@ -9,7 +9,7 @@ objects: - type: urdf name: tag_0 uri: package://mrover/urdf/world/tag_0.urdf.xacro - translation: [5, 0, 0.7] + translation: [5, 10, 0.7] - type: urdf name: tag_1 uri: package://mrover/urdf/world/tag_1.urdf.xacro @@ -17,7 +17,7 @@ objects: - type: urdf name: hammer uri: package://mrover/urdf/world/hammer.urdf.xacro - translation: [8, 7, 0.6] + translation: [5, 2, 0.7] - type: urdf name: bottle uri: package://mrover/urdf/world/bottle.urdf.xacro From 7e36572c353cc9ceaa88309258411796d4da2b58 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Thu, 1 Feb 2024 13:38:36 -0500 Subject: [PATCH 145/197] change hit counts + nav tf frame names --- src/navigation/context.py | 4 ++-- src/perception/object_detector/object_detector.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/navigation/context.py b/src/navigation/context.py index bc3fc19bf..3b67c54f4 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -111,9 +111,9 @@ def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray] if current_waypoint.type.val == WaypointType.POST: return self.get_target_pos(f"fiducial{current_waypoint.tag_id}", in_odom) elif current_waypoint.type.val == WaypointType.MALLET: - return self.get_target_pos("hammer", in_odom) + return self.get_target_pos("detectedObjectHammer", in_odom) elif current_waypoint.type == WaypointType.WATER_BOTTLE: - return self.get_target_pos("bottle", in_odom) + return self.get_target_pos("detectedObjectBottle", in_odom) else: # print("CURRENT WAYPOINT IS NOT A POST OR OBJECT") return None diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 29e28f567..d274fe338 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -25,8 +25,8 @@ namespace mrover { //Read ROS Params mNh.param("obj_increment_weight", mObjIncrementWeight, 2); mNh.param("obj_decrement_weight", mObjDecrementWeight, 1); - mNh.param("obj_hitcount_threshold", mObjHitThreshold, 50); - mNh.param("obj_hitcount_max", mObjMaxHitcount, 60); + mNh.param("obj_hitcount_threshold", mObjHitThreshold, 5); + mNh.param("obj_hitcount_max", mObjMaxHitcount, 10); //Initialize Object Hit Cout to Zeros mHitCountHammer = 0; From 60778760460c2d8fb33e5a33605cada29e2f49b8 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 1 Feb 2024 20:03:54 -0500 Subject: [PATCH 146/197] Pre Refactoring --- scripts/debug_course_publisher.py | 12 +- src/perception/object_detector/inference.cu | 32 ++--- src/perception/object_detector/inference.cuh | 24 +++- .../object_detector.processing.cpp | 131 +++++++++--------- 4 files changed, 100 insertions(+), 99 deletions(-) diff --git a/scripts/debug_course_publisher.py b/scripts/debug_course_publisher.py index 5f1468ba0..c9d2314de 100755 --- a/scripts/debug_course_publisher.py +++ b/scripts/debug_course_publisher.py @@ -20,15 +20,15 @@ [ convert_waypoint_to_gps(waypoint) for waypoint in [ - # ( - # Waypoint(tag_id=0, type=WaypointType(val=WaypointType.MALLET)), - # SE3(position=np.array([8, 2, 0])), - # ) ( - Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)), - SE3(position=np.array([3, 0, 0])), + Waypoint(tag_id=0, type=WaypointType(val=WaypointType.MALLET)), + SE3(position=np.array([4, 2, 0])), ) # ( + # Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)), + # SE3(position=np.array([3, 0, 0])), + # ) + # ( # Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)), # SE3(position=np.array([11, -10, 0])), # ) diff --git a/src/perception/object_detector/inference.cu b/src/perception/object_detector/inference.cu index 921136110..a41052f3d 100644 --- a/src/perception/object_detector/inference.cu +++ b/src/perception/object_detector/inference.cu @@ -34,8 +34,7 @@ namespace mrover { if (mEngine->getTensorIOMode(INPUT_BINDING_NAME) != TensorIOMode::kINPUT) throw std::runtime_error("Expected Input Binding 0 Is An Input"); if (mEngine->getTensorIOMode(OUTPUT_BINDING_NAME) != TensorIOMode::kOUTPUT) throw std::runtime_error("Expected Input Binding Input To Be 1"); - //Create the execution context - setUpContext(); + createExecutionContext(); //Init the io tensors on the GPU prepTensors(); @@ -78,17 +77,7 @@ namespace mrover { std::filesystem::path enginePath = packagePath / "data" / "tensorrt-engine-yolov8n_mallet_bottle_better.engine"; //Check if engine file exists - if (exists(enginePath)) { - //Load engine from file - std::ifstream inputFileStream(enginePath, std::ios::binary); - std::stringstream engineBuffer; - - //Stream in the engine file to the buffer - engineBuffer << inputFileStream.rdbuf(); - std::string enginePlan = engineBuffer.str(); - //Deserialize the Cuda Engine file from the buffer - return runtime->deserializeCudaEngine(enginePlan.data(), enginePlan.size()); - } else { + if (!exists(enginePath)) { ROS_WARN_STREAM("Optimizing ONXX model for TensorRT. This make take a long time..."); //Create the Engine from onnx file @@ -107,9 +96,19 @@ namespace mrover { return tempEng; } + + //Load engine from file + std::ifstream inputFileStream(enginePath, std::ios::binary); + std::stringstream engineBuffer; + + //Stream in the engine file to the buffer + engineBuffer << inputFileStream.rdbuf(); + std::string enginePlan = engineBuffer.str(); + //Deserialize the Cuda Engine file from the buffer + return runtime->deserializeCudaEngine(enginePlan.data(), enginePlan.size()); } - void Inference::setUpContext() { + void Inference::createExecutionContext() { // Create Execution Context. mContext.reset(mEngine->createExecutionContext()); @@ -150,11 +149,6 @@ namespace mrover { } - /** -* Takes tensor bindings and allocates memory on the GPU for input and output tensors -* Requires enginePtr, bindings, inputTensor, and outputTensor -* Modifies bindings, inputTensor, and outputTensor -*/ void Inference::prepTensors() { //Assign the properties to the input and output tensors for (int i = 0; i < mEngine->getNbIOTensors(); i++) { diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index f7c113488..17b5dacd3 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -13,6 +13,7 @@ using nvinfer1::IExecutionContext; namespace mrover { class Inference { + private: //Init Logger nvinfer1::Logger mLogger; @@ -34,29 +35,38 @@ namespace mrover { cv::Size mModelOutputShape; //STATIC FUNCTIONS - static int getBindingInputIndex(const IExecutionContext* context); + static int getBindingInputIndex(IExecutionContext const* context); - private: //Creates a ptr to the engine ICudaEngine* createCudaEngine(std::filesystem::path const& onnxModelPath); //Launch the model execution onto the GPU void launchInference(cv::Mat const& input, cv::Mat const& output) const; - //Init the tensors + /** + * @brief Prepares the tensors for inference. + * + * Takes tensor bindings and allocates memory on the GPU for input and output tensors + * Requires enginePtr, bindings, inputTensor, and outputTensor + * + * Requires enginePtr, bindings, inputTensor, and outputTensor + * Modifies bindings, inputTensor, and outputTensor + */ void prepTensors(); - //Init the execution context - void setUpContext(); + /** + * @brief Creates the execution context for the model + */ + void createExecutionContext(); public: //Inference Constructor Inference(std::filesystem::path const& onnxModelPath); //Forward Pass of the model - void doDetections(const cv::Mat& img) const; + void doDetections(cv::Mat const& img) const; - //Get the output tensor with in a YOLO v8 style data structure + //Get the output tensor with in a YOLO v8 style data structure cv::Mat getOutputTensor(); }; diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index dc4efad6d..8cb3ab399 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -3,6 +3,7 @@ #include "../point.hpp" #include "inference_wrapper.hpp" #include +#include #include #include #include @@ -24,7 +25,10 @@ namespace mrover { mImg = cv::Mat{static_cast(msg->height), static_cast(msg->width), CV_8UC4, cv::Scalar{0, 0, 0, 0}}; } - //Convert the pointcloud data into rgba image + //TODO - BREAKOUT + //auto pixelPtr = convertPointCloudToRGBA(msg); + + //Convert the pointcloud data into rgba image and store in mImg auto* pixelPtr = reinterpret_cast(mImg.data); auto* pointPtr = reinterpret_cast(msg->data.data()); std::for_each(std::execution::par_unseq, pixelPtr, pixelPtr + mImg.total(), [&](cv::Vec4b& pixel) { @@ -35,13 +39,15 @@ namespace mrover { pixel[3] = pointPtr[i].a; }); + //Resize the image and change it from BGRA to BGR cv::Mat sizedImage; - cv::resize(mImg, sizedImage, cv::Size(640, 640)); + cv::Size imgSize{640, 640}; + cv::resize(mImg, sizedImage, imgSize); cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGRA2BGR); //Create the blob from the resized image - cv::dnn::blobFromImage(sizedImage, mImageBlob, 1.0 / 255.0, cv::Size{640, 640}, cv::Scalar(), true, false); + cv::dnn::blobFromImage(sizedImage, mImageBlob, 1.0 / 255.0, imgSize, cv::Scalar(), true, false); //Run the blob through the model mInferenceWrapper.doDetections(mImageBlob); @@ -52,19 +58,19 @@ namespace mrover { //Get model specific information int rows = output.rows; int dimensions = output.cols; - bool yolov8 = false; // yolov5 has an output of shape (batchSize, 25200, 85) (Num classes + box[x,y,w,h] + confidence[c]) // yolov8 has an output of shape (batchSize, 84, 8400) (Num classes + box[x,y,w,h]) - if (dimensions > rows) // Check if the shape[2] is more than shape[1] (yolov8) + if (dimensions <= rows) // Check if the shape[2] is more than shape[1] (yolov8) { - yolov8 = true; - rows = output.cols; - dimensions = output.rows; - - output = output.reshape(1, dimensions); - cv::transpose(output, output); + throw std::runtime_error("Something is wrong Model with interpretation"); } + + rows = output.cols; + dimensions = output.rows; + + output = output.reshape(1, dimensions); + cv::transpose(output, output); //Reinterpret data from the output to be in a usable form auto data = reinterpret_cast(output.data); @@ -89,42 +95,37 @@ namespace mrover { //Each of the possibilities do interpret the data for (int i = 0; i < rows; ++i) { - //This should always be true - if (yolov8) { - //This is because the first 4 points are box[x,y,w,h] - float* classes_scores = data + 4; - - //Create a mat to store all of the class scores - cv::Mat scores(1, static_cast(classes.size()), CV_32FC1, classes_scores); - cv::Point class_id; - double maxClassScore; - - //Find the max class score for the associated row - minMaxLoc(scores, nullptr, &maxClassScore, nullptr, &class_id); - - //Determine if the class is an acceptable confidence level else disregard - if (maxClassScore > modelScoreThreshold) { - //Push back data points into storage containers - confidences.push_back(static_cast(maxClassScore)); - class_ids.push_back(class_id.x); - - //Get the bounding box data - float x = data[0]; - float y = data[1]; - float w = data[2]; - float h = data[3]; - - //Cast the corners into integers to be used on pixels - int left = static_cast((x - 0.5 * w) * x_factor); - int top = static_cast((y - 0.5 * h) * y_factor); - int width = static_cast(w * x_factor); - int height = static_cast(h * y_factor); - - //Push abck the box into storage - boxes.emplace_back(left, top, width, height); - } - } else { //YOLO V5 - throw std::runtime_error("Something is wrong Model with interpretation"); + //This is because the first 4 points are box[x,y,w,h] + float* classes_scores = data + 4; + + //Create a mat to store all of the class scores + cv::Mat scores(1, static_cast(classes.size()), CV_32FC1, classes_scores); + cv::Point class_id; + double maxClassScore; + + //Find the max class score for the associated row + minMaxLoc(scores, nullptr, &maxClassScore, nullptr, &class_id); + + //Determine if the class is an acceptable confidence level else disregard + if (maxClassScore > modelScoreThreshold) { + //Push back data points into storage containers + confidences.push_back(static_cast(maxClassScore)); + class_ids.push_back(class_id.x); + + //Get the bounding box data + float x = data[0]; + float y = data[1]; + float w = data[2]; + float h = data[3]; + + //Cast the corners into integers to be used on pixels + int left = static_cast((x - 0.5 * w) * x_factor); + int top = static_cast((y - 0.5 * h) * y_factor); + int width = static_cast(w * x_factor); + int height = static_cast(h * y_factor); + + //Push abck the box into storage + boxes.emplace_back(left, top, width, height); } data += dimensions; @@ -156,18 +157,19 @@ namespace mrover { bool seenWaterBottle = false; bool seenHammer = false; std::pair center; - ROS_INFO("NUM DETECTIONS: %d", detections.size()); - for (Detection detection: detections) { - - Detection firstDetection = detection; - - if (firstDetection.class_id == 1 && !seenHammer) { + ROS_DEBUG("NUM DETECTIONS: %zu", detections.size()); + for (Detection const& detection: detections) { + cv::Rect box = detection.box; + center = std::pair(box.x + box.width / 2, box.y + box.height / 2); + auto centerWidth = static_cast(center.first * static_cast(msg->width) / sizedImage.cols); + auto centerHeight = static_cast(center.second * static_cast(msg->height) / sizedImage.rows); + if (detection.class_id == 1 && !seenHammer) { seenHammer = true; - cv::Rect box = firstDetection.box; - center = std::pair(box.x + box.width / 2, box.y + box.height / 2); + //TODO: BREAKOUT + //TODO: Refactor put immediates in the zed frame and finals in the map frame //Get the object's position in 3D from the point cloud and run this statement if the optional has a value - if (std::optional objectLocation = getObjectInCamFromPixel(msg, center.first * static_cast(msg->width) / sizedImage.cols, center.second * static_cast(msg->height) / sizedImage.rows, box.width, box.height); objectLocation) { + if (std::optional objectLocation = getObjectInCamFromPixel(msg, centerWidth, centerHeight, box.width, box.height); objectLocation) { try { //Publish Immediate std::string immediateFrameId = "immediateDetectedObjectHammer"; @@ -195,13 +197,13 @@ namespace mrover { } } - else if (firstDetection.class_id == 0 && !seenWaterBottle) { + else if (detection.class_id == 0 && !seenWaterBottle) { seenWaterBottle = true; - cv::Rect box = firstDetection.box; + cv::Rect box = detection.box; center = std::pair(box.x + box.width / 2, box.y + box.height / 2); //Get the object's position in 3D from the point cloud and run this statement if the optional has a value - if (std::optional objectLocation = getObjectInCamFromPixel(msg, center.first * static_cast(msg->width) / sizedImage.cols, center.second * static_cast(msg->height) / sizedImage.rows, box.width, box.height); objectLocation) { + if (std::optional objectLocation = getObjectInCamFromPixel(msg, centerWidth, centerHeight, box.width, box.height); objectLocation) { try { //Publish Immediate std::string immediateFrameId = "immediateDetectedObjectBottle"; @@ -237,7 +239,7 @@ namespace mrover { mHitCountBottle = std::max(0, mHitCountBottle - mObjDecrementWeight); } - + //TODO: CHange color //Draw the detected object's bounding boxes on the image for each of the objects detected for (size_t i = 0; i < detections.size(); i++) { std::cout << detections[i].className @@ -256,14 +258,9 @@ namespace mrover { } } - - // else { - // mHitCount = std::max(0, mHitCount - mObjDecrementWeight); - - // } - + //TODO - Breakout //We only want to publish the debug image if there is something lsitening, to reduce the operations going on - if (mDebugImgPub.getNumSubscribers() > 0 || true) { + if (mDebugImgPub.getNumSubscribers() > 0) { sensor_msgs::Image newDebugImageMessage; //I chose regular msg not ptr so it can be used outside of this process //Convert the image back to BGRA for ROS From 249bec06b21513257ec099616bcbe9cba9bffe81 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 2 Feb 2024 13:03:45 -0500 Subject: [PATCH 147/197] Some minor refactoring --- .../object_detector/object_detector.cpp | 3 +- .../object_detector/object_detector.hpp | 7 +- .../object_detector.processing.cpp | 162 ++++++++---------- 3 files changed, 74 insertions(+), 98 deletions(-) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index d274fe338..03de4d439 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -29,7 +29,6 @@ namespace mrover { mNh.param("obj_hitcount_max", mObjMaxHitcount, 10); //Initialize Object Hit Cout to Zeros - mHitCountHammer = 0; - mHitCountBottle = 0; + mObjectHitCounts = {0, 0}; } } // namespace mrover diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index f4641311e..b31612c45 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -55,8 +55,7 @@ namespace mrover { std::string mMapFrameId; //Hit counter - int mHitCountHammer; - int mHitCountBottle; + std::vector mObjectHitCounts; //Constants after initialization int mObjIncrementWeight; @@ -72,6 +71,10 @@ namespace mrover { std::optional spiralSearchInImg(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t xCenter, size_t yCenter, size_t width, size_t height); + void convertPointCloudToRGBA(sensor_msgs::PointCloud2ConstPtr const& msg, cv::Mat& img); + + void updateHitsObject(sensor_msgs::PointCloud2ConstPtr const& msg, Detection const& detection, std::vector& seenObjects, cv::Size const& imgSize = {640, 640}); + public: //Node constructor ObjectDetectorNodelet() = default; diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 8cb3ab399..d64557976 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -25,19 +25,9 @@ namespace mrover { mImg = cv::Mat{static_cast(msg->height), static_cast(msg->width), CV_8UC4, cv::Scalar{0, 0, 0, 0}}; } - //TODO - BREAKOUT - //auto pixelPtr = convertPointCloudToRGBA(msg); - + //DONE //Convert the pointcloud data into rgba image and store in mImg - auto* pixelPtr = reinterpret_cast(mImg.data); - auto* pointPtr = reinterpret_cast(msg->data.data()); - std::for_each(std::execution::par_unseq, pixelPtr, pixelPtr + mImg.total(), [&](cv::Vec4b& pixel) { - size_t const i = &pixel - pixelPtr; - pixel[0] = pointPtr[i].b; - pixel[1] = pointPtr[i].g; - pixel[2] = pointPtr[i].r; - pixel[3] = pointPtr[i].a; - }); + convertPointCloudToRGBA(msg, mImg); //Resize the image and change it from BGRA to BGR @@ -153,91 +143,20 @@ namespace mrover { detections.push_back(result); } + std::vector seenObjects = {false, false}; //If there are detections locate them in 3D - bool seenWaterBottle = false; - bool seenHammer = false; - std::pair center; - ROS_DEBUG("NUM DETECTIONS: %zu", detections.size()); for (Detection const& detection: detections) { - cv::Rect box = detection.box; - center = std::pair(box.x + box.width / 2, box.y + box.height / 2); - auto centerWidth = static_cast(center.first * static_cast(msg->width) / sizedImage.cols); - auto centerHeight = static_cast(center.second * static_cast(msg->height) / sizedImage.rows); - if (detection.class_id == 1 && !seenHammer) { - seenHammer = true; - - //TODO: BREAKOUT - //TODO: Refactor put immediates in the zed frame and finals in the map frame - //Get the object's position in 3D from the point cloud and run this statement if the optional has a value - if (std::optional objectLocation = getObjectInCamFromPixel(msg, centerWidth, centerHeight, box.width, box.height); objectLocation) { - try { - //Publish Immediate - std::string immediateFrameId = "immediateDetectedObjectHammer"; - SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, objectLocation.value()); - - //Since the object is seen we need to increment the hit counter - mHitCountHammer = std::min(mObjMaxHitcount, mHitCountHammer + mObjIncrementWeight); - - //Only publish to permament if we are confident in the object - if (mHitCountHammer > mObjHitThreshold) { - std::string permanentFrameId = "detectedObjectHammer"; - SE3::pushToTfTree(mTfBroadcaster, permanentFrameId, mCameraFrameId, objectLocation.value()); - } - - SE3 tagInsideCamera = SE3::fromTfTree(mTfBuffer, mMapFrameId, immediateFrameId); - } catch (tf2::ExtrapolationException const&) { - NODELET_WARN("Old data for immediate tag"); - } catch (tf2::LookupException const&) { - NODELET_WARN("Expected transform for immediate tag"); - } catch (tf::ConnectivityException const&) { - NODELET_WARN("Expected connection to odom frame. Is visual odometry running?"); - } catch (tf::LookupException const&) { - NODELET_WARN("LOOK UP NOT FOUND"); - } - } - } - else if (detection.class_id == 0 && !seenWaterBottle) { - seenWaterBottle = true; - cv::Rect box = detection.box; - center = std::pair(box.x + box.width / 2, box.y + box.height / 2); - - //Get the object's position in 3D from the point cloud and run this statement if the optional has a value - if (std::optional objectLocation = getObjectInCamFromPixel(msg, centerWidth, centerHeight, box.width, box.height); objectLocation) { - try { - //Publish Immediate - std::string immediateFrameId = "immediateDetectedObjectBottle"; - SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, objectLocation.value()); - - //Since the object is seen we need to increment the hit counter - mHitCountBottle = std::min(mObjMaxHitcount, mHitCountBottle + mObjIncrementWeight); - - //Only publish to permament if we are confident in the object - if (mHitCountBottle > mObjHitThreshold) { - std::string permanentFrameId = "detectedObjectBottle"; - SE3::pushToTfTree(mTfBroadcaster, permanentFrameId, mCameraFrameId, objectLocation.value()); - } - - SE3 tagInsideCamera = SE3::fromTfTree(mTfBuffer, mMapFrameId, immediateFrameId); - } catch (tf2::ExtrapolationException const&) { - NODELET_WARN("Old data for immediate tag"); - } catch (tf2::LookupException const&) { - NODELET_WARN("Expected transform for immediate tag"); - } catch (tf::ConnectivityException const&) { - NODELET_WARN("Expected connection to odom frame. Is visual odometry running?"); - } catch (tf::LookupException const&) { - NODELET_WARN("LOOK UP NOT FOUND"); - } - } - } + //Increment Object hit counts if theyre seen + updateHitsObject(msg, detection, seenObjects); - if (!seenHammer) { - mHitCountHammer = std::max(0, mHitCountHammer - mObjDecrementWeight); + //Decrement Object hit counts if they're not seen + for (size_t i = 0; i < seenObjects.size(); i++) { + if (!seenObjects.at(i)) { + mObjectHitCounts.at(i) = std::max(0, mObjectHitCounts.at(i) - mObjDecrementWeight); + } } - if (!seenWaterBottle) { - mHitCountBottle = std::max(0, mHitCountBottle - mObjDecrementWeight); - } //TODO: CHange color //Draw the detected object's bounding boxes on the image for each of the objects detected @@ -245,9 +164,7 @@ namespace mrover { std::cout << detections[i].className << i << std::endl; - cv::Rect tempRect(center.first, center.second, 10, 10); cv::rectangle(sizedImage, detections[i].box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); - cv::rectangle(sizedImage, tempRect, cv::Scalar(0, 0, 0), 3, cv::LINE_8, 0); //Put the text on the image cv::Point text_position(80, static_cast(80 * (i + 1))); @@ -283,7 +200,7 @@ namespace mrover { mDebugImgPub.publish(newDebugImageMessage); } - } + } // namespace mrover std::optional ObjectDetectorNodelet::getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v, size_t width, size_t height) { assert(cloudPtr); @@ -335,4 +252,61 @@ namespace mrover { return std::make_optional(R3{point.x, point.y, point.z}, SO3{}); } + + void ObjectDetectorNodelet::convertPointCloudToRGBA(sensor_msgs::PointCloud2ConstPtr const& msg, cv::Mat& img) { + auto* pixelPtr = reinterpret_cast(img.data); + auto* pointPtr = reinterpret_cast(msg->data.data()); + std::for_each(std::execution::par_unseq, pixelPtr, pixelPtr + img.total(), [&](cv::Vec4b& pixel) { + size_t const i = &pixel - pixelPtr; + pixel[0] = pointPtr[i].b; + pixel[1] = pointPtr[i].g; + pixel[2] = pointPtr[i].r; + pixel[3] = pointPtr[i].a; + }); + } + + void ObjectDetectorNodelet::updateHitsObject(sensor_msgs::PointCloud2ConstPtr const& msg, Detection const& detection, std::vector& seenObjects, cv::Size const& imgSize) { + + cv::Rect box = detection.box; + std::pair center = std::pair(box.x + box.width / 2, box.y + box.height / 2); + //Resize from {640, 640} image space to {720,1280} image space + auto centerWidth = static_cast(center.first * static_cast(msg->width) / imgSize.width); + auto centerHeight = static_cast(center.second * static_cast(msg->height) / imgSize.height); + + if (seenObjects.at(detection.class_id)) { + seenObjects.at(detection.class_id) = true; + + //Get the object's position in 3D from the point cloud and run this statement if the optional has a value + if (std::optional objectLocation = getObjectInCamFromPixel(msg, centerWidth, centerHeight, box.width, box.height); objectLocation) { + try { + std::string immediateFrameId = "immediateDetectedObject" + classes.at(detection.class_id); + + //Push the immediate detections to the zed frame + SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, objectLocation.value()); + + //Since the object is seen we need to increment the hit counter + mObjectHitCounts.at(detection.class_id) = std::min(mObjMaxHitcount, mObjectHitCounts.at(detection.class_id) + mObjIncrementWeight); + + //Only publish to permament if we are confident in the object + if (mObjectHitCounts.at(detection.class_id) > mObjHitThreshold) { + + std::string permanentFrameId = "detectedObject" + classes.at(detection.class_id); + + //Grab the object inside of the camera frame and push it into the map frame + SE3 objectInsideCamera = SE3::fromTfTree(mTfBuffer, mMapFrameId, immediateFrameId); + SE3::pushToTfTree(mTfBroadcaster, permanentFrameId, mCameraFrameId, objectInsideCamera); + } + + } catch (tf2::ExtrapolationException const&) { + NODELET_WARN("Old data for immediate tag"); + } catch (tf2::LookupException const&) { + NODELET_WARN("Expected transform for immediate tag"); + } catch (tf::ConnectivityException const&) { + NODELET_WARN("Expected connection to odom frame. Is visual odometry running?"); + } catch (tf::LookupException const&) { + NODELET_WARN("LOOK UP NOT FOUND"); + } + } + } + } } // namespace mrover From beb7b9d7f45a6f2af68443014fdf04f816f1ba45 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 2 Feb 2024 15:52:41 -0500 Subject: [PATCH 148/197] refactoring --- .../object_detector/object_detector.hpp | 4 +- .../object_detector.processing.cpp | 79 ++++++++++--------- 2 files changed, 43 insertions(+), 40 deletions(-) diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index b31612c45..aaf94a26c 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -71,10 +71,12 @@ namespace mrover { std::optional spiralSearchInImg(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t xCenter, size_t yCenter, size_t width, size_t height); - void convertPointCloudToRGBA(sensor_msgs::PointCloud2ConstPtr const& msg, cv::Mat& img); + static void convertPointCloudToRGBA(sensor_msgs::PointCloud2ConstPtr const& msg, cv::Mat& img); void updateHitsObject(sensor_msgs::PointCloud2ConstPtr const& msg, Detection const& detection, std::vector& seenObjects, cv::Size const& imgSize = {640, 640}); + void publishImg(cv::Mat const& img); + public: //Node constructor ObjectDetectorNodelet() = default; diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index d64557976..8cff27b3f 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -3,8 +3,8 @@ #include "../point.hpp" #include "inference_wrapper.hpp" #include +#include #include -#include #include #include #include @@ -65,10 +65,10 @@ namespace mrover { auto data = reinterpret_cast(output.data); //Model Information - float modelInputCols = 640; - float modelInputRows = 640; - float modelShapeWidth = 640; - float modelShapeHeight = 640; + auto modelInputCols = static_cast(imgSize.width); + auto modelInputRows = static_cast(imgSize.height); + auto modelShapeWidth = static_cast(imgSize.width); + auto modelShapeHeight = static_cast(imgSize.height); //Set model thresholds float modelScoreThreshold = 0.75; @@ -157,52 +157,28 @@ namespace mrover { } } - - //TODO: CHange color //Draw the detected object's bounding boxes on the image for each of the objects detected for (size_t i = 0; i < detections.size(); i++) { - std::cout << detections[i].className - << i << std::endl; - - cv::rectangle(sizedImage, detections[i].box, cv::Scalar(0, 0, 0), 1, cv::LINE_8, 0); + //Font color will change for each different detection + cv::Scalar font_Color(static_cast(i * 50), static_cast(i * 50), static_cast(i * 50)); + cv::rectangle(sizedImage, detections[i].box, font_Color, 1, cv::LINE_8, 0); //Put the text on the image cv::Point text_position(80, static_cast(80 * (i + 1))); int font_size = 1; - cv::Scalar font_Color(0, 0, 0); int font_weight = 2; putText(sizedImage, detections[i].className, text_position, cv::FONT_HERSHEY_COMPLEX, font_size, font_Color, font_weight); //Putting the text in the matrix// } } - //TODO - Breakout //We only want to publish the debug image if there is something lsitening, to reduce the operations going on if (mDebugImgPub.getNumSubscribers() > 0) { - sensor_msgs::Image newDebugImageMessage; //I chose regular msg not ptr so it can be used outside of this process - - //Convert the image back to BGRA for ROS - cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGR2BGRA); - - newDebugImageMessage.height = sizedImage.rows; - newDebugImageMessage.width = sizedImage.cols; - newDebugImageMessage.encoding = sensor_msgs::image_encodings::BGRA8; - newDebugImageMessage.step = sizedImage.channels() * sizedImage.cols; - newDebugImageMessage.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; - - auto imgPtr = sizedImage.data; - - //Calculate the image size - size_t size = newDebugImageMessage.step * newDebugImageMessage.height; - newDebugImageMessage.data.resize(size); - - //Copy the data to the image - memcpy(newDebugImageMessage.data.data(), imgPtr, size); - - mDebugImgPub.publish(newDebugImageMessage); + //Publishes the image to the debug publisher + publishImg(sizedImage); } } // namespace mrover - std::optional ObjectDetectorNodelet::getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v, size_t width, size_t height) { + auto ObjectDetectorNodelet::getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v, size_t width, size_t height) -> std::optional { assert(cloudPtr); if (u >= cloudPtr->width || v >= cloudPtr->height) { NODELET_WARN("Tag center out of bounds: [%zu %zu]", u, v); @@ -213,14 +189,14 @@ namespace mrover { return spiralSearchInImg(cloudPtr, u, v, width, height); } - std::optional ObjectDetectorNodelet::spiralSearchInImg(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t xCenter, size_t yCenter, size_t width, size_t height) { + auto ObjectDetectorNodelet::spiralSearchInImg(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t xCenter, size_t yCenter, size_t width, size_t height) -> std::optional { size_t currX = xCenter; size_t currY = yCenter; size_t radius = 0; int t = 0; int numPts = 16; bool isPointInvalid = true; - Point point; + Point point{}; //Find the smaller of the two box dimensions so we know the max spiral radius size_t smallDim = std::min(width / 2, height / 2); @@ -228,8 +204,8 @@ namespace mrover { while (isPointInvalid) { //This is the parametric equation to spiral around the center pnt - currX = xCenter + std::cos(t * 1.0 / numPts * 2 * M_PI) * radius; - currY = yCenter + std::sin(t * 1.0 / numPts * 2 * M_PI) * radius; + currX = static_cast(static_cast(xCenter) + std::cos(t * 1.0 / numPts * 2 * M_PI) * static_cast(radius)); + currY = static_cast(static_cast(yCenter) + std::sin(t * 1.0 / numPts * 2 * M_PI) * static_cast(radius)); //Grab the point from the pntCloud and determine if its a finite pnt point = reinterpret_cast(cloudPtr->data.data())[currX + currY * cloudPtr->width]; @@ -309,4 +285,29 @@ namespace mrover { } } } + + void ObjectDetectorNodelet::publishImg(cv::Mat const& img) { + sensor_msgs::Image newDebugImageMessage; //I chose regular msg not ptr so it can be used outside of this process + + //Convert the image back to BGRA for ROS + cv::Mat bgraImg; + cv::cvtColor(img, bgraImg, cv::COLOR_BGR2BGRA); + + newDebugImageMessage.height = bgraImg.rows; + newDebugImageMessage.width = bgraImg.cols; + newDebugImageMessage.encoding = sensor_msgs::image_encodings::BGRA8; + newDebugImageMessage.step = bgraImg.channels() * bgraImg.cols; + newDebugImageMessage.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; + + auto imgPtr = bgraImg.data; + + //Calculate the image size + size_t size = newDebugImageMessage.step * newDebugImageMessage.height; + newDebugImageMessage.data.resize(size); + + //Copy the data to the image + memcpy(newDebugImageMessage.data.data(), imgPtr, size); + + mDebugImgPub.publish(newDebugImageMessage); + } } // namespace mrover From 2e642cbfd0d8137344d53c02669287754e625d73 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 2 Feb 2024 15:54:09 -0500 Subject: [PATCH 149/197] refactoring --- src/perception/object_detector/object_detector.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index aaf94a26c..cded7fcaa 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -67,9 +67,9 @@ namespace mrover { void onInit() override; //Function to get SE3 from the point cloud - std::optional getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v, size_t width, size_t height); + auto getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v, size_t width, size_t height) -> std::optional; - std::optional spiralSearchInImg(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t xCenter, size_t yCenter, size_t width, size_t height); + auto spiralSearchInImg(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t xCenter, size_t yCenter, size_t width, size_t height) -> std::optional; static void convertPointCloudToRGBA(sensor_msgs::PointCloud2ConstPtr const& msg, cv::Mat& img); From 47428afa31fa8a62df225d20a1a42edc8578ed44 Mon Sep 17 00:00:00 2001 From: John Date: Sat, 3 Feb 2024 13:18:45 -0500 Subject: [PATCH 150/197] Removed Refactoring Bugs --- src/perception/object_detector/inference.cu | 7 ------- src/perception/object_detector/inference.cuh | 12 +++++++----- src/perception/object_detector/logger.cu | 1 - src/perception/object_detector/logger.cuh | 1 + src/perception/object_detector/main.cpp | 2 +- .../object_detector/object_detector.processing.cpp | 13 +++++++++---- 6 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/perception/object_detector/inference.cu b/src/perception/object_detector/inference.cu index a41052f3d..b101616fd 100644 --- a/src/perception/object_detector/inference.cu +++ b/src/perception/object_detector/inference.cu @@ -1,12 +1,5 @@ #include "inference.cuh" -#include -#include -#include -#include -#include - -#include "logger.cuh" using namespace nvinfer1; /** diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index 17b5dacd3..ef476a807 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -1,11 +1,13 @@ -// Cpp native #pragma once +#include "logger.cuh" #include "pch.hpp" #include - -#include "logger.cuh" +#include +#include +#include +#include using nvinfer1::ICudaEngine; using nvinfer1::IExecutionContext; @@ -35,10 +37,10 @@ namespace mrover { cv::Size mModelOutputShape; //STATIC FUNCTIONS - static int getBindingInputIndex(IExecutionContext const* context); + static auto getBindingInputIndex(IExecutionContext const* context) -> int; //Creates a ptr to the engine - ICudaEngine* createCudaEngine(std::filesystem::path const& onnxModelPath); + auto createCudaEngine(std::filesystem::path const& onnxModelPath) -> ICudaEngine*; //Launch the model execution onto the GPU void launchInference(cv::Mat const& input, cv::Mat const& output) const; diff --git a/src/perception/object_detector/logger.cu b/src/perception/object_detector/logger.cu index 2abd2ebf8..622bb8786 100644 --- a/src/perception/object_detector/logger.cu +++ b/src/perception/object_detector/logger.cu @@ -1,5 +1,4 @@ #include "logger.cuh" -#include "pch.hpp" namespace nvinfer1 { diff --git a/src/perception/object_detector/logger.cuh b/src/perception/object_detector/logger.cuh index 02bf62d1d..7467a8000 100644 --- a/src/perception/object_detector/logger.cuh +++ b/src/perception/object_detector/logger.cuh @@ -1,5 +1,6 @@ #pragma once +#include "pch.hpp" #include namespace nvinfer1 { diff --git a/src/perception/object_detector/main.cpp b/src/perception/object_detector/main.cpp index 450d16523..729f6caf2 100644 --- a/src/perception/object_detector/main.cpp +++ b/src/perception/object_detector/main.cpp @@ -1,6 +1,6 @@ #include "object_detector.hpp" -int main(int argc, char** argv) { +auto main(int argc, char** argv) -> int { ros::init(argc, argv, "object_detector"); // Start the ZED Nodelet diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 8cff27b3f..a27f9a44e 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -71,7 +71,7 @@ namespace mrover { auto modelShapeHeight = static_cast(imgSize.height); //Set model thresholds - float modelScoreThreshold = 0.75; + float modelScoreThreshold = 0.50; float modelNMSThreshold = 0.50; //Get x and y scale factors @@ -172,7 +172,7 @@ namespace mrover { } //We only want to publish the debug image if there is something lsitening, to reduce the operations going on - if (mDebugImgPub.getNumSubscribers() > 0) { + if (mDebugImgPub.getNumSubscribers() > 0 || true) { //Publishes the image to the debug publisher publishImg(sizedImage); } @@ -249,7 +249,10 @@ namespace mrover { auto centerWidth = static_cast(center.first * static_cast(msg->width) / imgSize.width); auto centerHeight = static_cast(center.second * static_cast(msg->height) / imgSize.height); - if (seenObjects.at(detection.class_id)) { + std::cout << mObjectHitCounts.at(0) << ", " << mObjectHitCounts.at(1) << std::endl; + ROS_INFO("%d, %d", mObjectHitCounts.at(0), mObjectHitCounts.at(1)); + + if (!seenObjects.at(detection.class_id)) { seenObjects.at(detection.class_id) = true; //Get the object's position in 3D from the point cloud and run this statement if the optional has a value @@ -260,14 +263,16 @@ namespace mrover { //Push the immediate detections to the zed frame SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, objectLocation.value()); + //Since the object is seen we need to increment the hit counter mObjectHitCounts.at(detection.class_id) = std::min(mObjMaxHitcount, mObjectHitCounts.at(detection.class_id) + mObjIncrementWeight); - + ROS_INFO("PUSHED TO TF TEMP"); //Only publish to permament if we are confident in the object if (mObjectHitCounts.at(detection.class_id) > mObjHitThreshold) { std::string permanentFrameId = "detectedObject" + classes.at(detection.class_id); + //Grab the object inside of the camera frame and push it into the map frame SE3 objectInsideCamera = SE3::fromTfTree(mTfBuffer, mMapFrameId, immediateFrameId); SE3::pushToTfTree(mTfBroadcaster, permanentFrameId, mCameraFrameId, objectInsideCamera); From 30e9dce814763c3c5cade2b1e4ed8facfdd58319 Mon Sep 17 00:00:00 2001 From: John Date: Sat, 3 Feb 2024 14:09:03 -0500 Subject: [PATCH 151/197] COLORS --- .../object_detector/object_detector.processing.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index a27f9a44e..17e38d5c2 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -158,9 +159,11 @@ namespace mrover { } //Draw the detected object's bounding boxes on the image for each of the objects detected + std::vector font_Colors = {cv::Scalar{232, 115, 5}, + cv::Scalar{0, 4, 227}}; for (size_t i = 0; i < detections.size(); i++) { //Font color will change for each different detection - cv::Scalar font_Color(static_cast(i * 50), static_cast(i * 50), static_cast(i * 50)); + cv::Scalar font_Color = font_Colors.at(detections.at(i).class_id); cv::rectangle(sizedImage, detections[i].box, font_Color, 1, cv::LINE_8, 0); //Put the text on the image @@ -249,10 +252,10 @@ namespace mrover { auto centerWidth = static_cast(center.first * static_cast(msg->width) / imgSize.width); auto centerHeight = static_cast(center.second * static_cast(msg->height) / imgSize.height); - std::cout << mObjectHitCounts.at(0) << ", " << mObjectHitCounts.at(1) << std::endl; + std::cout << mObjectHitCounts.at(0) << ", " << mObjectHitCounts.at(1) << std::endl; ROS_INFO("%d, %d", mObjectHitCounts.at(0), mObjectHitCounts.at(1)); - if (!seenObjects.at(detection.class_id)) { + if (!seenObjects.at(detection.class_id)) { seenObjects.at(detection.class_id) = true; //Get the object's position in 3D from the point cloud and run this statement if the optional has a value @@ -266,7 +269,7 @@ namespace mrover { //Since the object is seen we need to increment the hit counter mObjectHitCounts.at(detection.class_id) = std::min(mObjMaxHitcount, mObjectHitCounts.at(detection.class_id) + mObjIncrementWeight); - ROS_INFO("PUSHED TO TF TEMP"); + ROS_INFO("PUSHED TO TF TEMP"); //Only publish to permament if we are confident in the object if (mObjectHitCounts.at(detection.class_id) > mObjHitThreshold) { From 01d1b413ed72b52737c2dcbc2b92a706803e0f64 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 4 Feb 2024 11:29:02 -0500 Subject: [PATCH 152/197] Loop Profiler --- .../object_detector/object_detector.cpp | 3 ++ .../object_detector/object_detector.hpp | 8 ++-- .../object_detector.processing.cpp | 43 ++++++++++++++++++- 3 files changed, 50 insertions(+), 4 deletions(-) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 03de4d439..52f4c451e 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -1,6 +1,7 @@ #include "object_detector.hpp" #include "inference_wrapper.hpp" #include "pch.hpp" +#include namespace mrover { @@ -30,5 +31,7 @@ namespace mrover { //Initialize Object Hit Cout to Zeros mObjectHitCounts = {0, 0}; + + mEnableLoopProfiler = true; } } // namespace mrover diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index cded7fcaa..c8dc95c90 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -2,6 +2,7 @@ #include "inference_wrapper.hpp" #include "pch.hpp" +#include #include #include @@ -17,6 +18,10 @@ namespace mrover { class ObjectDetectorNodelet : public nodelet::Nodelet { + //Loop Profiler + LoopProfiler mLoopProfiler{"Object Detector", 1}; + bool mEnableLoopProfiler; + //Mat for the image from the point cloud cv::Mat mImg; @@ -44,9 +49,6 @@ namespace mrover { // Internal state cv::dnn::Net mNet; - // Debug - LoopProfiler mProfiler{"Object Detector"}; - //TF Member Variables tf2_ros::Buffer mTfBuffer; tf2_ros::TransformListener mTfListener{mTfBuffer}; diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 17e38d5c2..1a6516730 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -11,10 +11,19 @@ #include #include - namespace mrover { void ObjectDetectorNodelet::imageCallback(sensor_msgs::PointCloud2ConstPtr const& msg) { + + // + if (mEnableLoopProfiler) { + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { + ros::console::notifyLoggerLevelsChanged(); + } + mLoopProfiler.beginLoop(); + mLoopProfiler.measureEvent("Wait"); + } + //Does the msg exist with a height and width assert(msg); assert(msg->height > 0); @@ -40,12 +49,20 @@ namespace mrover { //Create the blob from the resized image cv::dnn::blobFromImage(sizedImage, mImageBlob, 1.0 / 255.0, imgSize, cv::Scalar(), true, false); + if (mEnableLoopProfiler) { + mLoopProfiler.measureEvent("Convert Image"); + } + //Run the blob through the model mInferenceWrapper.doDetections(mImageBlob); //Retrieve the output from the model cv::Mat output = mInferenceWrapper.getOutputTensor(); + if (mEnableLoopProfiler) { + mLoopProfiler.measureEvent("Execute on GPU"); + } + //Get model specific information int rows = output.rows; int dimensions = output.cols; @@ -144,6 +161,14 @@ namespace mrover { detections.push_back(result); } + if (mEnableLoopProfiler) { + mLoopProfiler.measureEvent("Extract Detections"); + } + + if (mEnableLoopProfiler) { + //mLoopProfiler.measureEvent("Push to TF START"); + } + std::vector seenObjects = {false, false}; //If there are detections locate them in 3D for (Detection const& detection: detections) { @@ -151,6 +176,10 @@ namespace mrover { //Increment Object hit counts if theyre seen updateHitsObject(msg, detection, seenObjects); + if (mEnableLoopProfiler) { + //mLoopProfiler.measureEvent("Push to TF 1"); + } + //Decrement Object hit counts if they're not seen for (size_t i = 0; i < seenObjects.size(); i++) { if (!seenObjects.at(i)) { @@ -158,6 +187,10 @@ namespace mrover { } } + if (mEnableLoopProfiler) { + //mLoopProfiler.measureEvent("Push to TF 2"); + } + //Draw the detected object's bounding boxes on the image for each of the objects detected std::vector font_Colors = {cv::Scalar{232, 115, 5}, cv::Scalar{0, 4, 227}}; @@ -174,11 +207,19 @@ namespace mrover { } } + if (mEnableLoopProfiler) { + mLoopProfiler.measureEvent("Push to TF FINAL"); + } + //We only want to publish the debug image if there is something lsitening, to reduce the operations going on if (mDebugImgPub.getNumSubscribers() > 0 || true) { //Publishes the image to the debug publisher publishImg(sizedImage); } + + if (mEnableLoopProfiler) { + mLoopProfiler.measureEvent("Publish Debug Img"); + } } // namespace mrover auto ObjectDetectorNodelet::getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v, size_t width, size_t height) -> std::optional { From 9f418653012d5076b35c253e682abf3a2bb8f8f4 Mon Sep 17 00:00:00 2001 From: John Date: Mon, 5 Feb 2024 20:52:42 -0500 Subject: [PATCH 153/197] Cleaned up Image Capture --- .../image_capture/image_capturepy.py | 38 ++++++++----------- 1 file changed, 16 insertions(+), 22 deletions(-) diff --git a/src/perception/image_capture/image_capturepy.py b/src/perception/image_capture/image_capturepy.py index 095f27247..95d0b09e2 100755 --- a/src/perception/image_capture/image_capturepy.py +++ b/src/perception/image_capture/image_capturepy.py @@ -2,13 +2,11 @@ from pynput.keyboard import Key, Controller - # python linear algebra library import numpy as np # library for interacting with ROS and TF tree import rospy -import tf2_ros import cv2 @@ -18,42 +16,39 @@ # ROS message types we need to use from sensor_msgs.msg import Image -# SE3 library for handling poses and TF tree -from util.SE3 import SE3 - -import math - import threading + class KeyboardThread(threading.Thread): - def __init__(self, input_cbk = None, name='keyboard-input-thread'): + def __init__(self, input_cbk=None, name="keyboard-input-thread"): self.input_cbk = input_cbk super(KeyboardThread, self).__init__(name=name) self.start() self.run = True - def run(self): while True: - self.input_cbk(input()) #waits to get input + Return - if(not self.run): + self.input_cbk(input()) # waits to get input + Return + if not self.run: break def stop(self): self.run = False -showcounter = 0 #something to demonstrate the change + +showcounter = 0 # something to demonstrate the change + def my_callback(inp): - #evaluate the keyboard input + # evaluate the keyboard input image_capturepy.showcounter += 1 -#start the Keyboard thread -class image_capturepy: - pose: SE3 +# start the Keyboard thread + +class image_capturepy: showcounter = 0 def __init__(self): @@ -67,17 +62,16 @@ def __del__(self): self.keyboard.release(Key.enter) def img_callback(self, msg): - data = np.empty(msg.height * msg.width * 4, dtype = np.uint8) + data = np.empty(msg.height * msg.width * 4, dtype=np.uint8) for x in range(msg.height * msg.width * 4): data[x] = msg.data[x] - + image = np.reshape(data, [msg.height, msg.width, 4]) print(image_capturepy.showcounter) - if(image_capturepy.showcounter != 0): + if image_capturepy.showcounter != 0: unique_id = "{date:%Y-%m-%d_%H:%M:%S}".format(date=datetime.datetime.now()) - print(cv2.imwrite(f'//home//john//Desktop//Rover//Images//image_{unique_id}.jpg', image)) + print(cv2.imwrite(f"//home//john//Desktop//Rover//Images//image_{unique_id}.jpg", image)) image_capturepy.showcounter = 0 - def main(): @@ -92,4 +86,4 @@ def main(): if __name__ == "__main__": - main() \ No newline at end of file + main() From 298d32f5bcf40e69d233e8e76c509155bb19e1e7 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 6 Feb 2024 18:18:22 -0500 Subject: [PATCH 154/197] updates --- launch/auton.launch | 47 ++++++++++--------- src/perception/object_detector/inference.cu | 5 +- src/perception/object_detector/inference.cuh | 4 ++ .../object_detector/object_detector.cpp | 5 +- .../object_detector/object_detector.hpp | 4 ++ 5 files changed, 40 insertions(+), 25 deletions(-) diff --git a/launch/auton.launch b/launch/auton.launch index 11f8e19f9..d642ecbf2 100644 --- a/launch/auton.launch +++ b/launch/auton.launch @@ -1,11 +1,11 @@ - - - - - - + + + + + + + args="manager" output="screen" /> + pkg="nodelet" type="nodelet" name="tag_detector" + args="load mrover/TagDetectorNodelet nodelet_manager" output="screen" /> - + - - + + - - - + + + - + clear_params="true" launch-prefix="bash -c 'sleep $(arg ekf_start_delay); $0 $@'"> + - + - - + + \ No newline at end of file diff --git a/src/perception/object_detector/inference.cu b/src/perception/object_detector/inference.cu index b101616fd..27f5a3894 100644 --- a/src/perception/object_detector/inference.cu +++ b/src/perception/object_detector/inference.cu @@ -34,6 +34,9 @@ namespace mrover { } ICudaEngine* Inference::createCudaEngine(std::filesystem::path const& onnxModelPath) { + + mModelPath = {onnxModelPath.c_str()}; + //Define the size of Batches constexpr auto explicitBatch = 1U << static_cast(NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); @@ -67,7 +70,7 @@ namespace mrover { //Define the engine file location relative to the mrover package std::filesystem::path packagePath = ros::package::getPath("mrover"); - std::filesystem::path enginePath = packagePath / "data" / "tensorrt-engine-yolov8n_mallet_bottle_better.engine"; + std::filesystem::path enginePath = packagePath / "data" / std::string("tensorrt-engine-").append(mModelPath).append(".engine"); //Check if engine file exists if (!exists(enginePath)) { diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index ef476a807..8363ac8f4 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -8,6 +8,7 @@ #include #include #include +#include using nvinfer1::ICudaEngine; using nvinfer1::IExecutionContext; @@ -16,6 +17,9 @@ namespace mrover { class Inference { private: + //Model Path + std::string mModelPath; + //Init Logger nvinfer1::Logger mLogger; diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 52f4c451e..647559c70 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -2,6 +2,7 @@ #include "inference_wrapper.hpp" #include "pch.hpp" #include +#include namespace mrover { @@ -11,7 +12,7 @@ namespace mrover { mPnh = getMTPrivateNodeHandle(); std::filesystem::path packagePath = ros::package::getPath("mrover"); - std::filesystem::path modelPath = packagePath / "data" / "yolov8n_mallet_bottle_better.onnx"; + std::filesystem::path modelPath = packagePath / "data" / mModelName.append(".onnx"); mInferenceWrapper = InferenceWrapper{modelPath}; @@ -28,6 +29,8 @@ namespace mrover { mNh.param("obj_decrement_weight", mObjDecrementWeight, 1); mNh.param("obj_hitcount_threshold", mObjHitThreshold, 5); mNh.param("obj_hitcount_max", mObjMaxHitcount, 10); + mNh.param("model_name", mModelName, "yolov8n_mallet_bottle_better"); + //Initialize Object Hit Cout to Zeros mObjectHitCounts = {0, 0}; diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index c8dc95c90..7130ff961 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -5,6 +5,7 @@ #include #include #include +#include namespace mrover { @@ -18,6 +19,9 @@ namespace mrover { class ObjectDetectorNodelet : public nodelet::Nodelet { + //Model Name for Object Detector + std::string mModelName; + //Loop Profiler LoopProfiler mLoopProfiler{"Object Detector", 1}; bool mEnableLoopProfiler; From 988bf770df765ea93523a430e1dae12c52de1e13 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 8 Feb 2024 10:30:39 -0500 Subject: [PATCH 155/197] Remove long range stuff from this PR --- CMakeLists.txt | 6 - launch/long_range_cam.launch | 19 -- msg/DetectedObject.msg | 4 - msg/DetectedObjectType.msg | 3 - msg/DetectedObjects.txt | 5 - msg/LongRangeTag.msg | 4 - msg/LongRangeTags.msg | 1 - plugins/long_range_tag_detector_plugin.xml | 6 - src/navigation/approach_object.py | 34 --- src/navigation/approach_post.py | 110 ++++----- src/navigation/approach_target_base.py | 63 ------ src/navigation/context.py | 107 ++------- src/navigation/long_range.py | 63 ------ src/navigation/navigation.py | 30 +-- src/navigation/post_backup.py | 4 +- src/navigation/search.py | 42 +--- src/navigation/waypoint.py | 24 +- .../long_range_cam/long_range_cam.cpp | 88 ------- .../long_range_cam/long_range_cam.hpp | 38 ---- src/perception/long_range_cam/main.cpp | 23 -- src/perception/long_range_cam/pch.hpp | 32 --- .../long_range_tag_detector.cpp | 147 ------------ .../long_range_tag_detector.hpp | 158 ------------- .../long_range_tag_detector.processing.cpp | 214 ------------------ .../tag_detector/long_range_cam/main.cpp | 22 -- .../tag_detector/long_range_cam/pch.hpp | 35 --- 26 files changed, 78 insertions(+), 1204 deletions(-) delete mode 100644 launch/long_range_cam.launch delete mode 100644 msg/DetectedObject.msg delete mode 100644 msg/DetectedObjectType.msg delete mode 100644 msg/DetectedObjects.txt delete mode 100644 msg/LongRangeTag.msg delete mode 100644 msg/LongRangeTags.msg delete mode 100644 plugins/long_range_tag_detector_plugin.xml delete mode 100644 src/navigation/approach_object.py delete mode 100644 src/navigation/approach_target_base.py delete mode 100644 src/navigation/long_range.py delete mode 100644 src/perception/long_range_cam/long_range_cam.cpp delete mode 100644 src/perception/long_range_cam/long_range_cam.hpp delete mode 100644 src/perception/long_range_cam/main.cpp delete mode 100644 src/perception/long_range_cam/pch.hpp delete mode 100644 src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp delete mode 100644 src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp delete mode 100644 src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp delete mode 100644 src/perception/tag_detector/long_range_cam/main.cpp delete mode 100644 src/perception/tag_detector/long_range_cam/pch.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 36910b059..1aed42b0e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -186,12 +186,6 @@ endif () mrover_add_nodelet(zed_tag_detector src/perception/tag_detector/zed/*.cpp src/perception/tag_detector/zed src/perception/tag_detector/zed/pch.hpp) mrover_nodelet_link_libraries(zed_tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc tbb lie) -mrover_add_nodelet(long_range_tag_detector src/perception/tag_detector/long_range_cam/*.cpp src/perception/tag_detector/long_range_cam src/perception/tag_detector/long_range_cam/pch.hpp) -mrover_nodelet_link_libraries(long_range_tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc tbb lie) - -mrover_add_nodelet(usb_camera src/perception/long_range_cam/*.cpp src/perception/long_range_cam src/perception/long_range_cam/pch.hpp) -mrover_nodelet_link_libraries(usb_camera opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui tbb lie) - if (CUDA_FOUND) mrover_add_library(streaming src/esw/streaming/*.c* src/esw/streaming) # target_link_libraries(streaming PUBLIC opencv_core opencv_cudacodec) diff --git a/launch/long_range_cam.launch b/launch/long_range_cam.launch deleted file mode 100644 index be21fde31..000000000 --- a/launch/long_range_cam.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/msg/DetectedObject.msg b/msg/DetectedObject.msg deleted file mode 100644 index 9d812a11f..000000000 --- a/msg/DetectedObject.msg +++ /dev/null @@ -1,4 +0,0 @@ -DetectedObjectType type -float32 detection_confidence -float32 bearing -float32 distance diff --git a/msg/DetectedObjectType.msg b/msg/DetectedObjectType.msg deleted file mode 100644 index 28c2319ff..000000000 --- a/msg/DetectedObjectType.msg +++ /dev/null @@ -1,3 +0,0 @@ -uint8 NO_OBJ=0 -uint8 MALLET=1 -uint8 WATER_BOTTLE=2 \ No newline at end of file diff --git a/msg/DetectedObjects.txt b/msg/DetectedObjects.txt deleted file mode 100644 index b0fe335be..000000000 --- a/msg/DetectedObjects.txt +++ /dev/null @@ -1,5 +0,0 @@ -Header header - -int num_detections - -DetectedObject[] detections \ No newline at end of file diff --git a/msg/LongRangeTag.msg b/msg/LongRangeTag.msg deleted file mode 100644 index ae4d49ab8..000000000 --- a/msg/LongRangeTag.msg +++ /dev/null @@ -1,4 +0,0 @@ -# Message published by the LongRangeTagDetector nodelet. -# Bearing is in degrees from center of image -uint8 id -float32 bearing \ No newline at end of file diff --git a/msg/LongRangeTags.msg b/msg/LongRangeTags.msg deleted file mode 100644 index 4a48ffb9b..000000000 --- a/msg/LongRangeTags.msg +++ /dev/null @@ -1 +0,0 @@ -LongRangeTag[] longRangeTags \ No newline at end of file diff --git a/plugins/long_range_tag_detector_plugin.xml b/plugins/long_range_tag_detector_plugin.xml deleted file mode 100644 index 6a7d82109..000000000 --- a/plugins/long_range_tag_detector_plugin.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - \ No newline at end of file diff --git a/src/navigation/approach_object.py b/src/navigation/approach_object.py deleted file mode 100644 index 55faf80ed..000000000 --- a/src/navigation/approach_object.py +++ /dev/null @@ -1,34 +0,0 @@ -import tf2_ros - -from util.ros_utils import get_rosparam -from util.state_lib.state import State -from typing import Optional - -from navigation import state -from navigation.approach_target_base import ApproachTargetBaseState - - -class ApproachObjectState(ApproachTargetBaseState): - """ - State for when we see an object (mallet or water bottle). - We are only using the ZED camera. - Transitions: - -If arrived at target: DoneState - -Did not arrive at target: ApproachObjectState - """ - - def on_enter(self, context): - pass - - def on_exit(self, context): - pass - - def get_target_pos(self, context) -> Optional[int]: - object_pos = context.env.current_target_pos() - return object_pos - - def determine_next(self, context, finished: bool) -> State: - if finished: - return state.DoneState() - else: - return self diff --git a/src/navigation/approach_post.py b/src/navigation/approach_post.py index 5eaae0bc8..b3ebfb676 100644 --- a/src/navigation/approach_post.py +++ b/src/navigation/approach_post.py @@ -2,80 +2,52 @@ from util.ros_utils import get_rosparam from util.state_lib.state import State -from typing import Optional -from navigation import search, state, waypoint -from navigation.approach_target_base import ApproachTargetBaseState +from navigation import search, waypoint -class ApproachPostState(ApproachTargetBaseState): - """ - State for when the tag is seen in the ZED camera. - Transitions: - -If arrived at target: DoneState - -Did not arrive at target: ApproachPostState - """ +class ApproachPostState(State): + STOP_THRESH = get_rosparam("single_fiducial/stop_thresh", 0.7) + FIDUCIAL_STOP_THRESHOLD = get_rosparam("single_fiducial/fiducial_stop_threshold", 1.75) + DRIVE_FWD_THRESH = get_rosparam("waypoint/drive_fwd_thresh", 0.34) # 20 degrees - def on_enter(self, context): + def on_enter(self, context) -> None: pass - def on_exit(self, context): + def on_exit(self, context) -> None: pass - def get_target_pos(self, context) -> Optional[int]: - # return fid_pos, either position or None - fid_pos = context.env.current_target_pos() - return fid_pos - - def determine_next(self, context, finished: bool) -> State: - if finished: - return state.DoneState() - else: - return self - - -# class ApproachPostState(State): -# STOP_THRESH = get_rosparam("single_fiducial/stop_thresh", 0.7) -# FIDUCIAL_STOP_THRESHOLD = get_rosparam("single_fiducial/fiducial_stop_threshold", 1.75) -# DRIVE_FWD_THRESH = get_rosparam("waypoint/drive_fwd_thresh", 0.34) # 20 degrees - -# def on_enter(self, context) -> None: -# pass - -# def on_exit(self, context) -> None: -# pass - -# def on_loop(self, context) -> State: -# """ -# Drive towards a single fiducial if we see it and stop within a certain threshold if we see it. -# Else conduct a search to find it. -# :param ud: State machine user data -# :return: Next state -# """ -# fid_pos = context.env.current_target_pos() -# if fid_pos is None: -# return search.SearchState() - -# try: -# cmd_vel, arrived = context.rover.driver.get_drive_command( -# fid_pos, -# context.rover.get_pose(in_odom_frame=True), -# self.STOP_THRESH, -# self.DRIVE_FWD_THRESH, -# in_odom=context.use_odom, -# ) -# if arrived: -# context.env.arrived_at_post = True -# context.env.last_post_location = context.env.current_target_pos(odom_override=False) -# context.course.increment_waypoint() -# return waypoint.WaypointState() # we want to go to done state now -# context.rover.send_drive_command(cmd_vel) -# except ( -# tf2_ros.LookupException, -# tf2_ros.ConnectivityException, -# tf2_ros.ExtrapolationException, -# ): -# # TODO: probably go into some waiting state (old todo) -# pass - -# return self + def on_loop(self, context) -> State: + """ + Drive towards a single fiducial if we see it and stop within a certain threshold if we see it. + Else conduct a search to find it. + :param ud: State machine user data + :return: Next state + """ + fid_pos = context.env.current_fid_pos() + if fid_pos is None: + return search.SearchState() + + try: + cmd_vel, arrived = context.rover.driver.get_drive_command( + fid_pos, + context.rover.get_pose(in_odom_frame=True), + self.STOP_THRESH, + self.DRIVE_FWD_THRESH, + in_odom=context.use_odom, + ) + if arrived: + context.env.arrived_at_post = True + context.env.last_post_location = context.env.current_fid_pos(odom_override=False) + context.course.increment_waypoint() + return waypoint.WaypointState() + context.rover.send_drive_command(cmd_vel) + except ( + tf2_ros.LookupException, + tf2_ros.ConnectivityException, + tf2_ros.ExtrapolationException, + ): + # TODO: probably go into some waiting state + pass + + return self diff --git a/src/navigation/approach_target_base.py b/src/navigation/approach_target_base.py deleted file mode 100644 index 135acc1f9..000000000 --- a/src/navigation/approach_target_base.py +++ /dev/null @@ -1,63 +0,0 @@ -import tf2_ros - -from util.ros_utils import get_rosparam -from util.state_lib.state import State -from abc import abstractmethod -from typing import Optional - -from navigation import search - - -class ApproachTargetBaseState(State): - STOP_THRESH = get_rosparam("single_fiducial/stop_thresh", 0.7) - FIDUCIAL_STOP_THRESHOLD = get_rosparam("single_fiducial/fiducial_stop_threshold", 1.75) - DRIVE_FWD_THRESH = get_rosparam("waypoint/drive_fwd_thresh", 0.34) # 20 degrees - - def on_enter(self, context) -> None: - pass - - def on_exit(self, context) -> None: - pass - - @abstractmethod - def get_target_pos(self, context) -> Optional[int]: - raise NotImplementedError - - @abstractmethod - def determine_next(self, context, finished: bool) -> State: - raise NotImplementedError - - def on_loop(self, context) -> State: - """ - Drive towards a target based on what gets returned from get_target_pos(). - Return to search if there is no target position. - :param context: rover context - :return: Next state - """ - target_pos = self.get_target_pos(context) - if target_pos is None: - return search.SearchState() - - try: - cmd_vel, arrived = context.rover.driver.get_drive_command( - target_pos, - context.rover.get_pose(in_odom_frame=True), - self.STOP_THRESH, - self.DRIVE_FWD_THRESH, - in_odom=context.use_odom, - ) - next_state = self.determine_next(context, arrived) - if arrived: - context.env.arrived_at_target = True - context.env.last_target_location = self.get_target_pos(context) - context.course.increment_waypoint() - else: - context.rover.send_drive_command(cmd_vel) - except ( - tf2_ros.LookupException, - tf2_ros.ConnectivityException, - tf2_ros.ExtrapolationException, - ): - pass - - return next_state diff --git a/src/navigation/context.py b/src/navigation/context.py index 3b67c54f4..0edfb9cc3 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -9,26 +9,15 @@ import rospy import tf2_ros from geometry_msgs.msg import Twist -from mrover.msg import ( - Waypoint, - GPSWaypoint, - WaypointType, - GPSPointList, - Course as CourseMsg, - LongRangeTag, - LongRangeTags, -) +from mrover.msg import Waypoint, GPSWaypoint, WaypointType, GPSPointList, Course as CourseMsg from mrover.srv import EnableAuton, EnableAutonRequest, EnableAutonResponse from navigation.drive import DriveController from std_msgs.msg import Time, Bool from util.SE3 import SE3 from visualization_msgs.msg import Marker -from util.ros_utils import get_rosparam TAG_EXPIRATION_TIME_SECONDS = 60 -TIME_THRESHOLD = get_rosparam("long_range/time_threshold", 5) - REF_LAT = rospy.get_param("gps_linearization/reference_point_latitude") REF_LON = rospy.get_param("gps_linearization/reference_point_longitude") @@ -70,12 +59,11 @@ class Environment: """ ctx: Context - long_range_tags: LongRangeTagStore NO_FIDUCIAL: ClassVar[int] = -1 - arrived_at_target: bool = False - last_target_location: Optional[np.ndarray] = None + arrived_at_post: bool = False + last_post_location: Optional[np.ndarray] = None - def get_target_pos(self, id: str, in_odom_frame: bool = True) -> Optional[np.ndarray]: + def get_fid_pos(self, fid_id: int, in_odom_frame: bool = True) -> Optional[np.ndarray]: """ Retrieves the pose of the given fiducial ID from the TF tree in the odom frame if in_odom_frame is True otherwise in the world frame @@ -83,10 +71,12 @@ def get_target_pos(self, id: str, in_odom_frame: bool = True) -> Optional[np.nda """ try: parent_frame = self.ctx.odom_frame if in_odom_frame else self.ctx.world_frame - target_pose, time = SE3.from_tf_time(self.ctx.tf_buffer, parent_frame=parent_frame, child_frame=f"{id}") + fid_pose, time = SE3.from_tf_time( + self.ctx.tf_buffer, parent_frame=parent_frame, child_frame=f"fiducial{fid_id}" + ) now = rospy.Time.now() if now.to_sec() - time.to_sec() >= TAG_EXPIRATION_TIME_SECONDS: - print(f"EXPIRED {id}!") + print(f"TAG EXPIRED {fid_id}!") return None except ( tf2_ros.LookupException, @@ -94,11 +84,11 @@ def get_target_pos(self, id: str, in_odom_frame: bool = True) -> Optional[np.nda tf2_ros.ExtrapolationException, ) as e: return None - return target_pose.position + return fid_pose.position - def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray]: + def current_fid_pos(self, odom_override: bool = True) -> Optional[np.ndarray]: """ - Retrieves the position of the current fiducial or object (and we are looking for it) + Retrieves the position of the current fiducial (and we are looking for it) :param: odom_override if false will force it to be in the map frame """ assert self.ctx.course @@ -108,61 +98,7 @@ def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray] print("CURRENT WAYPOINT IS NONE") return None - if current_waypoint.type.val == WaypointType.POST: - return self.get_target_pos(f"fiducial{current_waypoint.tag_id}", in_odom) - elif current_waypoint.type.val == WaypointType.MALLET: - return self.get_target_pos("detectedObjectHammer", in_odom) - elif current_waypoint.type == WaypointType.WATER_BOTTLE: - return self.get_target_pos("detectedObjectBottle", in_odom) - else: - # print("CURRENT WAYPOINT IS NOT A POST OR OBJECT") - return None - - -class LongRangeTagStore: - @dataclass - class TagData: - hit_count: int - tag: LongRangeTag - time: rospy.Time - - ctx: Context - __data: dict[int, TagData] - min_hits: int - - def __init__(self, ctx: Context, min_hits: int, max_hits: int = 10) -> None: - self.ctx = ctx - self.__data = {} - self.min_hits = min_hits - self.max_hits = max_hits - - def push_frame(self, tags: List[LongRangeTag]) -> None: - for _, cur_tag in self.__data.items(): - if cur_tag.tag.id not in tags: - cur_tag.hit_count -= 1 - if cur_tag.hit_count <= 0: - del self.__data[cur_tag.tag.id] - else: - cur_tag.hit_count += 1 - cur_tag.hit_count = min(cur_tag.hit_count, self.max_hits) - - for tag in tags: - if tag.id not in self.__data: - self.__data[tag.id] = self.TagData(hit_count=1, tag=tag, time=rospy.get_time()) - - def get(self, tag_id: int) -> Optional[LongRangeTag]: - if len(self.__data) == 0: - return None - - time_difference = rospy.get_time() - self.__data[tag_id].time - if ( - tag_id in self.__data - and self.__data[tag_id].hit_count >= self.min_hits - and time_difference <= TIME_THRESHOLD - ): - return self.__data[tag_id].tag - else: - return None + return self.get_fid_pos(current_waypoint.tag_id, in_odom) @dataclass @@ -210,17 +146,6 @@ def look_for_post(self) -> bool: else: return False - def look_for_object(self) -> bool: - """ - Returns whether the currently active waypoint (if it exists) indicates - that we should go to either the mallet or the water bottle. - """ - waypoint = self.current_waypoint() - if waypoint is not None: - return waypoint.type.val == WaypointType.MALLET or waypoint.type.val == WaypointType.WATER_BOTTLE - else: - return False - def is_complete(self) -> bool: return self.waypoint_index == len(self.course_data.waypoints) @@ -275,7 +200,6 @@ class Context: vis_publisher: rospy.Publisher course_listener: rospy.Subscriber stuck_listener: rospy.Subscriber - # tag_data_listener: rospy.Subscriber # Use these as the primary interfaces in states course: Optional[Course] @@ -299,14 +223,12 @@ def __init__(self): self.stuck_listener = rospy.Subscriber("nav_stuck", Bool, self.stuck_callback) self.course = None self.rover = Rover(self, False, "") - # TODO update min_hits to be a param - self.env = Environment(self, long_range_tags=LongRangeTagStore(self, 3)) + self.env = Environment(self) self.disable_requested = False self.use_odom = rospy.get_param("use_odom_frame") self.world_frame = rospy.get_param("world_frame") self.odom_frame = rospy.get_param("odom_frame") self.rover_frame = rospy.get_param("rover_frame") - self.tag_data_listener = rospy.Subscriber("tags", LongRangeTags, self.tag_data_callback) def recv_enable_auton(self, req: EnableAutonRequest) -> EnableAutonResponse: if req.enable: @@ -317,6 +239,3 @@ def recv_enable_auton(self, req: EnableAutonRequest) -> EnableAutonResponse: def stuck_callback(self, msg: Bool): self.rover.stuck = msg.data - - def tag_data_callback(self, tags: LongRangeTags) -> None: - self.env.long_range_tags.push_frame(tags.longRangeTags) diff --git a/src/navigation/long_range.py b/src/navigation/long_range.py deleted file mode 100644 index 2f13bc4b0..000000000 --- a/src/navigation/long_range.py +++ /dev/null @@ -1,63 +0,0 @@ -import tf2_ros -import rospy - -from util.ros_utils import get_rosparam -from util.state_lib.state import State -from util.np_utils import normalized -from typing import Optional - -from navigation import approach_post -from navigation.approach_target_base import ApproachTargetBaseState -import numpy as np - -DIST_AHEAD = get_rosparam("long_range/distance_ahead", 20) - - -class LongRangeState(ApproachTargetBaseState): - """ - State for when the tag is seen only in the long range camera. - Transitions: - -If tag seen in ZED: ApproachPostState - -Not seen in ZED and moved: LongRangeState - -Don't see the tag in long range camera: SearchState - -Stuck? - """ - - def on_enter(self, context): - pass - - def on_exit(self, context): - pass - - def get_target_pos(self, context) -> Optional[int]: - tag_id = context.course.get_current_waypoint().tag_id - tag = context.env.long_range_tags.get(tag_id) - if tag is None: - return None - pose = context.rover.get_pose() - if pose is None: - return None - - rover_position = pose.position - rover_direction = pose.rotation.direction_vector() - - bearing_to_tag = np.radians(tag.bearing) - - # rover_direction rotated by bearing to tag - bearing_rotation_mat = np.array( - [[np.cos(bearing_to_tag), -np.sin(bearing_to_tag)], [np.sin(bearing_to_tag), np.cos(bearing_to_tag)]] - ) - - direction_to_tag = bearing_rotation_mat @ rover_direction[:2] - - direction_to_tag = normalized(direction_to_tag) - distance = DIST_AHEAD - tag_position = rover_position + direction_to_tag * distance - return tag_position - - def determine_next(self, context, finished: bool) -> State: - fid_pos = context.env.current_fid_pos() - if fid_pos is None: - return self - else: - return approach_post.ApproachPostState() diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py index 056ac5cfc..82eeee331 100755 --- a/src/navigation/navigation.py +++ b/src/navigation/navigation.py @@ -14,8 +14,6 @@ from navigation.search import SearchState from navigation.state import DoneState, OffState, off_check from navigation.waypoint import WaypointState -from navigation.approach_object import ApproachObjectState -from navigation.long_range import LongRangeState class Navigation(threading.Thread): @@ -32,35 +30,13 @@ def __init__(self, context: Context): ) self.state_machine.add_transitions(PostBackupState(), [WaypointState(), RecoveryState()]) self.state_machine.add_transitions( - RecoveryState(), - [ - WaypointState(), - SearchState(), - PostBackupState(), - ApproachPostState(), - ApproachObjectState(), - LongRangeState(), - ], - ) - self.state_machine.add_transitions( - SearchState(), - [ApproachPostState(), ApproachObjectState(), LongRangeState(), WaypointState(), RecoveryState()], + RecoveryState(), [WaypointState(), SearchState(), PostBackupState(), ApproachPostState()] ) + self.state_machine.add_transitions(SearchState(), [ApproachPostState(), WaypointState(), RecoveryState()]) self.state_machine.add_transitions(DoneState(), [WaypointState()]) self.state_machine.add_transitions( - WaypointState(), - [ - PostBackupState(), - ApproachPostState(), - ApproachObjectState(), - LongRangeState(), - SearchState(), - RecoveryState(), - DoneState(), - ], + WaypointState(), [PostBackupState(), ApproachPostState(), SearchState(), RecoveryState(), DoneState()] ) - self.state_machine.add_transitions(ApproachObjectState(), [DoneState(), SearchState()]) - self.state_machine.add_transitions(LongRangeState(), [ApproachPostState()]) self.state_machine.add_transitions(OffState(), [WaypointState(), DoneState()]) self.state_machine.configure_off_switch(OffState(), off_check) self.state_machine_server = StatePublisher(self.state_machine, "nav_structure", 1, "nav_state", 10) diff --git a/src/navigation/post_backup.py b/src/navigation/post_backup.py index d205cd5c9..b7c51246f 100644 --- a/src/navigation/post_backup.py +++ b/src/navigation/post_backup.py @@ -99,12 +99,12 @@ def on_exit(self, context): self.traj = None def on_enter(self, context) -> None: - if context.env.last_target_location is None: + if context.env.last_post_location is None: self.traj = None else: self.traj = AvoidPostTrajectory.avoid_post_trajectory( context.rover.get_pose(), - context.env.last_target_location, + context.env.last_post_location, context.course.current_waypoint_pose().position, ) self.traj.cur_pt = 0 diff --git a/src/navigation/search.py b/src/navigation/search.py index 3db5ede04..2b0d8ee3b 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -4,13 +4,12 @@ from typing import Optional import numpy as np -from mrover.msg import GPSPointList, WaypointType - +from mrover.msg import GPSPointList from util.ros_utils import get_rosparam from util.state_lib.state import State -from navigation import approach_post, approach_object, recovery, waypoint, long_range +from navigation import approach_post, recovery, waypoint from navigation.context import convert_cartesian_to_gps from navigation.trajectory import Trajectory @@ -94,24 +93,14 @@ class SearchState(State): def on_enter(self, context) -> None: search_center = context.course.current_waypoint() - if not self.is_recovering: - if search_center.type.val == WaypointType.POST: - self.traj = SearchTrajectory.spiral_traj( - context.rover.get_pose().position[0:2], - self.SPIRAL_COVERAGE_RADIUS, - self.DISTANCE_BETWEEN_SPIRALS, - self.SEGMENTS_PER_ROTATION, - search_center.tag_id, - ) - else: # water bottle or mallet - self.traj = SearchTrajectory.spiral_traj( - context.rover.get_pose().position[0:2], - self.SPIRAL_COVERAGE_RADIUS / 2, - self.DISTANCE_BETWEEN_SPIRALS / 2, - self.SEGMENTS_PER_ROTATION, - search_center.tag_id, - ) + self.traj = SearchTrajectory.spiral_traj( + context.rover.get_pose().position[0:2], + self.SPIRAL_COVERAGE_RADIUS, + self.DISTANCE_BETWEEN_SPIRALS, + self.SEGMENTS_PER_ROTATION, + search_center.tag_id, + ) self.prev_target = None def on_exit(self, context) -> None: @@ -145,15 +134,6 @@ def on_loop(self, context) -> State: ) context.rover.send_drive_command(cmd_vel) - current_waypoint = context.course.current_waypoint() - if current_waypoint.type.val == WaypointType.POST: - # if we see the tag in the ZED, go to ApproachPostState - # if context.env.current_target_pos() is not None and context.course.look_for_post(): - # return approach_post.ApproachPostState() - # if we see the tag in the long range camera, go to LongRangeState - if context.env.long_range_tags.get(current_waypoint.tag_id) is not None: - return long_range.LongRangeState() - else: - if context.env.current_target_pos() is not None and context.course.look_for_object(): - return approach_object.ApproachObjectState() # if we see the object + if context.env.current_fid_pos() is not None and context.course.look_for_post(): + return approach_post.ApproachPostState() return self diff --git a/src/navigation/waypoint.py b/src/navigation/waypoint.py index 62181a5c6..0a964dc48 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -3,7 +3,7 @@ from util.ros_utils import get_rosparam from util.state_lib.state import State -from navigation import search, recovery, approach_post, post_backup, state, approach_object, long_range +from navigation import search, recovery, approach_post, post_backup, state class WaypointState(State): @@ -30,19 +30,13 @@ def on_loop(self, context) -> State: return state.DoneState() # if we are at a post currently (from a previous leg), backup to avoid collision - if context.env.arrived_at_target: - context.env.arrived_at_target = False + if context.env.arrived_at_post: + context.env.arrived_at_post = False return post_backup.PostBackupState() if context.course.look_for_post(): - # if context.env.current_target_pos() is not None: - # return approach_post.ApproachPostState() - # if we see the tag in the long range camera, go to LongRangeState - if context.env.long_range_tags.get(current_waypoint.tag_id) is not None: - return long_range.LongRangeState() - elif context.course.look_for_object(): - if context.env.current_target_pos() is not None: - return approach_object.ApproachObjectState() + if context.env.current_fid_pos() is not None: + return approach_post.ApproachPostState() # Attempt to find the waypoint in the TF tree and drive to it try: @@ -54,14 +48,12 @@ def on_loop(self, context) -> State: self.DRIVE_FWD_THRESH, ) if arrived: - if not context.course.look_for_post() and not context.course.look_for_object(): + if not context.course.look_for_post(): # We finished a regular waypoint, go onto the next one context.course.increment_waypoint() - elif context.course.look_for_post() or context.course.look_for_object(): - # We finished a waypoint associated with a post or mallet, but we have not seen it yet. + else: + # We finished a waypoint associated with a tag id, but we have not seen it yet. return search.SearchState() - # TODO elif looking for water bottle: - # return water_bottle_search.WaterBottleSearchState() if context.rover.stuck: context.rover.previous_state = self diff --git a/src/perception/long_range_cam/long_range_cam.cpp b/src/perception/long_range_cam/long_range_cam.cpp deleted file mode 100644 index 783ffe52d..000000000 --- a/src/perception/long_range_cam/long_range_cam.cpp +++ /dev/null @@ -1,88 +0,0 @@ -#include "long_range_cam.hpp" -#include -#include -#include -#include -#include -#include - -namespace mrover { - - using namespace std::chrono_literals; - - /** - * @brief Load config, open the camera, and start our threads - */ - void LongRangeCamNodelet::onInit() { - try { - mGrabThread = std::jthread(&LongRangeCamNodelet::grabUpdate, this); - } catch (std::exception const& e) { - NODELET_FATAL("Exception while starting: %s", e.what()); - ros::shutdown(); - } - } - - void fillImageMessage(cv::Mat& bgra, sensor_msgs::ImagePtr const& msg) { - assert(msg); - - msg->height = bgra.rows; - msg->width = bgra.cols; - msg->encoding = sensor_msgs::image_encodings::BGR8; - msg->step = bgra.step[0]; - msg->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; - auto* imgPtr = bgra.ptr(0); - size_t size = msg->step * msg->height; - msg->data.resize(size); - std::cout << "data size " << msg->data.size() << std::endl; - std::cout << "size obj " << size << std::endl; - memcpy(msg->data.data(), imgPtr, size); - } - - void LongRangeCamNodelet::grabUpdate() { - // See: http://wiki.ros.org/roscpp/Overview/NodeHandles for public vs. private node handle - // MT means multithreaded - mNh = getMTNodeHandle(); - mPnh = getMTPrivateNodeHandle(); - mCamInfoPub = mNh.advertise("long_range_cam/camera_info", 1); - mImgPub = mNh.advertise("long_range_cam/image", 1); - // While dtor not called - while (ros::ok()) { - cv::VideoCapture mCapture{std::format("v4l2src device=/dev/arducam ! videoconvert ! video/x-raw,width={},height={},format=I420,framerate={}/1 ! appsink", 1920, 1080, 5), cv::CAP_GSTREAMER}; - if (!mCapture.isOpened()) { - throw std::runtime_error("Long range cam failed to open"); - } - cv::Mat frame; - while (true) { - mCapture.read(frame); - if (frame.empty()) { - break; - } - cv::imshow("Sender", frame); - if (mImgPub.getNumSubscribers()) { - auto imgMsg = boost::make_shared(); - cv::Mat bgra; - cv::cvtColor(frame, bgra, cv::COLOR_YUV2BGR_I420); - fillImageMessage(bgra, imgMsg); - imgMsg->header.frame_id = "long_range_cam_frame"; - imgMsg->header.stamp = ros::Time::now(); - imgMsg->header.seq = mGrabUpdateTick; - mImgPub.publish(imgMsg); - } - if (cv::waitKey(1) == 's') { - break; - } - ++mGrabUpdateTick; - } - } - } - - LongRangeCamNodelet::~LongRangeCamNodelet() { - NODELET_INFO("Long range cam node shutting down"); - } - -} // namespace mrover - -#ifdef MROVER_IS_NODELET -#include -PLUGINLIB_EXPORT_CLASS(mrover::LongRangeCamNodelet, nodelet::Nodelet) -#endif diff --git a/src/perception/long_range_cam/long_range_cam.hpp b/src/perception/long_range_cam/long_range_cam.hpp deleted file mode 100644 index 8c8141534..000000000 --- a/src/perception/long_range_cam/long_range_cam.hpp +++ /dev/null @@ -1,38 +0,0 @@ -#pragma once - -#include "pch.hpp" - -namespace mrover { - - class LongRangeCamNodelet : public nodelet::Nodelet { - - private: - cv::VideoCapture mCapture; - - ros::NodeHandle mNh, mPnh; - - ros::Publisher mCamInfoPub; - ros::Publisher mImgPub; - - std::jthread mGrabThread; - std::mutex mSwapMutex; - boost::condition_variable mSwapCv; - std::atomic_bool mIsSwapReady = false; - - LoopProfiler mGrabThreadProfiler{"Long Range Cam Grab"}; - - size_t mGrabUpdateTick = 0; - - void onInit() override; - - public: - LongRangeCamNodelet() = default; - - ~LongRangeCamNodelet() override; - - void grabUpdate(); - }; - - void fillImageMessage(cv::Mat& bgra, sensor_msgs::ImagePtr const& msg); - -} // namespace mrover diff --git a/src/perception/long_range_cam/main.cpp b/src/perception/long_range_cam/main.cpp deleted file mode 100644 index 0e5e833da..000000000 --- a/src/perception/long_range_cam/main.cpp +++ /dev/null @@ -1,23 +0,0 @@ -#include "long_range_cam.hpp" - -#ifdef MROVER_IS_NODELET - -#include -PLUGINLIB_EXPORT_CLASS(mrover::LongRangeCamNodelet, nodelet::Nodelet) - -#else - -int main(int argc, char** argv) { - ros::init(argc, argv, "usb_camera"); - - // Start the ZED Nodelet - nodelet::Loader nodelet; - std::cout << ros::this_node::getName() << std::endl; - nodelet.load(ros::this_node::getName(), "mrover/LongRangeCamNodelet", ros::names::getRemappings(), {}); - - ros::spin(); - - return EXIT_SUCCESS; -} - -#endif \ No newline at end of file diff --git a/src/perception/long_range_cam/pch.hpp b/src/perception/long_range_cam/pch.hpp deleted file mode 100644 index f0c16770d..000000000 --- a/src/perception/long_range_cam/pch.hpp +++ /dev/null @@ -1,32 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp deleted file mode 100644 index ce825d003..000000000 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.cpp +++ /dev/null @@ -1,147 +0,0 @@ -#include "long_range_tag_detector.hpp" -#include "mrover/LongRangeTags.h" -#include - -namespace mrover { - - void LongRangeTagDetectorNodelet::onInit() { - mNh = getMTNodeHandle(); - mPnh = getMTPrivateNodeHandle(); - mDetectorParams = new cv::aruco::DetectorParameters(); - auto defaultDetectorParams = cv::makePtr(); - int dictionaryNumber; - - mPnh.param("publish_images", mPublishImages, true); - mPnh.param("min_hit_count_before_publish", mMinHitCountBeforePublish, 5); - mPnh.param("max_hit_count", mMaxHitCount, 5); - mPnh.param("tag_increment_weight", mTagIncrementWeight, 2); - mPnh.param("tag_decrement_weight", mTagDecrementWeight, 1); - mPnh.param("long_range_fov", mLongRangeFov, 80.0); - - mImgPub = mNh.advertise("long_range_tag_detection", 1); - mLongRangeTagsPub = mNh.advertise("tags", 1); - - // topic: long_range_cam/image - //todo - mImgSub = mNh.subscribe("long_range_image", 1, &LongRangeTagDetectorNodelet::imageCallback, this); - mDictionary = cv::makePtr(cv::aruco::getPredefinedDictionary(dictionaryNumber)); - mServiceEnableDetections = mNh.advertiseService("enable_detections", &LongRangeTagDetectorNodelet::enableDetectionsCallback, this); - - // Lambda handles passing class pointer (implicit first parameter) to configCallback - mCallbackType = [this](mrover::DetectorParamsConfig& config, uint32_t level) { configCallback(config, level); }; - mConfigServer.setCallback(mCallbackType); - - mPnh.param("adaptiveThreshConstant", - mDetectorParams->adaptiveThreshConstant, defaultDetectorParams->adaptiveThreshConstant); - mPnh.param("adaptiveThreshWinSizeMax", - mDetectorParams->adaptiveThreshWinSizeMax, defaultDetectorParams->adaptiveThreshWinSizeMax); - mPnh.param("adaptiveThreshWinSizeMin", - mDetectorParams->adaptiveThreshWinSizeMin, defaultDetectorParams->adaptiveThreshWinSizeMin); - mPnh.param("adaptiveThreshWinSizeStep", - mDetectorParams->adaptiveThreshWinSizeStep, defaultDetectorParams->adaptiveThreshWinSizeStep); - mPnh.param("cornerRefinementMaxIterations", - mDetectorParams->cornerRefinementMaxIterations, - defaultDetectorParams->cornerRefinementMaxIterations); - mPnh.param("cornerRefinementMinAccuracy", - mDetectorParams->cornerRefinementMinAccuracy, - defaultDetectorParams->cornerRefinementMinAccuracy); - mPnh.param("cornerRefinementWinSize", - mDetectorParams->cornerRefinementWinSize, defaultDetectorParams->cornerRefinementWinSize); - - bool doCornerRefinement = true; - mPnh.param("doCornerRefinement", doCornerRefinement, true); - if (doCornerRefinement) { - bool cornerRefinementSubPix = true; - mPnh.param("cornerRefinementSubPix", cornerRefinementSubPix, true); - if (cornerRefinementSubPix) { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; - } else { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR; - } - } else { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; - } - - mPnh.param("errorCorrectionRate", - mDetectorParams->errorCorrectionRate, defaultDetectorParams->errorCorrectionRate); - mPnh.param("minCornerDistanceRate", - mDetectorParams->minCornerDistanceRate, defaultDetectorParams->minCornerDistanceRate); - mPnh.param("markerBorderBits", - mDetectorParams->markerBorderBits, defaultDetectorParams->markerBorderBits); - mPnh.param("maxErroneousBitsInBorderRate", - mDetectorParams->maxErroneousBitsInBorderRate, - defaultDetectorParams->maxErroneousBitsInBorderRate); - mPnh.param("minDistanceToBorder", - mDetectorParams->minDistanceToBorder, defaultDetectorParams->minDistanceToBorder); - mPnh.param("minMarkerDistanceRate", - mDetectorParams->minMarkerDistanceRate, defaultDetectorParams->minMarkerDistanceRate); - mPnh.param("minMarkerPerimeterRate", - mDetectorParams->minMarkerPerimeterRate, defaultDetectorParams->minMarkerPerimeterRate); - mPnh.param("maxMarkerPerimeterRate", - mDetectorParams->maxMarkerPerimeterRate, defaultDetectorParams->maxMarkerPerimeterRate); - mPnh.param("minOtsuStdDev", mDetectorParams->minOtsuStdDev, defaultDetectorParams->minOtsuStdDev); - mPnh.param("perspectiveRemoveIgnoredMarginPerCell", - mDetectorParams->perspectiveRemoveIgnoredMarginPerCell, - defaultDetectorParams->perspectiveRemoveIgnoredMarginPerCell); - mPnh.param("perspectiveRemovePixelPerCell", - mDetectorParams->perspectiveRemovePixelPerCell, - defaultDetectorParams->perspectiveRemovePixelPerCell); - mPnh.param("polygonalApproxAccuracyRate", - mDetectorParams->polygonalApproxAccuracyRate, - defaultDetectorParams->polygonalApproxAccuracyRate); - - NODELET_INFO("Tag detection ready, use odom frame: %s, min hit count: %d, max hit count: %d, hit increment weight: %d, hit decrement weight: %d", mUseOdom ? "true" : "false", mMinHitCountBeforePublish, mMaxHitCount, mTagIncrementWeight, mTagDecrementWeight); - } - - void LongRangeTagDetectorNodelet::configCallback(mrover::DetectorParamsConfig& config, uint32_t level) { - // Don't load initial config, since it will overwrite the rosparam settings - if (level == std::numeric_limits::max()) return; - - mDetectorParams->adaptiveThreshConstant = config.adaptiveThreshConstant; - mDetectorParams->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin; - mDetectorParams->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax; - mDetectorParams->adaptiveThreshWinSizeStep = config.adaptiveThreshWinSizeStep; - mDetectorParams->cornerRefinementMaxIterations = config.cornerRefinementMaxIterations; - mDetectorParams->cornerRefinementMinAccuracy = config.cornerRefinementMinAccuracy; - mDetectorParams->cornerRefinementWinSize = config.cornerRefinementWinSize; - if (config.doCornerRefinement) { - if (config.cornerRefinementSubpix) { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; - } else { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR; - } - } else { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; - } - mDetectorParams->errorCorrectionRate = config.errorCorrectionRate; - mDetectorParams->minCornerDistanceRate = config.minCornerDistanceRate; - mDetectorParams->markerBorderBits = config.markerBorderBits; - mDetectorParams->maxErroneousBitsInBorderRate = config.maxErroneousBitsInBorderRate; - mDetectorParams->minDistanceToBorder = config.minDistanceToBorder; - mDetectorParams->minMarkerDistanceRate = config.minMarkerDistanceRate; - mDetectorParams->minMarkerPerimeterRate = config.minMarkerPerimeterRate; - mDetectorParams->maxMarkerPerimeterRate = config.maxMarkerPerimeterRate; - mDetectorParams->minOtsuStdDev = config.minOtsuStdDev; - mDetectorParams->perspectiveRemoveIgnoredMarginPerCell = config.perspectiveRemoveIgnoredMarginPerCell; - mDetectorParams->perspectiveRemovePixelPerCell = config.perspectiveRemovePixelPerCell; - mDetectorParams->polygonalApproxAccuracyRate = config.polygonalApproxAccuracyRate; - } - - bool LongRangeTagDetectorNodelet::enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { - mEnableDetections = req.data; - if (mEnableDetections) { - res.message = "Enabled tag detections."; - NODELET_INFO("Enabled tag detections."); - } else { - res.message = "Disabled tag detections."; - NODELET_INFO("Disabled tag detections."); - } - - res.success = true; - return true; - } - -} // namespace mrover - -#include -PLUGINLIB_EXPORT_CLASS(mrover::LongRangeTagDetectorNodelet, nodelet::Nodelet) diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp deleted file mode 100644 index cb0b4bf3d..000000000 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.hpp +++ /dev/null @@ -1,158 +0,0 @@ -#include "pch.hpp" -#include - -namespace mrover { - - struct LongRangeTagStruct { - bool updated = false; - int id = -1; - int hitCount = 0; - cv::Point2f imageCenter{}; - }; - - class LongRangeTagDetectorNodelet : public nodelet::Nodelet { - ros::NodeHandle mNh, mPnh; - - //Image Subscriber - ros::Subscriber mImgSub; - ros::Publisher mImgPub; - - //Publishes LongRangeTags messages - ros::Publisher mLongRangeTagsPub; - - //Publishing Flags - bool mEnableDetections = true; - // TODO: change this to param - bool mPublishImages = true; // If set, we publish the images with the tags drawn on top - - //Constants set in ROS params for num hits needed to publish - int mMinHitCountBeforePublish{}; - int mBaseHitCount{}; - int mMaxHitCount{}; - int mTagIncrementWeight{}; - int mTagDecrementWeight{}; - int mTagRemoveWeight{}; - float mLongRangeFov{}; - - cv::Ptr mDetectorParams; - cv::Ptr mDictionary; - - //IMAGE VARIABLES - cv::Mat mImg; - sensor_msgs::Image mImgMsg; - - //Raw Tag Data from CV::ARUCO - std::vector> mImmediateCorners; - std::vector mImmediateIds; - - //Map from tagID to Tag - std::unordered_map mTags; - - //TODO - //All these variables are unused by long_range_tag_detector.processing.cpp - //A bunch of things I think I should be able to delete - //But I am not sure about - uint32_t mSeqNum{}; - std::optional mPrevDetectedCount; // Log spam prevention - dynamic_reconfigure::Server mConfigServer; - dynamic_reconfigure::Server::CallbackType mCallbackType; - LoopProfiler mProfiler{"Long Range Tag Detector"}; - std::unordered_map mThreshPubs; // Map from threshold scale to publisher - ros::ServiceServer mServiceEnableDetections; - bool mUseOdom{}; - std::string mOdomFrameId, mMapFrameId, mCameraFrameId; - - void onInit() override; - - /** - * Detects tags in an image, draws the detected markers onto the image, and publishes them to /long_range_tag_detection - * 1. Updates mImg to store the underlying image matrix - * @param msg image message - */ - void imageCallback(sensor_msgs::ImageConstPtr const& msg); - - /** - * Stores msg to mImgMsg - * Creates image matrix from message, stores to mImg - * - * @param msg Current Image Message - */ - void updateImageMatrices(sensor_msgs::ImageConstPtr const& msg); - - /** - * Runs the cv::aruco tag detection on the cv::mat - * Modifies mImmedateCorners and mImmediateIds - * Uses the mDetectorParams and mDictionary member variables - */ - void runTagDetection(); - - /** - * For all the tags stored in mImmediateIds and mImmediateCorners: - * Check to see if they already exist in mTags - * If they do, increment their - */ - void updateHitCounts(); - - /** - * @see updateNewlyIdentifiedTags() - * @param tagId - the tagId of the current tag - * @param tag_bearing - Bearing of the current tag - * @return a new LongRangeTag - */ - LongRangeTagStruct createLrt(int tagId, std::vector& tagCorners); - - /** - * @see getNormedTagCenter - * Assumes that mImg exists - * @return a cv::Point2f that contains the centerx and centery - * centerx and centery are -0.5, 0.5, and reflect a normalized offset from the image center - * Average all x values - */ - cv::Point2f static getTagCenterPixels(std::vector& tagCorners); - - /** - * Assumes upper left corner is 0, 0 - * Want to return a negative pixel offset if width less than image_width / 2 - * similarily, negative if height > image_height / 2 - * @param tagCorners reference to tag corners, passed to @see getTagCenterPixesl - * @return Point2f of pixel offset from center - */ - cv::Point2f getTagCenterOffsetPixels(std::vector& tagCorners) const; - - cv::Point2f getNormedTagCenterOffset(std::vector& tagCorners) const; - - /** - * Given the known tag information and the ZED FOV (hard code for now, we'll - * assign params later), calculate relative bearing of a detected tag - * @param tagCorners reference to tag corners, passed to @see getTagCenterPixels - * @return float of tag bearing - */ - float getTagBearing(cv::Point2f& tagCenter) const; - - /** - * @see updateHitCounts() - * Helper function used in updateHitCounts - * Creates a new LongRangeTag for the identified tags and handles - * logic of adding it to the map - * @param tagIndex the index i of the target tag in the mImmediate vectors - */ - void updateNewlyIdentifiedTags(size_t tagIndex); - - - /** - * Publish the tags which have been detected for more than - * mMinHitCountBeforePublish - */ - void publishPermanentTags(); - - /** - * publishes the thresholded tags onto an image using OpenCV - * only if mPublishImages and the topic has a subscriber - */ - void publishTagsOnImage(); - - void configCallback(mrover::DetectorParamsConfig& config, uint32_t level); - - bool enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); - }; -}; // namespace mrover diff --git a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp deleted file mode 100644 index 6f76b94cc..000000000 --- a/src/perception/tag_detector/long_range_cam/long_range_tag_detector.processing.cpp +++ /dev/null @@ -1,214 +0,0 @@ -#include "long_range_tag_detector.hpp" -#include -#include -#include - -#include - -namespace mrover { - - /** - * Detects tags in an image, draws the detected markers onto the image, and publishes them to /long_range_tag_detection - * - * @param msg image message - */ - void LongRangeTagDetectorNodelet::imageCallback(sensor_msgs::ImageConstPtr const& msg) { - if (!mEnableDetections) return; - - //Store image contents to member variables - updateImageMatrices(msg); - - // 1. Detect tags - runTagDetection(); - - // 2. Update the hit counts of the tags in the mTags map - updateHitCounts(); - - // 3. We only want to publish the tags if the topic has subscribers - if (mPublishImages && mImgPub.getNumSubscribers()) { - // Draw the tags on the image using OpenCV - publishTagsOnImage(); - } - - //Publish all tags that meet threshold - publishPermanentTags(); - - //PUBLISH TAGS - } - - //HELPER FUNCTIONS - - void LongRangeTagDetectorNodelet::updateImageMatrices(sensor_msgs::ImageConstPtr const& msg) { - assert(msg); - assert(msg->height > 0); - assert(msg->width > 0); - - // TODO: Modify when going back to real cam - cv::Mat cvImage_C4 = cv::Mat{static_cast(msg->height), static_cast(msg->width), CV_8UC4, const_cast(msg->data.data())}; - cv::Mat cvImage = cv::Mat(); - cv::cvtColor(cvImage_C4, cvImage, cv::COLOR_BGRA2BGR); - - //Store to mImg member variable - unsure if necessary but need it to persist - cvImage.copyTo(mImg); - - publishTagsOnImage(); - // TODO: Set the grayImage if neccesary - } - - void LongRangeTagDetectorNodelet::runTagDetection() { - // std::cout << mDetectorParams->adaptiveThreshConstant << std::endl; - cv::aruco::detectMarkers(mImg, mDictionary, mImmediateCorners, mImmediateIds, mDetectorParams); - } - - - void LongRangeTagDetectorNodelet::updateHitCounts() { - //loop through all identified IDs - for (size_t i = 0; i < mImmediateIds.size(); i++) { - updateNewlyIdentifiedTags(i); - cv::Point2f center = getTagCenterPixels(mImmediateCorners[i]); - // std::cout << "bearing: " << getTagBearing(center) << "!!!" << std::endl; - } - - //Now decrement all the hitcounts for tags that were not updated - // Set updated status to false - for (auto it = mTags.begin(); it != mTags.end();) { - LongRangeTagStruct& currentTag = it->second; - if (currentTag.updated) { - currentTag.updated = false; - it++; - } else { - //Decrement weight of undetected tags - currentTag.hitCount -= mTagDecrementWeight; - - //if the value has fallen belown the minimum, remove it - if (currentTag.hitCount <= mTagRemoveWeight) { - it = mTags.erase(it); - } else { - it++; - } - } - } - - - //decrement non updated & set updated status to false - } - - LongRangeTagStruct LongRangeTagDetectorNodelet::createLrt(int tagId, std::vector& tagCorners) { - //TODO: Modify the LRT struct and this function to assign a bearing instead of an imageCenter - LongRangeTagStruct lrt; - - lrt.hitCount = mBaseHitCount; //Start at base hit count value - lrt.id = tagId; - lrt.updated = true; - - lrt.imageCenter = getNormedTagCenterOffset(tagCorners); - std::cout << "lrt image center " << lrt.imageCenter.x << std::endl; - - return lrt; - } - - void LongRangeTagDetectorNodelet::updateNewlyIdentifiedTags(size_t tagIndex) { - int currentId = mImmediateIds[tagIndex]; - - //Create new struct for each tag - LongRangeTagStruct lrt = createLrt(currentId, mImmediateCorners[tagIndex]); - - //Check if the tag was already detected and update hitCount to reflect - if (mTags.contains(currentId)) { - //Key exist sin mTags - lrt.hitCount = std::min(mTags[currentId].hitCount + mTagIncrementWeight, mMaxHitCount); - } - mTags[currentId] = lrt; - } - - cv::Point2f LongRangeTagDetectorNodelet::getTagCenterPixels(std::vector& tagCorners) { - cv::Point2f centerPoint; - float centerX = 0; - float centerY = 0; - - for (size_t i = 0; i < 4; i++) { - centerX += tagCorners[i].x; - centerY += tagCorners[i].y; - } - - centerX /= 4.0; - centerY /= 4.0; - - centerPoint.x = centerX; - centerPoint.y = centerY; - - return centerPoint; - } - - cv::Point2f LongRangeTagDetectorNodelet::getTagCenterOffsetPixels(std::vector& tagCorners) const { - cv::Point2f centerPoint = getTagCenterPixels(tagCorners); - - centerPoint.x -= static_cast(mImgMsg.width); - - //-1 is necessary because of 0,0 being in the top left - centerPoint.y = static_cast(-1.0) * (centerPoint.y - static_cast(mImgMsg.height)); - - return centerPoint; - } - - cv::Point2f LongRangeTagDetectorNodelet::getNormedTagCenterOffset(std::vector& tagCorners) const { - cv::Point2f offsetCenterPoint = getTagCenterOffsetPixels(tagCorners); - - offsetCenterPoint.x /= static_cast(mImgMsg.width); - offsetCenterPoint.y /= static_cast(mImgMsg.height); - - return offsetCenterPoint; - } - - float LongRangeTagDetectorNodelet::getTagBearing(cv::Point2f& tagCenter) const { - //for HD720 resolution - auto imageWidth = (float) mImgMsg.width; - std::cout << "width: " << imageWidth << " tag center x: " << tagCenter.x << std::endl; - float bearing = -1 * ((float) tagCenter.x + 0.5) * mLongRangeFov; - std::cout << "bearing: " << bearing << std::endl; - return bearing; - } - - void LongRangeTagDetectorNodelet::publishPermanentTags() { - //Loop through all the tagsj - LongRangeTags tagsMessage; // - - for (auto& tag: mTags) { - if (tag.second.hitCount >= mMinHitCountBeforePublish) { - //LongRangeTag message - LongRangeTag newTagMessage; - - //Fill in fields - newTagMessage.id = tag.second.id; - newTagMessage.bearing = getTagBearing(tag.second.imageCenter); - //Add to back of tagsMessage - tagsMessage.longRangeTags.push_back(newTagMessage); - } - } - - //tagsMessage should be a vector of LongRangeTag messages - //Need something like an mTagsPublisher - mLongRangeTagsPub.publish(tagsMessage); - } - - void LongRangeTagDetectorNodelet::publishTagsOnImage() { - // cv::Mat markedImage; - // markedImage.copyTo(mImg); - // std::cout << markedImage.total() << ", " << markedImage.channels() << std::endl; - - cv::aruco::drawDetectedMarkers(mImg, mImmediateCorners, mImmediateIds); - mImgMsg.header.seq = mSeqNum; - mImgMsg.header.stamp = ros::Time::now(); - mImgMsg.header.frame_id = "long_range_cam_frame"; - mImgMsg.height = mImg.rows; - mImgMsg.width = mImg.cols; - mImgMsg.encoding = sensor_msgs::image_encodings::BGR8; - mImgMsg.step = mImg.step; - mImgMsg.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; - size_t size = mImgMsg.step * mImgMsg.height; - mImgMsg.data.resize(size); - std::uninitialized_copy(std::execution::par_unseq, mImg.data, mImg.data + size, mImgMsg.data.begin()); - mImgPub.publish(mImgMsg); - } - -} // namespace mrover \ No newline at end of file diff --git a/src/perception/tag_detector/long_range_cam/main.cpp b/src/perception/tag_detector/long_range_cam/main.cpp deleted file mode 100644 index d62371420..000000000 --- a/src/perception/tag_detector/long_range_cam/main.cpp +++ /dev/null @@ -1,22 +0,0 @@ -#include "long_range_tag_detector.hpp" - -#ifdef MROVER_IS_NODELET - -#include -PLUGINLIB_EXPORT_CLASS(mrover::LongRangeTagDetectorNodelet, nodelet::Nodelet) - -#else - -int main(int argc, char** argv) { - ros::init(argc, argv, "long_range_tag_detector"); - - // Start the long range cam Nodelet - nodelet::Loader nodelet; - nodelet.load(ros::this_node::getName(), "mrover/LongRangeTagDetectorNodelet", ros::names::getRemappings(), {}); - - ros::spin(); - - return EXIT_SUCCESS; -} - -#endif \ No newline at end of file diff --git a/src/perception/tag_detector/long_range_cam/pch.hpp b/src/perception/tag_detector/long_range_cam/pch.hpp deleted file mode 100644 index afaebe976..000000000 --- a/src/perception/tag_detector/long_range_cam/pch.hpp +++ /dev/null @@ -1,35 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include From 33cc95fc07f2e81acada82d08148bde8bdcc15a7 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 8 Feb 2024 12:11:47 -0500 Subject: [PATCH 156/197] Mostly style changes --- CMakeLists.txt | 10 +- launch/perception.launch | 16 +-- launch/zed_test.launch | 2 +- package.xml | 15 +-- plugins/usb_camera_plugin.xml | 6 - src/perception/object_detector/inference.cu | 46 +++---- src/perception/object_detector/inference.cuh | 36 ++---- .../object_detector/inference_wrapper.cu | 16 +-- .../object_detector/inference_wrapper.hpp | 14 +-- src/perception/object_detector/logger.cu | 2 +- src/perception/object_detector/logger.cuh | 5 +- src/perception/object_detector/main.cpp | 2 +- .../object_detector/object_detector.cpp | 16 +-- .../object_detector/object_detector.hpp | 74 +++++------ .../object_detector.processing.cpp | 115 ++++++++---------- src/perception/object_detector/pch.hpp | 10 +- src/perception/tag_detector/zed/pch.hpp | 1 - src/preload/format | 2 +- .../autonomy/AutonomyStarterProject.cmake | 2 +- starter_project/autonomy/src/perception.hpp | 1 - 20 files changed, 156 insertions(+), 235 deletions(-) delete mode 100644 plugins/usb_camera_plugin.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index 1aed42b0e..21ddaa86a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -184,7 +184,7 @@ endif () ## Perception mrover_add_nodelet(zed_tag_detector src/perception/tag_detector/zed/*.cpp src/perception/tag_detector/zed src/perception/tag_detector/zed/pch.hpp) -mrover_nodelet_link_libraries(zed_tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc tbb lie) +mrover_nodelet_link_libraries(zed_tag_detector opencv_core opencv_aruco opencv_imgproc tbb lie) if (CUDA_FOUND) mrover_add_library(streaming src/esw/streaming/*.c* src/esw/streaming) @@ -195,15 +195,14 @@ if (CUDA_FOUND) endif () if (ZED_FOUND) - # object_detector mrover_add_nodelet(object_detector src/perception/object_detector/*.c* src/perception/object_detector src/perception/object_detector/pch.hpp) mrover_nodelet_link_libraries(object_detector PRIVATE opencv_core opencv_dnn opencv_imgproc lie nvinfer nvonnxparser tbb) - # Temporary - mrover_nodelet_link_libraries(object_detector PRIVATE opencv_imgcodecs) - mrover_nodelet_link_libraries(object_detector PRIVATE opencv_highgui) mrover_nodelet_defines(object_detector __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__ # Eigen includes some files it should not, ignore ) +# set_target_properties(object_detector_nodelet PROPERTIES CXX_STANDARD 11) + # Temporary + mrover_nodelet_link_libraries(object_detector PRIVATE opencv_imgcodecs opencv_highgui) mrover_add_nodelet(zed src/perception/zed_wrapper/*.c* src/perception/zed_wrapper src/perception/zed_wrapper/pch.hpp) mrover_nodelet_include_directories(zed ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) @@ -211,7 +210,6 @@ if (ZED_FOUND) mrover_nodelet_defines(zed ALLOW_BUILD_DEBUG # Ignore ZED warnings about Debug mode __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__ # Eigen includes some files it should not, ignore - ) endif () diff --git a/launch/perception.launch b/launch/perception.launch index 6fd430737..af742d42d 100644 --- a/launch/perception.launch +++ b/launch/perception.launch @@ -3,19 +3,15 @@ - + args="load mrover/TagDetectorNodelet nodelet_manager" output="screen" /> - - - - - \ No newline at end of file + args="load mrover/ObjectDetectorNodelet perception_nodelet_manager" output="screen" /> + diff --git a/launch/zed_test.launch b/launch/zed_test.launch index 1e4590589..4b78462b8 100644 --- a/launch/zed_test.launch +++ b/launch/zed_test.launch @@ -35,4 +35,4 @@ - \ No newline at end of file + diff --git a/package.xml b/package.xml index 70892f6c2..1b92c62fe 100644 --- a/package.xml +++ b/package.xml @@ -79,13 +79,10 @@ - - - - - - - - + + + + + - \ No newline at end of file + diff --git a/plugins/usb_camera_plugin.xml b/plugins/usb_camera_plugin.xml deleted file mode 100644 index 98d1f99e2..000000000 --- a/plugins/usb_camera_plugin.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - \ No newline at end of file diff --git a/src/perception/object_detector/inference.cu b/src/perception/object_detector/inference.cu index 27f5a3894..f09fb0557 100644 --- a/src/perception/object_detector/inference.cu +++ b/src/perception/object_detector/inference.cu @@ -1,7 +1,9 @@ #include "inference.cuh" - using namespace nvinfer1; + +#include + /** * cudaMemcpys CPU memory in inputTensor to GPU based on bindings * Queues that tensor to be passed through model @@ -11,34 +13,32 @@ using namespace nvinfer1; */ namespace mrover { - //Create names for common model names constexpr static char const* INPUT_BINDING_NAME = "images"; constexpr static char const* OUTPUT_BINDING_NAME = "output0"; Inference::Inference(std::filesystem::path const& onnxModelPath) { - //Create the engine object from either the file or from onnx file + // Create the engine object from either the file or from onnx file mEngine = std::unique_ptr{createCudaEngine(onnxModelPath)}; if (!mEngine) throw std::runtime_error("Failed to create CUDA engine"); mLogger.log(ILogger::Severity::kINFO, "Created CUDA Engine"); - //Check some assumptions about the model + // Check some assumptions about the model if (mEngine->getNbIOTensors() != 2) throw std::runtime_error("Invalid Binding Count"); if (mEngine->getTensorIOMode(INPUT_BINDING_NAME) != TensorIOMode::kINPUT) throw std::runtime_error("Expected Input Binding 0 Is An Input"); if (mEngine->getTensorIOMode(OUTPUT_BINDING_NAME) != TensorIOMode::kOUTPUT) throw std::runtime_error("Expected Input Binding Input To Be 1"); createExecutionContext(); - //Init the io tensors on the GPU prepTensors(); } - ICudaEngine* Inference::createCudaEngine(std::filesystem::path const& onnxModelPath) { + auto Inference::createCudaEngine(std::filesystem::path const& onnxModelPath) -> ICudaEngine* { - mModelPath = {onnxModelPath.c_str()}; + mModelPath = onnxModelPath; //Define the size of Batches - constexpr auto explicitBatch = 1U << static_cast(NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + constexpr auto explicitBatch = 1U << static_cast(NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); //Init logger std::unique_ptr builder{createInferBuilder(mLogger)}; @@ -104,7 +104,7 @@ namespace mrover { return runtime->deserializeCudaEngine(enginePlan.data(), enginePlan.size()); } - void Inference::createExecutionContext() { + auto Inference::createExecutionContext() -> void { // Create Execution Context. mContext.reset(mEngine->createExecutionContext()); @@ -112,18 +112,17 @@ namespace mrover { mContext->setInputShape(INPUT_BINDING_NAME, mEngine->getTensorShape(INPUT_BINDING_NAME)); } - void Inference::doDetections(cv::Mat const& img) const { + auto Inference::doDetections(cv::Mat const& img) const -> void { //Do the forward pass on the network launchInference(img, mOutputTensor); } - cv::Mat Inference::getOutputTensor() { + auto Inference::getOutputTensor() -> cv::Mat { //Returns the output tensor return mOutputTensor; } - - void Inference::launchInference(cv::Mat const& input, cv::Mat const& output) const { + auto Inference::launchInference(cv::Mat const& input, cv::Mat const& output) const -> void { //Assert these items have been initialized assert(!input.empty()); assert(!output.empty()); @@ -144,9 +143,8 @@ namespace mrover { cudaMemcpy(output.data, mBindings[1 - inputId], output.total() * output.elemSize(), cudaMemcpyDeviceToHost); } - - void Inference::prepTensors() { - //Assign the properties to the input and output tensors + auto Inference::prepTensors() -> void { + // Assign the properties to the input and output tensors for (int i = 0; i < mEngine->getNbIOTensors(); i++) { char const* tensorName = mEngine->getIOTensorName(i); auto [rank, extents] = mEngine->getTensorShape(tensorName); @@ -159,20 +157,22 @@ namespace mrover { } assert(mContext); - //Create an appropriately sized output tensor + // Create an appropriately sized output tensor Dims const outputTensorDims = mEngine->getTensorShape(OUTPUT_BINDING_NAME); for (int i = 0; i < outputTensorDims.nbDims; i++) { char message[512]; - std::snprintf(message, sizeof(message), "size %d %d", i, outputTensorDims.d[i]); - mLogger.log(nvinfer1::ILogger::Severity::kINFO, message); + std::snprintf(message, sizeof(message), "Size %d %d", i, outputTensorDims.d[i]); + mLogger.log(ILogger::Severity::kINFO, message); } - //Create the mat wrapper around the output matrix for ease of use - mOutputTensor = cv::Mat::zeros(outputTensorDims.d[1], outputTensorDims.d[2], CV_MAKE_TYPE(CV_32F, 1)); + // Create the mat wrapper around the output matrix for ease of use + assert(outputTensorDims.nbDims == 3); + assert(outputTensorDims.d[0] == 1); + mOutputTensor = cv::Mat::zeros(outputTensorDims.d[1], outputTensorDims.d[2], CV_32FC1); } - int Inference::getBindingInputIndex(IExecutionContext const* context) { + auto Inference::getBindingInputIndex(IExecutionContext const* context) -> int { //Returns the id for the input tensor return context->getEngine().getTensorIOMode(context->getEngine().getIOTensorName(0)) != TensorIOMode::kINPUT; // 0 (false) if bindingIsInput(0), 1 (true) otherwise } -} // namespace mrover +} // namespace mrover \ No newline at end of file diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index 8363ac8f4..55de3edb8 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -1,14 +1,10 @@ #pragma once -#include "logger.cuh" #include "pch.hpp" +#include "logger.cuh" + #include -#include -#include -#include -#include -#include using nvinfer1::ICudaEngine; using nvinfer1::IExecutionContext; @@ -16,38 +12,27 @@ using nvinfer1::IExecutionContext; namespace mrover { class Inference { - private: - //Model Path std::string mModelPath; - //Init Logger nvinfer1::Logger mLogger; - //Ptr to the engine std::unique_ptr mEngine{}; - //Ptr to the context std::unique_ptr mContext{}; - //Input, output and reference tensors cv::Mat mInputTensor; cv::Mat mOutputTensor; - //Bindings std::array mBindings{}; - //Size of Model cv::Size mModelInputShape; cv::Size mModelOutputShape; - //STATIC FUNCTIONS static auto getBindingInputIndex(IExecutionContext const* context) -> int; - //Creates a ptr to the engine auto createCudaEngine(std::filesystem::path const& onnxModelPath) -> ICudaEngine*; - //Launch the model execution onto the GPU - void launchInference(cv::Mat const& input, cv::Mat const& output) const; + auto launchInference(cv::Mat const& input, cv::Mat const& output) const -> void; /** * @brief Prepares the tensors for inference. @@ -58,22 +43,19 @@ namespace mrover { * Requires enginePtr, bindings, inputTensor, and outputTensor * Modifies bindings, inputTensor, and outputTensor */ - void prepTensors(); + auto prepTensors() -> void; /** * @brief Creates the execution context for the model */ - void createExecutionContext(); + auto createExecutionContext() -> void; public: - //Inference Constructor - Inference(std::filesystem::path const& onnxModelPath); + explicit Inference(std::filesystem::path const& onnxModelPath); - //Forward Pass of the model - void doDetections(cv::Mat const& img) const; + auto doDetections(cv::Mat const& img) const -> void; - //Get the output tensor with in a YOLO v8 style data structure - cv::Mat getOutputTensor(); + auto getOutputTensor() -> cv::Mat; }; -} // namespace mrover +} // namespace mrover \ No newline at end of file diff --git a/src/perception/object_detector/inference_wrapper.cu b/src/perception/object_detector/inference_wrapper.cu index 71cbb91b2..39fb1f9b3 100644 --- a/src/perception/object_detector/inference_wrapper.cu +++ b/src/perception/object_detector/inference_wrapper.cu @@ -1,13 +1,9 @@ #include "inference_wrapper.hpp" -#include - #include "inference.cuh" using namespace nvinfer1; - - /** * cudaMemcpys CPU memory in inputTensor to GPU based on bindings * Queues that tensor to be passed through model @@ -17,19 +13,17 @@ using namespace nvinfer1; */ namespace mrover { - InferenceWrapper::InferenceWrapper(std::string onnxModelPath) : mInference({}) { - //Initialize the unique_ptr to the inference class + InferenceWrapper::InferenceWrapper(std::string onnxModelPath) { mInference.reset(new Inference(std::move(onnxModelPath))); } - - void InferenceWrapper::doDetections(const cv::Mat& img) const { - //Execute the forward pass on the inference object + auto InferenceWrapper::doDetections(const cv::Mat& img) const -> void { + // Execute the forward pass on the inference object mInference->doDetections(img); } - cv::Mat InferenceWrapper::getOutputTensor() const { + auto InferenceWrapper::getOutputTensor() const -> cv::Mat { return mInference->getOutputTensor(); } -} // namespace mrover +} // namespace mrover \ No newline at end of file diff --git a/src/perception/object_detector/inference_wrapper.hpp b/src/perception/object_detector/inference_wrapper.hpp index 80683df6c..1ef593e47 100644 --- a/src/perception/object_detector/inference_wrapper.hpp +++ b/src/perception/object_detector/inference_wrapper.hpp @@ -3,27 +3,23 @@ #include "pch.hpp" namespace mrover { + class Inference; class InferenceWrapper { - //Ptr to the inference std::shared_ptr mInference; public: - //Default Constructor for the Wrapper InferenceWrapper() = default; - //Deconstructor for the Wrapper ~InferenceWrapper() = default; - //Inference Wrapper Constructor - InferenceWrapper(std::string onnxModelPath); + explicit InferenceWrapper(std::string onnxModelPath); - //Forward Pass on the model - void doDetections(cv::Mat const& img) const; + auto doDetections(cv::Mat const& img) const -> void; - //Retrieve the output tensor from the previous forward pass - cv::Mat getOutputTensor() const; + // Retrieve the output tensor from the previous forward pass + [[nodiscard]] auto getOutputTensor() const -> cv::Mat; }; } // namespace mrover diff --git a/src/perception/object_detector/logger.cu b/src/perception/object_detector/logger.cu index 622bb8786..d16796040 100644 --- a/src/perception/object_detector/logger.cu +++ b/src/perception/object_detector/logger.cu @@ -22,4 +22,4 @@ namespace nvinfer1 { } } -} // namespace nvinfer1 +} // namespace nvinfer1 \ No newline at end of file diff --git a/src/perception/object_detector/logger.cuh b/src/perception/object_detector/logger.cuh index 7467a8000..25b478a0d 100644 --- a/src/perception/object_detector/logger.cuh +++ b/src/perception/object_detector/logger.cuh @@ -1,13 +1,14 @@ #pragma once #include "pch.hpp" + #include namespace nvinfer1 { class Logger final : public ILogger { public: - void log(Severity severity, char const* msg) noexcept override; + auto log(Severity severity, char const* msg) noexcept -> void override; }; template @@ -17,4 +18,4 @@ namespace nvinfer1 { } }; -} // namespace nvinfer1 +} // namespace nvinfer1 \ No newline at end of file diff --git a/src/perception/object_detector/main.cpp b/src/perception/object_detector/main.cpp index 729f6caf2..294d43662 100644 --- a/src/perception/object_detector/main.cpp +++ b/src/perception/object_detector/main.cpp @@ -15,4 +15,4 @@ auto main(int argc, char** argv) -> int { #ifdef MROVER_IS_NODELET #include PLUGINLIB_EXPORT_CLASS(mrover::ObjectDetectorNodelet, nodelet::Nodelet) -#endif +#endif \ No newline at end of file diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 647559c70..b89230d4b 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -1,13 +1,9 @@ #include "object_detector.hpp" #include "inference_wrapper.hpp" -#include "pch.hpp" -#include -#include namespace mrover { void ObjectDetectorNodelet::onInit() { - //Get the Handlers mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); @@ -16,25 +12,17 @@ namespace mrover { mInferenceWrapper = InferenceWrapper{modelPath}; - //Create the publishers and subscribers for the detected image and the debug image mImgSub = mNh.subscribe("/camera/left/points", 1, &ObjectDetectorNodelet::imageCallback, this); mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); - //Create the Reference Frames mNh.param("camera_frame", mCameraFrameId, "zed2i_left_camera_frame"); mNh.param("world_frame", mMapFrameId, "map"); - //Read ROS Params mNh.param("obj_increment_weight", mObjIncrementWeight, 2); mNh.param("obj_decrement_weight", mObjDecrementWeight, 1); mNh.param("obj_hitcount_threshold", mObjHitThreshold, 5); mNh.param("obj_hitcount_max", mObjMaxHitcount, 10); mNh.param("model_name", mModelName, "yolov8n_mallet_bottle_better"); - - - //Initialize Object Hit Cout to Zeros - mObjectHitCounts = {0, 0}; - - mEnableLoopProfiler = true; } -} // namespace mrover + +} // namespace mrover \ No newline at end of file diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 7130ff961..cafddba6c 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -1,96 +1,80 @@ #pragma once #include "inference_wrapper.hpp" -#include "pch.hpp" -#include -#include -#include -#include - -namespace mrover { +namespace mrover +{ //Data type for detection - struct Detection { - int class_id{0}; + struct Detection + { + int classId{0}; std::string className; float confidence{0.0}; - cv::Rect box{}; + cv::Rect box; }; - class ObjectDetectorNodelet : public nodelet::Nodelet { - - //Model Name for Object Detector + class ObjectDetectorNodelet : public nodelet::Nodelet + { std::string mModelName; - //Loop Profiler LoopProfiler mLoopProfiler{"Object Detector", 1}; - bool mEnableLoopProfiler; + bool mEnableLoopProfiler = true; - //Mat for the image from the point cloud + // Mat for the image from the point cloud cv::Mat mImg; - //List of class names - std::vector classes{"Bottle", "Hammer"}; // {"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; + std::vector classes{"Bottle", "Hammer"}; - //ROS Handlers ros::NodeHandle mNh, mPnh; - //Inference inference; InferenceWrapper mInferenceWrapper; - // Publishers ros::Publisher mDebugImgPub; - // Subscribers ros::Subscriber mImgSub; - // Preallocated cv::Mats cv::Mat mImageBlob; dynamic_reconfigure::Server mConfigServer; dynamic_reconfigure::Server::CallbackType mCallbackType; - // Internal state cv::dnn::Net mNet; - //TF Member Variables tf2_ros::Buffer mTfBuffer; tf2_ros::TransformListener mTfListener{mTfBuffer}; tf2_ros::TransformBroadcaster mTfBroadcaster; std::string mCameraFrameId; std::string mMapFrameId; - //Hit counter - std::vector mObjectHitCounts; + std::vector mObjectHitCounts{0, 0}; - //Constants after initialization - int mObjIncrementWeight; - int mObjDecrementWeight; - int mObjHitThreshold; - int mObjMaxHitcount; + int mObjIncrementWeight{}; + int mObjDecrementWeight{}; + int mObjHitThreshold{}; + int mObjMaxHitcount{}; - //Init method - void onInit() override; + auto onInit() -> void override; - //Function to get SE3 from the point cloud - auto getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v, size_t width, size_t height) -> std::optional; + auto getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, + size_t u, size_t v, size_t width, size_t height) -> std::optional; - auto spiralSearchInImg(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t xCenter, size_t yCenter, size_t width, size_t height) -> std::optional; + auto spiralSearchInImg(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, + size_t xCenter, size_t yCenter, size_t width, size_t height) -> std::optional; - static void convertPointCloudToRGBA(sensor_msgs::PointCloud2ConstPtr const& msg, cv::Mat& img); + static auto convertPointCloudToRGBA(sensor_msgs::PointCloud2ConstPtr const& msg, cv::Mat& img) -> void; - void updateHitsObject(sensor_msgs::PointCloud2ConstPtr const& msg, Detection const& detection, std::vector& seenObjects, cv::Size const& imgSize = {640, 640}); + auto updateHitsObject(sensor_msgs::PointCloud2ConstPtr const& msg, + Detection const& detection, std::vector& seenObjects, + cv::Size const& imgSize = {640, 640}) -> void; - void publishImg(cv::Mat const& img); + auto publishImg(cv::Mat const& img) -> void; public: - //Node constructor ObjectDetectorNodelet() = default; - //Node Deconstructor ~ObjectDetectorNodelet() override = default; - //Callback for when ZED publishes data - void imageCallback(sensor_msgs::PointCloud2ConstPtr const& msg); + auto imageCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void; }; -} // namespace mrover \ No newline at end of file + +} // namespace mrover diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 1a6516730..8f9872613 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -1,21 +1,12 @@ #include "object_detector.hpp" -#include "../point.hpp" +#include "point.hpp" #include "inference_wrapper.hpp" -#include -#include -#include -#include -#include -#include -#include -#include namespace mrover { - void ObjectDetectorNodelet::imageCallback(sensor_msgs::PointCloud2ConstPtr const& msg) { + auto ObjectDetectorNodelet::imageCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void { - // if (mEnableLoopProfiler) { if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { ros::console::notifyLoggerLevelsChanged(); @@ -24,46 +15,43 @@ namespace mrover { mLoopProfiler.measureEvent("Wait"); } - //Does the msg exist with a height and width assert(msg); assert(msg->height > 0); assert(msg->width > 0); - //Adjust the picture size to be in line with the expected img size form the Point Cloud + // Adjust the picture size to be in line with the expected img size from the Point Cloud if (static_cast(msg->height) != mImg.rows || static_cast(msg->width) != mImg.cols) { NODELET_INFO("Image size changed from [%d %d] to [%u %u]", mImg.cols, mImg.rows, msg->width, msg->height); mImg = cv::Mat{static_cast(msg->height), static_cast(msg->width), CV_8UC4, cv::Scalar{0, 0, 0, 0}}; } - //DONE - //Convert the pointcloud data into rgba image and store in mImg + // Convert the pointcloud data into rgba image and store in mImg convertPointCloudToRGBA(msg, mImg); - - //Resize the image and change it from BGRA to BGR + // Resize the image and change it from BGRA to BGR cv::Mat sizedImage; cv::Size imgSize{640, 640}; cv::resize(mImg, sizedImage, imgSize); cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGRA2BGR); - //Create the blob from the resized image + // Create the blob from the resized image cv::dnn::blobFromImage(sizedImage, mImageBlob, 1.0 / 255.0, imgSize, cv::Scalar(), true, false); if (mEnableLoopProfiler) { mLoopProfiler.measureEvent("Convert Image"); } - //Run the blob through the model + // Run the blob through the model mInferenceWrapper.doDetections(mImageBlob); - //Retrieve the output from the model + // Retrieve the output from the model cv::Mat output = mInferenceWrapper.getOutputTensor(); if (mEnableLoopProfiler) { mLoopProfiler.measureEvent("Execute on GPU"); } - //Get model specific information + // Get model specific information int rows = output.rows; int dimensions = output.cols; @@ -112,7 +100,7 @@ namespace mrover { double maxClassScore; //Find the max class score for the associated row - minMaxLoc(scores, nullptr, &maxClassScore, nullptr, &class_id); + cv::minMaxLoc(scores, nullptr, &maxClassScore, nullptr, &class_id); //Determine if the class is an acceptable confidence level else disregard if (maxClassScore > modelScoreThreshold) { @@ -140,21 +128,21 @@ namespace mrover { } //Coalesce the boxes into a smaller number of distinct boxes - std::vector nms_result; - cv::dnn::NMSBoxes(boxes, confidences, modelScoreThreshold, modelNMSThreshold, nms_result); + std::vector nmsResult; + cv::dnn::NMSBoxes(boxes, confidences, modelScoreThreshold, modelNMSThreshold, nmsResult); //Storage for the detection from the model std::vector detections{}; - for (int idx: nms_result) { + for (int idx: nmsResult) { //Init the detection Detection result; //Fill in the id and confidence for the class - result.class_id = class_ids[idx]; + result.classId = class_ids[idx]; result.confidence = confidences[idx]; //Fill in the class name and box information - result.className = classes[result.class_id]; + result.className = classes[result.classId]; result.box = boxes[idx]; //Push back the detection into the for storagevector @@ -169,7 +157,7 @@ namespace mrover { //mLoopProfiler.measureEvent("Push to TF START"); } - std::vector seenObjects = {false, false}; + std::vector seenObjects{false, false}; //If there are detections locate them in 3D for (Detection const& detection: detections) { @@ -192,18 +180,18 @@ namespace mrover { } //Draw the detected object's bounding boxes on the image for each of the objects detected - std::vector font_Colors = {cv::Scalar{232, 115, 5}, - cv::Scalar{0, 4, 227}}; - for (size_t i = 0; i < detections.size(); i++) { + std::vector fontColors{cv::Scalar{232, 115, 5}, + cv::Scalar{0, 4, 227}}; + for (std::size_t i = 0; i < detections.size(); i++) { //Font color will change for each different detection - cv::Scalar font_Color = font_Colors.at(detections.at(i).class_id); - cv::rectangle(sizedImage, detections[i].box, font_Color, 1, cv::LINE_8, 0); + cv::Scalar fontColor = fontColors.at(detections.at(i).classId); + cv::rectangle(sizedImage, detections[i].box, fontColor, 1, cv::LINE_8, 0); //Put the text on the image - cv::Point text_position(80, static_cast(80 * (i + 1))); - int font_size = 1; - int font_weight = 2; - putText(sizedImage, detections[i].className, text_position, cv::FONT_HERSHEY_COMPLEX, font_size, font_Color, font_weight); //Putting the text in the matrix// + cv::Point textPosition(80, static_cast(80 * (i + 1))); + constexpr int fontSize = 1; + constexpr int fontWeight = 2; + putText(sizedImage, detections[i].className, textPosition, cv::FONT_HERSHEY_COMPLEX, fontSize, fontColor, fontWeight); //Putting the text in the matrix// } } @@ -238,33 +226,33 @@ namespace mrover { size_t currY = yCenter; size_t radius = 0; int t = 0; - int numPts = 16; + constexpr int numPts = 16; bool isPointInvalid = true; Point point{}; - //Find the smaller of the two box dimensions so we know the max spiral radius + // Find the smaller of the two box dimensions so we know the max spiral radius size_t smallDim = std::min(width / 2, height / 2); - while (isPointInvalid) { - //This is the parametric equation to spiral around the center pnt + // This is the parametric equation to spiral around the center pnt currX = static_cast(static_cast(xCenter) + std::cos(t * 1.0 / numPts * 2 * M_PI) * static_cast(radius)); currY = static_cast(static_cast(yCenter) + std::sin(t * 1.0 / numPts * 2 * M_PI) * static_cast(radius)); - //Grab the point from the pntCloud and determine if its a finite pnt + // Grab the point from the pntCloud and determine if its a finite pnt point = reinterpret_cast(cloudPtr->data.data())[currX + currY * cloudPtr->width]; - isPointInvalid = (!std::isfinite(point.x) || !std::isfinite(point.y) || !std::isfinite(point.z)); - if (isPointInvalid) NODELET_WARN("Tag center point not finite: [%f %f %f]", point.x, point.y, point.z); + isPointInvalid = !std::isfinite(point.x) || !std::isfinite(point.y) || !std::isfinite(point.z); + if (isPointInvalid) + NODELET_WARN("Tag center point not finite: [%f %f %f]", point.x, point.y, point.z); - //After a full circle increase the radius - if (static_cast(t) % numPts == 0) { + // After a full circle increase the radius + if (t % numPts == 0) { radius++; } - //Increase the parameter + // Increase the parameter t++; - //If we reach the edge of the box we stop spiraling + // If we reach the edge of the box we stop spiraling if (radius >= smallDim) { return std::nullopt; } @@ -273,7 +261,7 @@ namespace mrover { return std::make_optional(R3{point.x, point.y, point.z}, SO3{}); } - void ObjectDetectorNodelet::convertPointCloudToRGBA(sensor_msgs::PointCloud2ConstPtr const& msg, cv::Mat& img) { + auto ObjectDetectorNodelet::convertPointCloudToRGBA(sensor_msgs::PointCloud2ConstPtr const& msg, cv::Mat& img) -> void { auto* pixelPtr = reinterpret_cast(img.data); auto* pointPtr = reinterpret_cast(msg->data.data()); std::for_each(std::execution::par_unseq, pixelPtr, pixelPtr + img.total(), [&](cv::Vec4b& pixel) { @@ -285,36 +273,36 @@ namespace mrover { }); } - void ObjectDetectorNodelet::updateHitsObject(sensor_msgs::PointCloud2ConstPtr const& msg, Detection const& detection, std::vector& seenObjects, cv::Size const& imgSize) { + auto ObjectDetectorNodelet::updateHitsObject(sensor_msgs::PointCloud2ConstPtr const& msg, Detection const& detection, std::vector& seenObjects, cv::Size const& imgSize) -> void { cv::Rect box = detection.box; - std::pair center = std::pair(box.x + box.width / 2, box.y + box.height / 2); - //Resize from {640, 640} image space to {720,1280} image space + auto center = std::pair(box.x + box.width / 2, box.y + box.height / 2); + // Resize from {640, 640} image space to {720,1280} image space auto centerWidth = static_cast(center.first * static_cast(msg->width) / imgSize.width); auto centerHeight = static_cast(center.second * static_cast(msg->height) / imgSize.height); std::cout << mObjectHitCounts.at(0) << ", " << mObjectHitCounts.at(1) << std::endl; ROS_INFO("%d, %d", mObjectHitCounts.at(0), mObjectHitCounts.at(1)); - if (!seenObjects.at(detection.class_id)) { - seenObjects.at(detection.class_id) = true; + if (!seenObjects.at(detection.classId)) { + seenObjects.at(detection.classId) = true; //Get the object's position in 3D from the point cloud and run this statement if the optional has a value if (std::optional objectLocation = getObjectInCamFromPixel(msg, centerWidth, centerHeight, box.width, box.height); objectLocation) { try { - std::string immediateFrameId = "immediateDetectedObject" + classes.at(detection.class_id); + std::string immediateFrameId = "immediateDetectedObject" + classes.at(detection.classId); //Push the immediate detections to the zed frame SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, objectLocation.value()); //Since the object is seen we need to increment the hit counter - mObjectHitCounts.at(detection.class_id) = std::min(mObjMaxHitcount, mObjectHitCounts.at(detection.class_id) + mObjIncrementWeight); + mObjectHitCounts.at(detection.classId) = std::min(mObjMaxHitcount, mObjectHitCounts.at(detection.classId) + mObjIncrementWeight); ROS_INFO("PUSHED TO TF TEMP"); //Only publish to permament if we are confident in the object - if (mObjectHitCounts.at(detection.class_id) > mObjHitThreshold) { + if (mObjectHitCounts.at(detection.classId) > mObjHitThreshold) { - std::string permanentFrameId = "detectedObject" + classes.at(detection.class_id); + std::string permanentFrameId = "detectedObject" + classes.at(detection.classId); //Grab the object inside of the camera frame and push it into the map frame @@ -335,10 +323,10 @@ namespace mrover { } } - void ObjectDetectorNodelet::publishImg(cv::Mat const& img) { - sensor_msgs::Image newDebugImageMessage; //I chose regular msg not ptr so it can be used outside of this process + auto ObjectDetectorNodelet::publishImg(cv::Mat const& img) -> void { + sensor_msgs::Image newDebugImageMessage; // I chose regular msg not ptr so it can be used outside of this process - //Convert the image back to BGRA for ROS + // Convert the image back to BGRA for ROS cv::Mat bgraImg; cv::cvtColor(img, bgraImg, cv::COLOR_BGR2BGRA); @@ -355,8 +343,9 @@ namespace mrover { newDebugImageMessage.data.resize(size); //Copy the data to the image - memcpy(newDebugImageMessage.data.data(), imgPtr, size); + std::memcpy(newDebugImageMessage.data.data(), imgPtr, size); mDebugImgPub.publish(newDebugImageMessage); } -} // namespace mrover + +} // namespace mrover \ No newline at end of file diff --git a/src/perception/object_detector/pch.hpp b/src/perception/object_detector/pch.hpp index a36e3e35c..ebc19cb28 100644 --- a/src/perception/object_detector/pch.hpp +++ b/src/perception/object_detector/pch.hpp @@ -17,8 +17,13 @@ #include #include #include +#include +#include +#include +#include +#include -#include +#include #include #include @@ -35,9 +40,8 @@ #include #include -#include -#include #include +#include #include #include diff --git a/src/perception/tag_detector/zed/pch.hpp b/src/perception/tag_detector/zed/pch.hpp index 9fbf79e07..47c69dcae 100644 --- a/src/perception/tag_detector/zed/pch.hpp +++ b/src/perception/tag_detector/zed/pch.hpp @@ -15,7 +15,6 @@ #include #include #include -#include #include #include diff --git a/src/preload/format b/src/preload/format index 3176bfefe..1d8d28997 100644 --- a/src/preload/format +++ b/src/preload/format @@ -3,7 +3,7 @@ // macOS does not have std::format yet // So we define it and redirect it to fmt::format -#ifdef __APPLE__ +#if defined(__APPLE__) #include namespace std { diff --git a/starter_project/autonomy/AutonomyStarterProject.cmake b/starter_project/autonomy/AutonomyStarterProject.cmake index 13e0d3bc8..e3bdd782b 100644 --- a/starter_project/autonomy/AutonomyStarterProject.cmake +++ b/starter_project/autonomy/AutonomyStarterProject.cmake @@ -18,7 +18,7 @@ add_executable(starter_project_perception ${STARTER_PROJECT_PERCEPTION_SOURCES}) # Ensure that our project builds after message generation add_dependencies(starter_project_perception ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # Link needed libraries -target_link_libraries(starter_project_perception ${catkin_LIBRARIES} opencv_core opencv_objdetect opencv_aruco) +target_link_libraries(starter_project_perception ${catkin_LIBRARIES} opencv_core opencv_aruco) # Include needed directories target_include_directories(starter_project_perception PUBLIC ${catkin_INCLUDE_DIRS}) diff --git a/starter_project/autonomy/src/perception.hpp b/starter_project/autonomy/src/perception.hpp index 467e21a87..22b62f1b3 100644 --- a/starter_project/autonomy/src/perception.hpp +++ b/starter_project/autonomy/src/perception.hpp @@ -10,7 +10,6 @@ // OpenCV Headers, cv namespace #include #include -#include #include // ROS Headers, ros namespace From f127e2263e072f89be8f0fbf4400ae137a2c9908 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 8 Feb 2024 12:16:30 -0500 Subject: [PATCH 157/197] Undo formatting in tag detector --- .../zed/tag_detector.processing.cpp | 110 +++++++----------- 1 file changed, 39 insertions(+), 71 deletions(-) diff --git a/src/perception/tag_detector/zed/tag_detector.processing.cpp b/src/perception/tag_detector/zed/tag_detector.processing.cpp index 9f40c7532..2cc6c0e58 100644 --- a/src/perception/tag_detector/zed/tag_detector.processing.cpp +++ b/src/perception/tag_detector/zed/tag_detector.processing.cpp @@ -5,13 +5,12 @@ namespace mrover { /** - * @brief Retrieve the pose of the tag in camera space - * @param msg 3D Point Cloud with points stored relative to the camera - * @param u X Pixel Position - * @param v Y Pixel Position - */ - std::optional TagDetectorNodelet::getTagInCamFromPixel( - sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) { + * @brief Retrieve the pose of the tag in camera space + * @param msg 3D Point Cloud with points stored relative to the camera + * @param u X Pixel Position + * @param v Y Pixel Position + */ + auto TagDetectorNodelet::getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) -> std::optional { assert(cloudPtr); if (u >= cloudPtr->width || v >= cloudPtr->height) { @@ -19,13 +18,11 @@ namespace mrover { return std::nullopt; } - Point point = reinterpret_cast( - cloudPtr->data.data())[u + v * cloudPtr->width]; + Point point = reinterpret_cast(cloudPtr->data.data())[u + v * cloudPtr->width]; if (!std::isfinite(point.x) || !std::isfinite(point.y) || !std::isfinite(point.z)) { - NODELET_WARN("Tag center point not finite: [%f %f %f]", point.x, point.y, - point.z); + NODELET_WARN("Tag center point not finite: [%f %f %f]", point.x, point.y, point.z); return std::nullopt; } @@ -33,19 +30,17 @@ namespace mrover { } /** - * For each tag we have detected so far, fuse point cloud information. - * This information is where it is in the world. - * - * @param msg Point cloud message - */ - void TagDetectorNodelet::pointCloudCallback( - sensor_msgs::PointCloud2ConstPtr const& msg) { + * For each tag we have detected so far, fuse point cloud information. + * This information is where it is in the world. + * + * @param msg Point cloud message + */ + auto TagDetectorNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void { assert(msg); assert(msg->height > 0); assert(msg->width > 0); - if (!mEnableDetections) - return; + if (!mEnableDetections) return; mProfiler.beginLoop(); @@ -55,20 +50,17 @@ namespace mrover { // |BGRAXYZ...|...| So we need to copy the data into the correct format if (static_cast(msg->height) != mImg.rows || static_cast(msg->width) != mImg.cols) { - NODELET_INFO("Image size changed from [%d %d] to [%u %u]", mImg.cols, - mImg.rows, msg->width, msg->height); - mImg = cv::Mat{static_cast(msg->height), static_cast(msg->width), - CV_8UC3, cv::Scalar{0, 0, 0}}; + NODELET_INFO("Image size changed from [%d %d] to [%u %u]", mImg.cols, mImg.rows, msg->width, msg->height); + mImg = cv::Mat{static_cast(msg->height), static_cast(msg->width), CV_8UC3, cv::Scalar{0, 0, 0}}; } auto* pixelPtr = reinterpret_cast(mImg.data); auto* pointPtr = reinterpret_cast(msg->data.data()); - std::for_each(std::execution::par_unseq, pixelPtr, pixelPtr + mImg.total(), - [&](cv::Vec3b& pixel) { - size_t i = &pixel - pixelPtr; - pixel[0] = pointPtr[i].b; - pixel[1] = pointPtr[i].g; - pixel[2] = pointPtr[i].r; - }); + std::for_each(std::execution::par_unseq, pixelPtr, pixelPtr + mImg.total(), [&](cv::Vec3b& pixel) { + size_t i = &pixel - pixelPtr; + pixel[0] = pointPtr[i].b; + pixel[1] = pointPtr[i].g; + pixel[2] = pointPtr[i].r; + }); mProfiler.measureEvent("Convert"); // Call thresholding @@ -77,37 +69,23 @@ namespace mrover { // Detect the tag vertices in screen space and their respective ids // {mImmediateCorneres, mImmediateIds} are the outputs from OpenCV -#if CV_VERSION_MINOR <= 6 cv::aruco::detectMarkers(mImg, mDictionary, mImmediateCorners, mImmediateIds, mDetectorParams); -#else - // TODO(quintin) this is not ideal - static cv::aruco::ArucoDetector detector; - detector.setDictionary(*mDictionary); - detector.setDetectorParameters(*mDetectorParams); - detector.detectMarkers(mImg, mImmediateCorners, mImmediateIds); -#endif NODELET_DEBUG("OpenCV detect size: %zu", mImmediateIds.size()); mProfiler.measureEvent("OpenCV Detect"); // Update ID, image center, and increment hit count for all detected tags - for (size_t i = 0; i < mImmediateIds.size(); ++i) { + for (std::size_t i = 0; i < mImmediateIds.size(); ++i) { int id = mImmediateIds[i]; Tag& tag = mTags[id]; - tag.hitCount = - std::clamp(tag.hitCount + mTagIncrementWeight, 0, mMaxHitCount); + tag.hitCount = std::clamp(tag.hitCount + mTagIncrementWeight, 0, mMaxHitCount); tag.id = id; - tag.imageCenter = - std::reduce(mImmediateCorners[i].begin(), mImmediateCorners[i].end()) / - static_cast(mImmediateCorners[i].size()); - tag.tagInCam = getTagInCamFromPixel(msg, std::lround(tag.imageCenter.x), - std::lround(tag.imageCenter.y)); + tag.imageCenter = std::reduce(mImmediateCorners[i].begin(), mImmediateCorners[i].end()) / static_cast(mImmediateCorners[i].size()); + tag.tagInCam = getTagInCamFromPixel(msg, std::lround(tag.imageCenter.x), std::lround(tag.imageCenter.y)); if (tag.tagInCam) { // Publish tag to immediate - std::string immediateFrameId = - "immediateFiducial" + std::to_string(tag.id); - SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, - tag.tagInCam.value()); + std::string immediateFrameId = "immediateFiducial" + std::to_string(tag.id); + SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, tag.tagInCam.value()); } } @@ -131,22 +109,17 @@ namespace mrover { for (auto const& [id, tag]: mTags) { if (tag.hitCount >= mMinHitCountBeforePublish && tag.tagInCam) { try { - std::string immediateFrameId = - "immediateFiducial" + std::to_string(tag.id); + std::string immediateFrameId = "immediateFiducial" + std::to_string(tag.id); // Publish tag to odom - std::string const& parentFrameId = - mUseOdom ? mOdomFrameId : mMapFrameId; - SE3 tagInParent = - SE3::fromTfTree(mTfBuffer, parentFrameId, immediateFrameId); - SE3::pushToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), - parentFrameId, tagInParent); + std::string const& parentFrameId = mUseOdom ? mOdomFrameId : mMapFrameId; + SE3 tagInParent = SE3::fromTfTree(mTfBuffer, parentFrameId, immediateFrameId); + SE3::pushToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), parentFrameId, tagInParent); } catch (tf2::ExtrapolationException const&) { NODELET_WARN("Old data for immediate tag"); } catch (tf2::LookupException const&) { NODELET_WARN("Expected transform for immediate tag"); } catch (tf2::ConnectivityException const&) { - NODELET_WARN( - "Expected connection to odom frame. Is visual odometry running?"); + NODELET_WARN("Expected connection to odom frame. Is visual odometry running?"); } } } @@ -162,10 +135,8 @@ namespace mrover { for (auto& [id, tag]: mTags) { cv::Scalar color{255, 0, 0}; cv::Point pt{tagBoxWidth * tagCount, mImg.rows / 10}; - std::string text = - "id" + std::to_string(id) + ":" + std::to_string(tag.hitCount); - cv::putText(mImg, text, pt, cv::FONT_HERSHEY_COMPLEX, mImg.cols / 800.0, - color, mImg.cols / 300); + std::string text = "id" + std::to_string(id) + ":" + std::to_string(tag.hitCount); + cv::putText(mImg, text, pt, cv::FONT_HERSHEY_COMPLEX, mImg.cols / 800.0, color, mImg.cols / 300); ++tagCount; } @@ -180,15 +151,12 @@ namespace mrover { mImgMsg.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; size_t size = mImgMsg.step * mImgMsg.height; mImgMsg.data.resize(size); - std::uninitialized_copy(std::execution::par_unseq, mImg.data, - mImg.data + size, mImgMsg.data.begin()); + std::memcpy(mImgMsg.data.data(), mImg.data, size); mImgPub.publish(mImgMsg); } size_t detectedCount = mImmediateIds.size(); - NODELET_INFO_COND(!mPrevDetectedCount.has_value() || - detectedCount != mPrevDetectedCount.value(), - "Detected %zu markers", detectedCount); + NODELET_INFO_COND(!mPrevDetectedCount.has_value() || detectedCount != mPrevDetectedCount.value(), "Detected %zu markers", detectedCount); mPrevDetectedCount = detectedCount; mProfiler.measureEvent("Publish"); @@ -196,4 +164,4 @@ namespace mrover { mSeqNum++; } -} // namespace mrover +} // namespace mrover \ No newline at end of file From af12b8dfc621709a1e68b44afa43310ff4a3fcc3 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 8 Feb 2024 17:55:32 -0500 Subject: [PATCH 158/197] Quintin Told me too --- data/yolov8n_mallet_bottle_better.onnx | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 data/yolov8n_mallet_bottle_better.onnx diff --git a/data/yolov8n_mallet_bottle_better.onnx b/data/yolov8n_mallet_bottle_better.onnx new file mode 100644 index 000000000..35335cc1c --- /dev/null +++ b/data/yolov8n_mallet_bottle_better.onnx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:01b717f4c1aebdc31f23cf9f53a5dceb6cb6666745b8ba34056df688a44b263d +size 44720296 From b91bd2caaf3fd0553c87479563fb9f7849e10ae7 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Thu, 8 Feb 2024 18:29:14 -0500 Subject: [PATCH 159/197] Ros Params --- CMakeLists.txt | 2 +- .../files/networking_setup_jetson.sh | 2 +- src/perception/object_detector/inference.cu | 2 +- .../object_detector/object_detector.cpp | 15 ++++++++------- starter_project/autonomy/src/perception.hpp | 3 +-- 5 files changed, 12 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 21ddaa86a..3306b257d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -189,7 +189,7 @@ mrover_nodelet_link_libraries(zed_tag_detector opencv_core opencv_aruco opencv_i if (CUDA_FOUND) mrover_add_library(streaming src/esw/streaming/*.c* src/esw/streaming) # target_link_libraries(streaming PUBLIC opencv_core opencv_cudacodec) - target_link_libraries(streaming PUBLIC cuda nvidia-encode opencv_core) + target_link_libraries(streaming PUBLIC cuda /usr/local/lib/libnvidia-encode.so opencv_core) target_include_directories(streaming SYSTEM PUBLIC deps/nvenc) target_compile_definitions(streaming PUBLIC BOOST_ASIO_NO_DEPRECATED) endif () diff --git a/ansible/roles/jetson_networks/files/networking_setup_jetson.sh b/ansible/roles/jetson_networks/files/networking_setup_jetson.sh index 3d1bcec67..92b7f1e0b 100644 --- a/ansible/roles/jetson_networks/files/networking_setup_jetson.sh +++ b/ansible/roles/jetson_networks/files/networking_setup_jetson.sh @@ -1,3 +1,3 @@ #!/usr/bin/env bash -export ROS_IP=10.0.0.2 +export ROS_IP=10.0.0.10 diff --git a/src/perception/object_detector/inference.cu b/src/perception/object_detector/inference.cu index f09fb0557..f12d7c2db 100644 --- a/src/perception/object_detector/inference.cu +++ b/src/perception/object_detector/inference.cu @@ -35,7 +35,7 @@ namespace mrover { auto Inference::createCudaEngine(std::filesystem::path const& onnxModelPath) -> ICudaEngine* { - mModelPath = onnxModelPath; + mModelPath = onnxModelPath.string(); //Define the size of Batches constexpr auto explicitBatch = 1U << static_cast(NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index b89230d4b..a79bcb904 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -7,13 +7,6 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - std::filesystem::path packagePath = ros::package::getPath("mrover"); - std::filesystem::path modelPath = packagePath / "data" / mModelName.append(".onnx"); - - mInferenceWrapper = InferenceWrapper{modelPath}; - - mImgSub = mNh.subscribe("/camera/left/points", 1, &ObjectDetectorNodelet::imageCallback, this); - mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); mNh.param("camera_frame", mCameraFrameId, "zed2i_left_camera_frame"); mNh.param("world_frame", mMapFrameId, "map"); @@ -23,6 +16,14 @@ namespace mrover { mNh.param("obj_hitcount_threshold", mObjHitThreshold, 5); mNh.param("obj_hitcount_max", mObjMaxHitcount, 10); mNh.param("model_name", mModelName, "yolov8n_mallet_bottle_better"); + + std::filesystem::path packagePath = ros::package::getPath("mrover"); + std::filesystem::path modelPath = packagePath / "data" / mModelName.append(".onnx"); + + mInferenceWrapper = InferenceWrapper{modelPath}; + + mImgSub = mNh.subscribe("/camera/left/points", 1, &ObjectDetectorNodelet::imageCallback, this); + mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); } } // namespace mrover \ No newline at end of file diff --git a/starter_project/autonomy/src/perception.hpp b/starter_project/autonomy/src/perception.hpp index 22b62f1b3..712eb7ad8 100644 --- a/starter_project/autonomy/src/perception.hpp +++ b/starter_project/autonomy/src/perception.hpp @@ -8,9 +8,8 @@ #include // OpenCV Headers, cv namespace -#include -#include #include +#include // ROS Headers, ros namespace #include From 33c22745ac8b32a4d4760e3700cad4eb63d8c223 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 8 Feb 2024 19:15:40 -0500 Subject: [PATCH 160/197] Fixing ROS Params --- src/perception/object_detector/inference.cu | 9 +++++---- src/perception/object_detector/inference.cuh | 4 ++-- src/perception/object_detector/inference_wrapper.cu | 6 +++--- src/perception/object_detector/inference_wrapper.hpp | 2 +- src/perception/object_detector/object_detector.cpp | 4 ++-- 5 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/perception/object_detector/inference.cu b/src/perception/object_detector/inference.cu index f12d7c2db..7ac5c631f 100644 --- a/src/perception/object_detector/inference.cu +++ b/src/perception/object_detector/inference.cu @@ -16,9 +16,9 @@ namespace mrover { constexpr static char const* INPUT_BINDING_NAME = "images"; constexpr static char const* OUTPUT_BINDING_NAME = "output0"; - Inference::Inference(std::filesystem::path const& onnxModelPath) { + Inference::Inference(std::filesystem::path const& onnxModelPath, std::string const& modelName) { // Create the engine object from either the file or from onnx file - mEngine = std::unique_ptr{createCudaEngine(onnxModelPath)}; + mEngine = std::unique_ptr{createCudaEngine(onnxModelPath, modelName)}; if (!mEngine) throw std::runtime_error("Failed to create CUDA engine"); mLogger.log(ILogger::Severity::kINFO, "Created CUDA Engine"); @@ -33,7 +33,7 @@ namespace mrover { prepTensors(); } - auto Inference::createCudaEngine(std::filesystem::path const& onnxModelPath) -> ICudaEngine* { + auto Inference::createCudaEngine(std::filesystem::path const& onnxModelPath, std::string const& modelName) -> ICudaEngine* { mModelPath = onnxModelPath.string(); @@ -70,7 +70,8 @@ namespace mrover { //Define the engine file location relative to the mrover package std::filesystem::path packagePath = ros::package::getPath("mrover"); - std::filesystem::path enginePath = packagePath / "data" / std::string("tensorrt-engine-").append(mModelPath).append(".engine"); + std::filesystem::path enginePath = packagePath / "data" / std::string("tensorrt-engine-").append(modelName).append(".engine"); + ROS_INFO_STREAM(enginePath); //Check if engine file exists if (!exists(enginePath)) { diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index 55de3edb8..ca6e6a67b 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -30,7 +30,7 @@ namespace mrover { static auto getBindingInputIndex(IExecutionContext const* context) -> int; - auto createCudaEngine(std::filesystem::path const& onnxModelPath) -> ICudaEngine*; + auto createCudaEngine(std::filesystem::path const& onnxModelPath, std::string const& modelName) -> ICudaEngine*; auto launchInference(cv::Mat const& input, cv::Mat const& output) const -> void; @@ -51,7 +51,7 @@ namespace mrover { auto createExecutionContext() -> void; public: - explicit Inference(std::filesystem::path const& onnxModelPath); + explicit Inference(std::filesystem::path const& onnxModelPath, std::string const& modelName); auto doDetections(cv::Mat const& img) const -> void; diff --git a/src/perception/object_detector/inference_wrapper.cu b/src/perception/object_detector/inference_wrapper.cu index 39fb1f9b3..38ed6b216 100644 --- a/src/perception/object_detector/inference_wrapper.cu +++ b/src/perception/object_detector/inference_wrapper.cu @@ -13,11 +13,11 @@ using namespace nvinfer1; */ namespace mrover { - InferenceWrapper::InferenceWrapper(std::string onnxModelPath) { - mInference.reset(new Inference(std::move(onnxModelPath))); + InferenceWrapper::InferenceWrapper(std::string onnxModelPath, std::string const& modelName) { + mInference.reset(new Inference(std::move(onnxModelPath), modelName)); } - auto InferenceWrapper::doDetections(const cv::Mat& img) const -> void { + auto InferenceWrapper::doDetections(cv::Mat const& img) const -> void { // Execute the forward pass on the inference object mInference->doDetections(img); } diff --git a/src/perception/object_detector/inference_wrapper.hpp b/src/perception/object_detector/inference_wrapper.hpp index 1ef593e47..7828d007b 100644 --- a/src/perception/object_detector/inference_wrapper.hpp +++ b/src/perception/object_detector/inference_wrapper.hpp @@ -14,7 +14,7 @@ namespace mrover { ~InferenceWrapper() = default; - explicit InferenceWrapper(std::string onnxModelPath); + explicit InferenceWrapper(std::string onnxModelPath, std::string const& modelName); auto doDetections(cv::Mat const& img) const -> void; diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index a79bcb904..4423ce5b8 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -18,9 +18,9 @@ namespace mrover { mNh.param("model_name", mModelName, "yolov8n_mallet_bottle_better"); std::filesystem::path packagePath = ros::package::getPath("mrover"); - std::filesystem::path modelPath = packagePath / "data" / mModelName.append(".onnx"); + std::filesystem::path modelPath = packagePath / "data" / (mModelName + ".onnx"); - mInferenceWrapper = InferenceWrapper{modelPath}; + mInferenceWrapper = InferenceWrapper{modelPath, mModelName}; mImgSub = mNh.subscribe("/camera/left/points", 1, &ObjectDetectorNodelet::imageCallback, this); mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); From b20cc0773cf5674b7f89b115eb54e35d1e702c12 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Thu, 8 Feb 2024 19:53:46 -0500 Subject: [PATCH 161/197] Added Assert --- src/perception/object_detector/inference.cu | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/perception/object_detector/inference.cu b/src/perception/object_detector/inference.cu index 7ac5c631f..79a1c18da 100644 --- a/src/perception/object_detector/inference.cu +++ b/src/perception/object_detector/inference.cu @@ -1,4 +1,5 @@ #include "inference.cuh" +#include using namespace nvinfer1; @@ -155,6 +156,7 @@ namespace mrover { std::size_t const size = std::reduce(extents, extents + rank, sizeof(float), std::multiplies<>()); // Create GPU memory for TensorRT to operate on cudaMalloc(mBindings.data() + i, size); + assert(mBindings.data() + i); } assert(mContext); From bc331c226194b22d80ef65584dc813107477d8bc Mon Sep 17 00:00:00 2001 From: John Date: Fri, 9 Feb 2024 20:59:44 -0500 Subject: [PATCH 162/197] Thread Not DYING :((((((( --- .../image_capture/image_capturepy.py | 58 ++++--------------- 1 file changed, 12 insertions(+), 46 deletions(-) diff --git a/src/perception/image_capture/image_capturepy.py b/src/perception/image_capture/image_capturepy.py index 95d0b09e2..2dad35783 100755 --- a/src/perception/image_capture/image_capturepy.py +++ b/src/perception/image_capture/image_capturepy.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 -from pynput.keyboard import Key, Controller +from pynput.keyboard import Controller, Key + # python linear algebra library import numpy as np @@ -16,63 +17,28 @@ # ROS message types we need to use from sensor_msgs.msg import Image -import threading - - -class KeyboardThread(threading.Thread): - - def __init__(self, input_cbk=None, name="keyboard-input-thread"): - self.input_cbk = input_cbk - super(KeyboardThread, self).__init__(name=name) - self.start() - self.run = True - - def run(self): - while True: - self.input_cbk(input()) # waits to get input + Return - if not self.run: - break - - def stop(self): - self.run = False - - -showcounter = 0 # something to demonstrate the change - - -def my_callback(inp): - # evaluate the keyboard input - image_capturepy.showcounter += 1 - - # start the Keyboard thread class image_capturepy: - showcounter = 0 def __init__(self): - rospy.Subscriber("/camera/left/image", Image, self.img_callback) - self.kthread = KeyboardThread(my_callback) self.keyboard = Controller() + while True: + input() + msg = rospy.wait_for_message("/camera/left/image", Image, timeout=5) + data = np.empty(msg.height * msg.width * 4, dtype=np.uint8) + for x in range(msg.height * msg.width * 4): + data[x] = msg.data[x] + + image = np.reshape(data, [msg.height, msg.width, 4]) + unique_id = "{date:%Y-%m-%d_%H:%M:%S}".format(date=datetime.datetime.now()) + print(cv2.imwrite(f"//home//john//Desktop//Rover//Images//image_{unique_id}.jpg", image)) def __del__(self): - self.kthread.stop() self.keyboard.press(Key.enter) self.keyboard.release(Key.enter) - def img_callback(self, msg): - data = np.empty(msg.height * msg.width * 4, dtype=np.uint8) - for x in range(msg.height * msg.width * 4): - data[x] = msg.data[x] - - image = np.reshape(data, [msg.height, msg.width, 4]) - print(image_capturepy.showcounter) - if image_capturepy.showcounter != 0: - unique_id = "{date:%Y-%m-%d_%H:%M:%S}".format(date=datetime.datetime.now()) - print(cv2.imwrite(f"//home//john//Desktop//Rover//Images//image_{unique_id}.jpg", image)) - image_capturepy.showcounter = 0 - def main(): # initialize the node From 94d656a3aa7fb1300321e47be5d9b44d11efcc39 Mon Sep 17 00:00:00 2001 From: John Date: Sat, 10 Feb 2024 13:55:34 -0500 Subject: [PATCH 163/197] WIP Working on directories --- .../image_capture/image_capturepy.py | 50 ++++++++++++------- 1 file changed, 31 insertions(+), 19 deletions(-) diff --git a/src/perception/image_capture/image_capturepy.py b/src/perception/image_capture/image_capturepy.py index 2dad35783..f3916d1d5 100755 --- a/src/perception/image_capture/image_capturepy.py +++ b/src/perception/image_capture/image_capturepy.py @@ -1,7 +1,10 @@ #!/usr/bin/env python3 -from pynput.keyboard import Controller, Key +from pynput import keyboard +import rospkg + +import os # python linear algebra library import numpy as np @@ -17,35 +20,44 @@ # ROS message types we need to use from sensor_msgs.msg import Image -# start the Keyboard thread +def on_press(key): + if key == keyboard.Key.enter: -class image_capturepy: + msg = rospy.wait_for_message("/camera/left/image", Image, timeout=5) + data = np.empty(msg.height * msg.width * 4, dtype=np.uint8) + for x in range(msg.height * msg.width * 4): + data[x] = msg.data[x] + + image = np.reshape(data, [msg.height, msg.width, 4]) + unique_id = "{date:%Y-%m-%d_%H:%M:%S}".format(date=datetime.datetime.now()) + rospack = rospkg.RosPack() + pkgPath = rospack.get_path("mrover") + imagePath = pkgPath + f"/data/Images" + if not os.path.exists(imagePath): + os.mkdir(imagePath) + + print(cv2.imwrite(imagePath + f"image_{unique_id}.jpg", image)) - def __init__(self): - self.keyboard = Controller() - while True: - input() - msg = rospy.wait_for_message("/camera/left/image", Image, timeout=5) - data = np.empty(msg.height * msg.width * 4, dtype=np.uint8) - for x in range(msg.height * msg.width * 4): - data[x] = msg.data[x] - image = np.reshape(data, [msg.height, msg.width, 4]) - unique_id = "{date:%Y-%m-%d_%H:%M:%S}".format(date=datetime.datetime.now()) - print(cv2.imwrite(f"//home//john//Desktop//Rover//Images//image_{unique_id}.jpg", image)) +def on_release(key): + while False: + continue - def __del__(self): - self.keyboard.press(Key.enter) - self.keyboard.release(Key.enter) + +class image_capturepy: + + def __init__(self): + while False: + continue def main(): # initialize the node rospy.init_node("image_capturepy") - # create and start our localization system - image_capturepy1 = image_capturepy() + listener = keyboard.Listener(on_press=on_press, on_release=on_release) + listener.start() # let the callback functions run asynchronously in the background rospy.spin() From 7edf84a287954330387fd6e835168db206b98144 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 11 Feb 2024 11:26:09 -0500 Subject: [PATCH 164/197] More Clean up --- src/perception/object_detector/inference.cu | 8 +++---- src/perception/object_detector/inference.cuh | 16 ++++++++++++++ .../object_detector/inference_wrapper.cu | 2 +- .../object_detector/inference_wrapper.hpp | 2 +- .../object_detector/object_detector.cpp | 9 ++++++++ .../object_detector/object_detector.hpp | 15 +++++-------- .../object_detector.processing.cpp | 22 ++----------------- src/perception/object_detector/pch.hpp | 17 ++++++++------ 8 files changed, 49 insertions(+), 42 deletions(-) diff --git a/src/perception/object_detector/inference.cu b/src/perception/object_detector/inference.cu index 79a1c18da..ad554ca71 100644 --- a/src/perception/object_detector/inference.cu +++ b/src/perception/object_detector/inference.cu @@ -1,5 +1,4 @@ #include "inference.cuh" -#include using namespace nvinfer1; @@ -18,6 +17,8 @@ namespace mrover { constexpr static char const* OUTPUT_BINDING_NAME = "output0"; Inference::Inference(std::filesystem::path const& onnxModelPath, std::string const& modelName) { + mModelPath = onnxModelPath.string(); + // Create the engine object from either the file or from onnx file mEngine = std::unique_ptr{createCudaEngine(onnxModelPath, modelName)}; if (!mEngine) throw std::runtime_error("Failed to create CUDA engine"); @@ -36,8 +37,6 @@ namespace mrover { auto Inference::createCudaEngine(std::filesystem::path const& onnxModelPath, std::string const& modelName) -> ICudaEngine* { - mModelPath = onnxModelPath.string(); - //Define the size of Batches constexpr auto explicitBatch = 1U << static_cast(NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); @@ -73,7 +72,6 @@ namespace mrover { std::filesystem::path packagePath = ros::package::getPath("mrover"); std::filesystem::path enginePath = packagePath / "data" / std::string("tensorrt-engine-").append(modelName).append(".engine"); ROS_INFO_STREAM(enginePath); - //Check if engine file exists if (!exists(enginePath)) { ROS_WARN_STREAM("Optimizing ONXX model for TensorRT. This make take a long time..."); @@ -109,6 +107,8 @@ namespace mrover { auto Inference::createExecutionContext() -> void { // Create Execution Context. mContext.reset(mEngine->createExecutionContext()); + if (!mContext) throw std::runtime_error("Failed to create execution context"); + //Set up the input tensor sizing mContext->setInputShape(INPUT_BINDING_NAME, mEngine->getTensorShape(INPUT_BINDING_NAME)); diff --git a/src/perception/object_detector/inference.cuh b/src/perception/object_detector/inference.cuh index ca6e6a67b..9e8713a50 100644 --- a/src/perception/object_detector/inference.cuh +++ b/src/perception/object_detector/inference.cuh @@ -30,8 +30,18 @@ namespace mrover { static auto getBindingInputIndex(IExecutionContext const* context) -> int; + /** + * @brief Creates the Cuda engine which is machine specific + * + * Uses the file path to locate the onnx model and the modelName to locate the engine file + */ auto createCudaEngine(std::filesystem::path const& onnxModelPath, std::string const& modelName) -> ICudaEngine*; + /** + * @brief Performs the Model forward pass and allocates the reults in the output tensor + * + * Takes the input Mat in the format for CNN and requires a SIZED empty Mat to store the output + */ auto launchInference(cv::Mat const& input, cv::Mat const& output) const -> void; /** @@ -53,8 +63,14 @@ namespace mrover { public: explicit Inference(std::filesystem::path const& onnxModelPath, std::string const& modelName); + /** + * @brief Runs the forward pass on the given input image in CNN format + */ auto doDetections(cv::Mat const& img) const -> void; + /** + * @brief Retrieves the mat where the output from the forward pass was stored + */ auto getOutputTensor() -> cv::Mat; }; diff --git a/src/perception/object_detector/inference_wrapper.cu b/src/perception/object_detector/inference_wrapper.cu index 38ed6b216..f1e41787a 100644 --- a/src/perception/object_detector/inference_wrapper.cu +++ b/src/perception/object_detector/inference_wrapper.cu @@ -13,7 +13,7 @@ using namespace nvinfer1; */ namespace mrover { - InferenceWrapper::InferenceWrapper(std::string onnxModelPath, std::string const& modelName) { + InferenceWrapper::InferenceWrapper(std::string const& onnxModelPath, std::string const& modelName) { mInference.reset(new Inference(std::move(onnxModelPath), modelName)); } diff --git a/src/perception/object_detector/inference_wrapper.hpp b/src/perception/object_detector/inference_wrapper.hpp index 7828d007b..8d52fce54 100644 --- a/src/perception/object_detector/inference_wrapper.hpp +++ b/src/perception/object_detector/inference_wrapper.hpp @@ -14,7 +14,7 @@ namespace mrover { ~InferenceWrapper() = default; - explicit InferenceWrapper(std::string onnxModelPath, std::string const& modelName); + explicit InferenceWrapper(std::string const& onnxModelPath, std::string const& modelName); auto doDetections(cv::Mat const& img) const -> void; diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 4423ce5b8..9daeddf27 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -7,21 +7,30 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); + //Loop profiler param + mNh.param("enable_loop_profiler", mEnableLoopProfiler, false); + //TF Params mNh.param("camera_frame", mCameraFrameId, "zed2i_left_camera_frame"); mNh.param("world_frame", mMapFrameId, "map"); + //Hit count params mNh.param("obj_increment_weight", mObjIncrementWeight, 2); mNh.param("obj_decrement_weight", mObjDecrementWeight, 1); mNh.param("obj_hitcount_threshold", mObjHitThreshold, 5); mNh.param("obj_hitcount_max", mObjMaxHitcount, 10); + + //Model Params mNh.param("model_name", mModelName, "yolov8n_mallet_bottle_better"); + //Init Model Path std::filesystem::path packagePath = ros::package::getPath("mrover"); std::filesystem::path modelPath = packagePath / "data" / (mModelName + ".onnx"); + //Wrapper mInferenceWrapper = InferenceWrapper{modelPath, mModelName}; + //Pub sub mImgSub = mNh.subscribe("/camera/left/points", 1, &ObjectDetectorNodelet::imageCallback, this); mDebugImgPub = mNh.advertise("/object_detector/debug_img", 1); } diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index cafddba6c..05a8acc51 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -1,26 +1,23 @@ #pragma once -#include "inference_wrapper.hpp" +#include "pch.hpp" -namespace mrover -{ +namespace mrover { //Data type for detection - struct Detection - { + struct Detection { int classId{0}; std::string className; float confidence{0.0}; cv::Rect box; }; - class ObjectDetectorNodelet : public nodelet::Nodelet - { + class ObjectDetectorNodelet : public nodelet::Nodelet { + std::string mModelName; LoopProfiler mLoopProfiler{"Object Detector", 1}; - bool mEnableLoopProfiler = true; + bool mEnableLoopProfiler{}; - // Mat for the image from the point cloud cv::Mat mImg; std::vector classes{"Bottle", "Hammer"}; diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 8f9872613..0bda1ef6a 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -1,8 +1,5 @@ #include "object_detector.hpp" -#include "point.hpp" -#include "inference_wrapper.hpp" - namespace mrover { auto ObjectDetectorNodelet::imageCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void { @@ -153,10 +150,6 @@ namespace mrover { mLoopProfiler.measureEvent("Extract Detections"); } - if (mEnableLoopProfiler) { - //mLoopProfiler.measureEvent("Push to TF START"); - } - std::vector seenObjects{false, false}; //If there are detections locate them in 3D for (Detection const& detection: detections) { @@ -164,10 +157,6 @@ namespace mrover { //Increment Object hit counts if theyre seen updateHitsObject(msg, detection, seenObjects); - if (mEnableLoopProfiler) { - //mLoopProfiler.measureEvent("Push to TF 1"); - } - //Decrement Object hit counts if they're not seen for (size_t i = 0; i < seenObjects.size(); i++) { if (!seenObjects.at(i)) { @@ -175,10 +164,6 @@ namespace mrover { } } - if (mEnableLoopProfiler) { - //mLoopProfiler.measureEvent("Push to TF 2"); - } - //Draw the detected object's bounding boxes on the image for each of the objects detected std::vector fontColors{cv::Scalar{232, 115, 5}, cv::Scalar{0, 4, 227}}; @@ -196,7 +181,7 @@ namespace mrover { } if (mEnableLoopProfiler) { - mLoopProfiler.measureEvent("Push to TF FINAL"); + mLoopProfiler.measureEvent("Push to TF"); } //We only want to publish the debug image if there is something lsitening, to reduce the operations going on @@ -281,9 +266,6 @@ namespace mrover { auto centerWidth = static_cast(center.first * static_cast(msg->width) / imgSize.width); auto centerHeight = static_cast(center.second * static_cast(msg->height) / imgSize.height); - std::cout << mObjectHitCounts.at(0) << ", " << mObjectHitCounts.at(1) << std::endl; - ROS_INFO("%d, %d", mObjectHitCounts.at(0), mObjectHitCounts.at(1)); - if (!seenObjects.at(detection.classId)) { seenObjects.at(detection.classId) = true; @@ -298,7 +280,7 @@ namespace mrover { //Since the object is seen we need to increment the hit counter mObjectHitCounts.at(detection.classId) = std::min(mObjMaxHitcount, mObjectHitCounts.at(detection.classId) + mObjIncrementWeight); - ROS_INFO("PUSHED TO TF TEMP"); + //Only publish to permament if we are confident in the object if (mObjectHitCounts.at(detection.classId) > mObjHitThreshold) { diff --git a/src/perception/object_detector/pch.hpp b/src/perception/object_detector/pch.hpp index ebc19cb28..18c32f3a5 100644 --- a/src/perception/object_detector/pch.hpp +++ b/src/perception/object_detector/pch.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -13,15 +14,13 @@ #include #include #include +#include #include +#include #include #include #include -#include -#include -#include -#include -#include + #include #include @@ -31,17 +30,21 @@ #include #include #include +#include #include #include #include #include #include #include -#include #include -#include #include +#include #include #include + +//Object Detector Includes +#include "inference_wrapper.hpp" +#include "point.hpp" From a7cb1b70bfccf110e8369f55ae5150ad92cdbb7c Mon Sep 17 00:00:00 2001 From: John Date: Sun, 11 Feb 2024 11:46:21 -0500 Subject: [PATCH 165/197] Remove starter project changes --- .../autonomy/AutonomyStarterProject.cmake | 2 +- starter_project/autonomy/config/soil_base.png | 1 + .../autonomy/config/soil_normal.png | 1 + .../autonomy/config/starter_project.rviz | 334 ++++++++++++++++++ .../autonomy/config/starter_project.world | 131 +++++++ .../config/starter_project_ground.blend | 3 + .../config/starter_project_ground.dae | 3 + starter_project/autonomy/src/perception.cpp | 5 +- starter_project/teleop/README.md | 0 starter_project/teleop/babel.config.js | 7 + starter_project/teleop/gui_starter.sh | 32 ++ starter_project/teleop/package.json | 37 ++ starter_project/teleop/src/App.vue | 19 + starter_project/teleop/src/app.js | 20 ++ starter_project/teleop/src/assets/logo.png | 3 + .../teleop/src/components/DriveControls.vue | 73 ++++ .../teleop/src/components/Menu.vue | 66 ++++ .../teleop/src/components/MenuButton.vue | 43 +++ starter_project/teleop/src/index.html | 12 + starter_project/teleop/src/router/index.js | 15 + starter_project/teleop/src/static/mrover.png | 3 + 21 files changed, 805 insertions(+), 5 deletions(-) create mode 120000 starter_project/autonomy/config/soil_base.png create mode 120000 starter_project/autonomy/config/soil_normal.png create mode 100644 starter_project/autonomy/config/starter_project.rviz create mode 100644 starter_project/autonomy/config/starter_project.world create mode 100644 starter_project/autonomy/config/starter_project_ground.blend create mode 100644 starter_project/autonomy/config/starter_project_ground.dae create mode 100644 starter_project/teleop/README.md create mode 100755 starter_project/teleop/babel.config.js create mode 100755 starter_project/teleop/gui_starter.sh create mode 100755 starter_project/teleop/package.json create mode 100755 starter_project/teleop/src/App.vue create mode 100755 starter_project/teleop/src/app.js create mode 100755 starter_project/teleop/src/assets/logo.png create mode 100755 starter_project/teleop/src/components/DriveControls.vue create mode 100755 starter_project/teleop/src/components/Menu.vue create mode 100755 starter_project/teleop/src/components/MenuButton.vue create mode 100755 starter_project/teleop/src/index.html create mode 100755 starter_project/teleop/src/router/index.js create mode 100755 starter_project/teleop/src/static/mrover.png diff --git a/starter_project/autonomy/AutonomyStarterProject.cmake b/starter_project/autonomy/AutonomyStarterProject.cmake index e3bdd782b..4b84c75af 100644 --- a/starter_project/autonomy/AutonomyStarterProject.cmake +++ b/starter_project/autonomy/AutonomyStarterProject.cmake @@ -18,7 +18,7 @@ add_executable(starter_project_perception ${STARTER_PROJECT_PERCEPTION_SOURCES}) # Ensure that our project builds after message generation add_dependencies(starter_project_perception ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # Link needed libraries -target_link_libraries(starter_project_perception ${catkin_LIBRARIES} opencv_core opencv_aruco) +target_link_libraries(starter_project_perception ${catkin_LIBRARIES} ${OpenCV_LIBS}) # Include needed directories target_include_directories(starter_project_perception PUBLIC ${catkin_INCLUDE_DIRS}) diff --git a/starter_project/autonomy/config/soil_base.png b/starter_project/autonomy/config/soil_base.png new file mode 120000 index 000000000..02756e2cc --- /dev/null +++ b/starter_project/autonomy/config/soil_base.png @@ -0,0 +1 @@ +../../../config/gazebo/env_description/soil_base.png \ No newline at end of file diff --git a/starter_project/autonomy/config/soil_normal.png b/starter_project/autonomy/config/soil_normal.png new file mode 120000 index 000000000..3399db4f9 --- /dev/null +++ b/starter_project/autonomy/config/soil_normal.png @@ -0,0 +1 @@ +../../../config/gazebo/env_description/soil_normal.png \ No newline at end of file diff --git a/starter_project/autonomy/config/starter_project.rviz b/starter_project/autonomy/config/starter_project.rviz new file mode 100644 index 000000000..ca7a99f10 --- /dev/null +++ b/starter_project/autonomy/config/starter_project.rviz @@ -0,0 +1,334 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /Image1 + Splitter Ratio: 0.5 + Tree Height: 537 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: Image +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + back_right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + center_left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + center_right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + chassis_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + right_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: true + back_left_wheel_link: + Value: true + back_right_wheel_link: + Value: true + base_link: + Value: true + camera_link: + Value: true + center_left_wheel_link: + Value: true + center_right_wheel_link: + Value: true + chassis_link: + Value: true + front_left_wheel_link: + Value: true + front_right_wheel_link: + Value: true + imu_link: + Value: true + right_camera_link: + Value: true + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + base_link: + chassis_link: + back_left_wheel_link: + {} + back_right_wheel_link: + {} + camera_link: + {} + center_left_wheel_link: + {} + center_right_wheel_link: + {} + front_left_wheel_link: + {} + front_right_wheel_link: + {} + imu_link: + {} + right_camera_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: false + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: false + Value: false + - Class: rviz/Image + Enabled: true + Image Topic: /camera/left/image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 1 + Transport Hint: raw + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /camera/depth/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /odometry/filtered + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.6909778118133545 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0.1978837251663208 + Y: 0.135016530752182 + Z: 0.33381450176239014 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5552026629447937 + Target Frame: + Yaw: 5.666762828826904 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1047 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000379fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002a4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002e7000000cf0000001600ffffff00000001000001000000035dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006f80000003efc0100000002fb0000000800540069006d00650100000000000006f8000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000059c0000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1784 + X: 159 + Y: 125 diff --git a/starter_project/autonomy/config/starter_project.world b/starter_project/autonomy/config/starter_project.world new file mode 100644 index 000000000..a8235fb42 --- /dev/null +++ b/starter_project/autonomy/config/starter_project.world @@ -0,0 +1,131 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.7 0.7 0.7 1 + 0.529 0.808 0.922 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 5.5 2.0 0.5 0 0 4.71239 + 1 + + + + + model://post/meshes/4x4_1000-0.dae + + + 10 + + + + + + + + + + + + + + + + + model://post/meshes/4x4_1000-0.dae + + + + 0 + 0 + 0 + + + + 0 0 -0.1 0 0 0 + 1 + + + + + model://starter_project_ground.dae + + + 10 + + + 65535 + + + + + 100 + 50 + + + + + + + + + + + + model://starter_project_ground.dae + + + + + model://soil_normal.png + + + + 0 + 0 + 0 + + + + + 6.01711 -3.68905 2.02916 0 0.275643 2.35619 + orbit + perspective + + + + \ No newline at end of file diff --git a/starter_project/autonomy/config/starter_project_ground.blend b/starter_project/autonomy/config/starter_project_ground.blend new file mode 100644 index 000000000..dad3e614c --- /dev/null +++ b/starter_project/autonomy/config/starter_project_ground.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:dc79a8b52a191b1ebdcf462e986cc787f7467db36aa0a381f4fd664814744fce +size 906528 diff --git a/starter_project/autonomy/config/starter_project_ground.dae b/starter_project/autonomy/config/starter_project_ground.dae new file mode 100644 index 000000000..53c9fe8b7 --- /dev/null +++ b/starter_project/autonomy/config/starter_project_ground.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:079221b516b0918a668fa6e3ac68357b404e5dc28d1fb0b85576584a721e9ab9 +size 71515 diff --git a/starter_project/autonomy/src/perception.cpp b/starter_project/autonomy/src/perception.cpp index 3a677c6cd..986b4a6a4 100644 --- a/starter_project/autonomy/src/perception.cpp +++ b/starter_project/autonomy/src/perception.cpp @@ -3,9 +3,6 @@ // ROS Headers, ros namespace #include -#include -#include - int main(int argc, char** argv) { ros::init(argc, argv, "starter_project_perception"); // Our node name (See: http://wiki.ros.org/Nodes) @@ -50,7 +47,7 @@ namespace mrover { } void Perception::findTagsInImage(cv::Mat const& image, std::vector& tags) { // NOLINT(*-convert-member-functions-to-static) - // hint: take a look at OpenCV's documentation for the detectMarkers function that can be called on mTagDetector + // hint: take a look at OpenCV's documentation for the detectMarkers function // hint: you have mTagDictionary, mTagCorners, mTagIds, and mTagDetectorParams member variables already defined! // hint: write and use the "getCenterFromTagCorners" and "getClosenessMetricFromTagCorners" functions diff --git a/starter_project/teleop/README.md b/starter_project/teleop/README.md new file mode 100644 index 000000000..e69de29bb diff --git a/starter_project/teleop/babel.config.js b/starter_project/teleop/babel.config.js new file mode 100755 index 000000000..b74838b52 --- /dev/null +++ b/starter_project/teleop/babel.config.js @@ -0,0 +1,7 @@ +{ + loaders: [{ + test: /\.jsx?$/, + loaders: ['babel?retainLines=true'], + include: path.join(__dirname, 'src') + }] +} \ No newline at end of file diff --git a/starter_project/teleop/gui_starter.sh b/starter_project/teleop/gui_starter.sh new file mode 100755 index 000000000..3814d59e0 --- /dev/null +++ b/starter_project/teleop/gui_starter.sh @@ -0,0 +1,32 @@ +#!/bin/bash + +# Ensure proper NODE_PATH +export NODE_PATH=/usr/lib/nodejs:/usr/share/nodejs + + +node_executable=node +node_executable_path=$(which "$node_executable") +yarn_executable=yarn +yarn_executable_path=$(which "$yarn_executable") + +if [ -z "$node_executable_path" ] +then + echo "Node installation not found, please run src/teleop/gui/gui_install.sh" + exit 1 +fi +if [ -z "$yarn_executable_path" ] +then + echo "Yarn installation not found, please run src/teleop/gui/gui_install.sh" + exit 1 +fi + +# Check if node_modules up to date +yarn check --verify-tree + +# If not up to date, install +if [ $? == 1 ] +then + yarn install --check-files +fi + +yarn run serve diff --git a/starter_project/teleop/package.json b/starter_project/teleop/package.json new file mode 100755 index 000000000..0319e3bee --- /dev/null +++ b/starter_project/teleop/package.json @@ -0,0 +1,37 @@ +{ + "private":true, + "scripts": { + "build": "webpack --config webpack.config.dev.js", + "dev": "webpack --config webpack.config.dev.js", + "serve": "webpack serve --config webpack.config.dev.js" + }, + "devDependencies": { + "@webpack-cli/serve": "^1.7.0", + "babel-core": "^6.26.3", + "babel-loader": "^7.1.5", + "babel-preset-es2015": "^6.24.1", + "copy-webpack-plugin": "^4.5.2", + "css-loader": "^1.0.0", + "uglifyjs-webpack-plugin": "^1.3.0", + "webpack": "^5.74.0", + "webpack-cli": "^4.10.0", + "webpack-dev-server": "4.9.3" + }, + "dependencies": { + "chart.js": "^2.9.4", + "file-loader": "6.2.0", + "fnv-plus": "git+https://git@github.com/tjwebb/fnv-plus#1.3.0", + "html-webpack-plugin": "^3.2.0", + "path": "^0.12.7", + "roslib": "^1.3.0", + "vue": "^2.5.17", + "vue-chartjs": "^3.5.1", + "vue-loader": "^15.4.0", + "vue-router": "^3.0.1", + "vue-style-loader": "^4.1.2", + "vue-template-compiler": "^2.5.17", + "vue2-leaflet": "^1.2.3", + "vuedraggable": "^2.16.0", + "vuex": "^3.0.1" + } +} diff --git a/starter_project/teleop/src/App.vue b/starter_project/teleop/src/App.vue new file mode 100755 index 000000000..8ecf5d18d --- /dev/null +++ b/starter_project/teleop/src/App.vue @@ -0,0 +1,19 @@ + + + + + diff --git a/starter_project/teleop/src/app.js b/starter_project/teleop/src/app.js new file mode 100755 index 000000000..26d469914 --- /dev/null +++ b/starter_project/teleop/src/app.js @@ -0,0 +1,20 @@ +// The Vue build version to load with the `import` command +// (runtime-only or standalone) has been set in webpack.base.conf with an alias. +import Vue from 'vue' +import App from './App.vue' +import ROSLIB from "roslib" +import router from './router' +import 'leaflet/dist/leaflet.css' + +Vue.config.productionTip = false +Vue.prototype.$ros = new ROSLIB.Ros({ + url : 'ws://localhost:9090' +}); + +/* eslint-disable no-new */ +new Vue({ + el: '#app', + router, + components: { App }, + template: '' +}) diff --git a/starter_project/teleop/src/assets/logo.png b/starter_project/teleop/src/assets/logo.png new file mode 100755 index 000000000..ad5721992 --- /dev/null +++ b/starter_project/teleop/src/assets/logo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:03d6d6da2545d3b3402855b8e721b779abaa87d113e69d9329ea6ea6325a83ce +size 6849 diff --git a/starter_project/teleop/src/components/DriveControls.vue b/starter_project/teleop/src/components/DriveControls.vue new file mode 100755 index 000000000..47a835ecf --- /dev/null +++ b/starter_project/teleop/src/components/DriveControls.vue @@ -0,0 +1,73 @@ + + + + + \ No newline at end of file diff --git a/starter_project/teleop/src/components/Menu.vue b/starter_project/teleop/src/components/Menu.vue new file mode 100755 index 000000000..c05579e0b --- /dev/null +++ b/starter_project/teleop/src/components/Menu.vue @@ -0,0 +1,66 @@ + + + + + diff --git a/starter_project/teleop/src/components/MenuButton.vue b/starter_project/teleop/src/components/MenuButton.vue new file mode 100755 index 000000000..22ee2f83c --- /dev/null +++ b/starter_project/teleop/src/components/MenuButton.vue @@ -0,0 +1,43 @@ + + + + + diff --git a/starter_project/teleop/src/index.html b/starter_project/teleop/src/index.html new file mode 100755 index 000000000..8af775c56 --- /dev/null +++ b/starter_project/teleop/src/index.html @@ -0,0 +1,12 @@ + + + + + + mrover-workspace + + +
+ + + diff --git a/starter_project/teleop/src/router/index.js b/starter_project/teleop/src/router/index.js new file mode 100755 index 000000000..8bcc3fffb --- /dev/null +++ b/starter_project/teleop/src/router/index.js @@ -0,0 +1,15 @@ +import Vue from 'vue' +import Router from 'vue-router' +import Menu from '../components/Menu.vue' + +Vue.use(Router) + +export default new Router({ + routes: [ + { + path: '/', + name: 'Menu', + component: Menu + }, + ] +}) diff --git a/starter_project/teleop/src/static/mrover.png b/starter_project/teleop/src/static/mrover.png new file mode 100755 index 000000000..ca0bd4775 --- /dev/null +++ b/starter_project/teleop/src/static/mrover.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2a14558572f10cac45fd24d081d70b83550203205008bf57e8c52aa3f746b364 +size 8210 From e84018219e0944855c198a40b88c37edeb678161 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:04:23 -0500 Subject: [PATCH 166/197] Delete starter_project/teleop/src/router/index.js --- starter_project/teleop/src/router/index.js | 15 --------------- 1 file changed, 15 deletions(-) delete mode 100755 starter_project/teleop/src/router/index.js diff --git a/starter_project/teleop/src/router/index.js b/starter_project/teleop/src/router/index.js deleted file mode 100755 index 8bcc3fffb..000000000 --- a/starter_project/teleop/src/router/index.js +++ /dev/null @@ -1,15 +0,0 @@ -import Vue from 'vue' -import Router from 'vue-router' -import Menu from '../components/Menu.vue' - -Vue.use(Router) - -export default new Router({ - routes: [ - { - path: '/', - name: 'Menu', - component: Menu - }, - ] -}) From 0ee3f0edcf40b0c0a46f8d3f54d67555006531d9 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:04:46 -0500 Subject: [PATCH 167/197] Delete starter_project/teleop/src/index.html --- starter_project/teleop/src/index.html | 12 ------------ 1 file changed, 12 deletions(-) delete mode 100755 starter_project/teleop/src/index.html diff --git a/starter_project/teleop/src/index.html b/starter_project/teleop/src/index.html deleted file mode 100755 index 8af775c56..000000000 --- a/starter_project/teleop/src/index.html +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - mrover-workspace - - -
- - - From 3128092b6a2d3e0324dbf401dc4cbd4e62e8f067 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:05:17 -0500 Subject: [PATCH 168/197] Delete starter_project/teleop/src/components/MenuButton.vue --- .../teleop/src/components/MenuButton.vue | 43 ------------------- 1 file changed, 43 deletions(-) delete mode 100755 starter_project/teleop/src/components/MenuButton.vue diff --git a/starter_project/teleop/src/components/MenuButton.vue b/starter_project/teleop/src/components/MenuButton.vue deleted file mode 100755 index 22ee2f83c..000000000 --- a/starter_project/teleop/src/components/MenuButton.vue +++ /dev/null @@ -1,43 +0,0 @@ - - - - - From cd23e4da2012bb545d9a943a1aa2c24afd25bb30 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:05:38 -0500 Subject: [PATCH 169/197] Delete starter_project/teleop/src/app.js --- starter_project/teleop/src/app.js | 20 -------------------- 1 file changed, 20 deletions(-) delete mode 100755 starter_project/teleop/src/app.js diff --git a/starter_project/teleop/src/app.js b/starter_project/teleop/src/app.js deleted file mode 100755 index 26d469914..000000000 --- a/starter_project/teleop/src/app.js +++ /dev/null @@ -1,20 +0,0 @@ -// The Vue build version to load with the `import` command -// (runtime-only or standalone) has been set in webpack.base.conf with an alias. -import Vue from 'vue' -import App from './App.vue' -import ROSLIB from "roslib" -import router from './router' -import 'leaflet/dist/leaflet.css' - -Vue.config.productionTip = false -Vue.prototype.$ros = new ROSLIB.Ros({ - url : 'ws://localhost:9090' -}); - -/* eslint-disable no-new */ -new Vue({ - el: '#app', - router, - components: { App }, - template: '' -}) From 0d2781013adf249fa27b3029cd11859b28eb4985 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:05:58 -0500 Subject: [PATCH 170/197] Delete starter_project/teleop/src/components/Menu.vue --- .../teleop/src/components/Menu.vue | 66 ------------------- 1 file changed, 66 deletions(-) delete mode 100755 starter_project/teleop/src/components/Menu.vue diff --git a/starter_project/teleop/src/components/Menu.vue b/starter_project/teleop/src/components/Menu.vue deleted file mode 100755 index c05579e0b..000000000 --- a/starter_project/teleop/src/components/Menu.vue +++ /dev/null @@ -1,66 +0,0 @@ - - - - - From a4740a79f52519e52cbdfd5b4e64b54b99a1732b Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:06:35 -0500 Subject: [PATCH 171/197] Delete starter_project/teleop/src/components/DriveControls.vue --- .../teleop/src/components/DriveControls.vue | 73 ------------------- 1 file changed, 73 deletions(-) delete mode 100755 starter_project/teleop/src/components/DriveControls.vue diff --git a/starter_project/teleop/src/components/DriveControls.vue b/starter_project/teleop/src/components/DriveControls.vue deleted file mode 100755 index 47a835ecf..000000000 --- a/starter_project/teleop/src/components/DriveControls.vue +++ /dev/null @@ -1,73 +0,0 @@ - - - - - \ No newline at end of file From 1d403c52038aacde2d1ec24242cfc4a6fb3369e6 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:06:53 -0500 Subject: [PATCH 172/197] Delete starter_project/teleop/src/App.vue --- starter_project/teleop/src/App.vue | 19 ------------------- 1 file changed, 19 deletions(-) delete mode 100755 starter_project/teleop/src/App.vue diff --git a/starter_project/teleop/src/App.vue b/starter_project/teleop/src/App.vue deleted file mode 100755 index 8ecf5d18d..000000000 --- a/starter_project/teleop/src/App.vue +++ /dev/null @@ -1,19 +0,0 @@ - - - - - From 3624874b3432ac13a64192f780c877a8eaa83d30 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:07:11 -0500 Subject: [PATCH 173/197] Delete starter_project/teleop/package.json --- starter_project/teleop/package.json | 37 ----------------------------- 1 file changed, 37 deletions(-) delete mode 100755 starter_project/teleop/package.json diff --git a/starter_project/teleop/package.json b/starter_project/teleop/package.json deleted file mode 100755 index 0319e3bee..000000000 --- a/starter_project/teleop/package.json +++ /dev/null @@ -1,37 +0,0 @@ -{ - "private":true, - "scripts": { - "build": "webpack --config webpack.config.dev.js", - "dev": "webpack --config webpack.config.dev.js", - "serve": "webpack serve --config webpack.config.dev.js" - }, - "devDependencies": { - "@webpack-cli/serve": "^1.7.0", - "babel-core": "^6.26.3", - "babel-loader": "^7.1.5", - "babel-preset-es2015": "^6.24.1", - "copy-webpack-plugin": "^4.5.2", - "css-loader": "^1.0.0", - "uglifyjs-webpack-plugin": "^1.3.0", - "webpack": "^5.74.0", - "webpack-cli": "^4.10.0", - "webpack-dev-server": "4.9.3" - }, - "dependencies": { - "chart.js": "^2.9.4", - "file-loader": "6.2.0", - "fnv-plus": "git+https://git@github.com/tjwebb/fnv-plus#1.3.0", - "html-webpack-plugin": "^3.2.0", - "path": "^0.12.7", - "roslib": "^1.3.0", - "vue": "^2.5.17", - "vue-chartjs": "^3.5.1", - "vue-loader": "^15.4.0", - "vue-router": "^3.0.1", - "vue-style-loader": "^4.1.2", - "vue-template-compiler": "^2.5.17", - "vue2-leaflet": "^1.2.3", - "vuedraggable": "^2.16.0", - "vuex": "^3.0.1" - } -} From 6b105fb19926d6f1e62522974bc5799471e5829b Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:07:34 -0500 Subject: [PATCH 174/197] Delete starter_project/teleop/gui_starter.sh --- starter_project/teleop/gui_starter.sh | 32 --------------------------- 1 file changed, 32 deletions(-) delete mode 100755 starter_project/teleop/gui_starter.sh diff --git a/starter_project/teleop/gui_starter.sh b/starter_project/teleop/gui_starter.sh deleted file mode 100755 index 3814d59e0..000000000 --- a/starter_project/teleop/gui_starter.sh +++ /dev/null @@ -1,32 +0,0 @@ -#!/bin/bash - -# Ensure proper NODE_PATH -export NODE_PATH=/usr/lib/nodejs:/usr/share/nodejs - - -node_executable=node -node_executable_path=$(which "$node_executable") -yarn_executable=yarn -yarn_executable_path=$(which "$yarn_executable") - -if [ -z "$node_executable_path" ] -then - echo "Node installation not found, please run src/teleop/gui/gui_install.sh" - exit 1 -fi -if [ -z "$yarn_executable_path" ] -then - echo "Yarn installation not found, please run src/teleop/gui/gui_install.sh" - exit 1 -fi - -# Check if node_modules up to date -yarn check --verify-tree - -# If not up to date, install -if [ $? == 1 ] -then - yarn install --check-files -fi - -yarn run serve From 6c71aca203a994bf69b68a73366fe0cba8800467 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:07:53 -0500 Subject: [PATCH 175/197] Delete starter_project/teleop/babel.config.js --- starter_project/teleop/babel.config.js | 7 ------- 1 file changed, 7 deletions(-) delete mode 100755 starter_project/teleop/babel.config.js diff --git a/starter_project/teleop/babel.config.js b/starter_project/teleop/babel.config.js deleted file mode 100755 index b74838b52..000000000 --- a/starter_project/teleop/babel.config.js +++ /dev/null @@ -1,7 +0,0 @@ -{ - loaders: [{ - test: /\.jsx?$/, - loaders: ['babel?retainLines=true'], - include: path.join(__dirname, 'src') - }] -} \ No newline at end of file From b522f1164ddee5a588d76134340a6f0ac8056ee1 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:08:13 -0500 Subject: [PATCH 176/197] Delete starter_project/teleop/README.md --- starter_project/teleop/README.md | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 starter_project/teleop/README.md diff --git a/starter_project/teleop/README.md b/starter_project/teleop/README.md deleted file mode 100644 index e69de29bb..000000000 From 5118686aa2bdc2f6808054ce3df63f2246896652 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:08:44 -0500 Subject: [PATCH 177/197] Delete starter_project/autonomy/config/starter_project.world --- .../autonomy/config/starter_project.world | 131 ------------------ 1 file changed, 131 deletions(-) delete mode 100644 starter_project/autonomy/config/starter_project.world diff --git a/starter_project/autonomy/config/starter_project.world b/starter_project/autonomy/config/starter_project.world deleted file mode 100644 index a8235fb42..000000000 --- a/starter_project/autonomy/config/starter_project.world +++ /dev/null @@ -1,131 +0,0 @@ - - - - 1 - 0 0 10 0 -0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -0.5 0.1 -0.9 - - 0 - 0 - 0 - - - 0 0 -9.8 - 6e-06 2.3e-05 -4.2e-05 - - - 0.001 - 1 - 1000 - - - 0.7 0.7 0.7 1 - 0.529 0.808 0.922 1 - 1 - - - - EARTH_WGS84 - 0 - 0 - 0 - 0 - - - 5.5 2.0 0.5 0 0 4.71239 - 1 - - - - - model://post/meshes/4x4_1000-0.dae - - - 10 - - - - - - - - - - - - - - - - - model://post/meshes/4x4_1000-0.dae - - - - 0 - 0 - 0 - - - - 0 0 -0.1 0 0 0 - 1 - - - - - model://starter_project_ground.dae - - - 10 - - - 65535 - - - - - 100 - 50 - - - - - - - - - - - - model://starter_project_ground.dae - - - - - model://soil_normal.png - - - - 0 - 0 - 0 - - - - - 6.01711 -3.68905 2.02916 0 0.275643 2.35619 - orbit - perspective - - - - \ No newline at end of file From 407e4721d7e76243060356e7cac5464d138b0bd7 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:09:05 -0500 Subject: [PATCH 178/197] Delete starter_project/autonomy/config/starter_project.rviz --- .../autonomy/config/starter_project.rviz | 334 ------------------ 1 file changed, 334 deletions(-) delete mode 100644 starter_project/autonomy/config/starter_project.rviz diff --git a/starter_project/autonomy/config/starter_project.rviz b/starter_project/autonomy/config/starter_project.rviz deleted file mode 100644 index ca7a99f10..000000000 --- a/starter_project/autonomy/config/starter_project.rviz +++ /dev/null @@ -1,334 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - - /Image1 - Splitter Ratio: 0.5 - Tree Height: 537 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Name: Time - SyncMode: 0 - SyncSource: Image -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - back_left_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - back_right_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - center_left_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - center_right_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - chassis_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - front_left_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - front_right_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - right_camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 15 - Frames: - All Enabled: true - back_left_wheel_link: - Value: true - back_right_wheel_link: - Value: true - base_link: - Value: true - camera_link: - Value: true - center_left_wheel_link: - Value: true - center_right_wheel_link: - Value: true - chassis_link: - Value: true - front_left_wheel_link: - Value: true - front_right_wheel_link: - Value: true - imu_link: - Value: true - right_camera_link: - Value: true - Marker Alpha: 1 - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - base_link: - chassis_link: - back_left_wheel_link: - {} - back_right_wheel_link: - {} - camera_link: - {} - center_left_wheel_link: - {} - center_right_wheel_link: - {} - front_left_wheel_link: - {} - front_right_wheel_link: - {} - imu_link: - {} - right_camera_link: - {} - Update Interval: 0 - Value: true - - Alpha: 1 - Class: rviz/Axes - Enabled: false - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Show Trail: false - Value: false - - Class: rviz/Image - Enabled: true - Image Topic: /camera/left/image - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 1 - Transport Hint: raw - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /camera/depth/points - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Angle Tolerance: 0.10000000149011612 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Keep: 100 - Name: Odometry - Position Tolerance: 0.10000000149011612 - Queue Size: 10 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Arrow - Topic: /odometry/filtered - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 3.6909778118133545 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 0.1978837251663208 - Y: 0.135016530752182 - Z: 0.33381450176239014 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5552026629447937 - Target Frame: - Yaw: 5.666762828826904 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1047 - Hide Left Dock: false - Hide Right Dock: true - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000379fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002a4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002e7000000cf0000001600ffffff00000001000001000000035dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006f80000003efc0100000002fb0000000800540069006d00650100000000000006f8000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000059c0000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1784 - X: 159 - Y: 125 From f90ef42948f8ffc56cf05fe463acf67f9cb9dc34 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:09:21 -0500 Subject: [PATCH 179/197] Delete starter_project/autonomy/config/soil_normal.png --- starter_project/autonomy/config/soil_normal.png | 1 - 1 file changed, 1 deletion(-) delete mode 120000 starter_project/autonomy/config/soil_normal.png diff --git a/starter_project/autonomy/config/soil_normal.png b/starter_project/autonomy/config/soil_normal.png deleted file mode 120000 index 3399db4f9..000000000 --- a/starter_project/autonomy/config/soil_normal.png +++ /dev/null @@ -1 +0,0 @@ -../../../config/gazebo/env_description/soil_normal.png \ No newline at end of file From 896b7292f618acba975ad248b40716b5b328ab3e Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:09:36 -0500 Subject: [PATCH 180/197] Delete starter_project/autonomy/config/soil_base.png --- starter_project/autonomy/config/soil_base.png | 1 - 1 file changed, 1 deletion(-) delete mode 120000 starter_project/autonomy/config/soil_base.png diff --git a/starter_project/autonomy/config/soil_base.png b/starter_project/autonomy/config/soil_base.png deleted file mode 120000 index 02756e2cc..000000000 --- a/starter_project/autonomy/config/soil_base.png +++ /dev/null @@ -1 +0,0 @@ -../../../config/gazebo/env_description/soil_base.png \ No newline at end of file From 6a49a11d5cb1f3764dc11ab94fb190b07681849e Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:20:55 -0500 Subject: [PATCH 181/197] Delete src/perception/tag_detector/zed/tag_detector.threshold.cpp --- .../zed/tag_detector.threshold.cpp | 54 ------------------- 1 file changed, 54 deletions(-) delete mode 100644 src/perception/tag_detector/zed/tag_detector.threshold.cpp diff --git a/src/perception/tag_detector/zed/tag_detector.threshold.cpp b/src/perception/tag_detector/zed/tag_detector.threshold.cpp deleted file mode 100644 index 41ff4a527..000000000 --- a/src/perception/tag_detector/zed/tag_detector.threshold.cpp +++ /dev/null @@ -1,54 +0,0 @@ -#include "tag_detector.hpp" - -namespace mrover { - - void threshold(cv::InputArray in, cv::OutputArray out, int windowSize, double constant) { - CV_Assert(windowSize >= 3); - - if (windowSize % 2 == 0) windowSize++; // win size must be odd - cv::adaptiveThreshold(in, out, 255, cv::ADAPTIVE_THRESH_MEAN_C, cv::THRESH_BINARY_INV, windowSize, constant); - } - - /** - * Detect tags from raw image using OpenCV and calculate their screen space centers. - * Tag pose information relative to the camera in 3D space is filled in when we receive point cloud data. - * - * @param msg - */ - void TagDetectorNodelet::publishThresholdedImage() { - cvtColor(mImg, mGrayImg, cv::COLOR_BGR2GRAY); - - // number of window sizes (scales) to apply adaptive thresholding - int scaleCount = (mDetectorParams->adaptiveThreshWinSizeMax - mDetectorParams->adaptiveThreshWinSizeMin) / mDetectorParams->adaptiveThreshWinSizeStep + 1; - - // for each value in the interval of thresholding window sizes - for (int scale = 0; scale < scaleCount; ++scale) { - auto it = mThreshPubs.find(scale); - if (it == mThreshPubs.end()) { - ROS_INFO("Creating new publisher for thresholded scale %d", scale); - std::tie(it, std::ignore) = mThreshPubs.emplace(scale, mNh.advertise("tag_detection_threshold_" + std::to_string(scale), 1)); - } - auto& [_, publisher] = *it; - - if (publisher.getNumSubscribers() == 0) continue; - - int windowSize = mDetectorParams->adaptiveThreshWinSizeMin + scale * mDetectorParams->adaptiveThreshWinSizeStep; - threshold(mGrayImg, mGrayImg, windowSize, mDetectorParams->adaptiveThreshConstant); - - mThreshMsg.header.seq = mSeqNum; - mThreshMsg.header.stamp = ros::Time::now(); - mThreshMsg.header.frame_id = "zed2i_left_camera_frame"; - mThreshMsg.height = mGrayImg.rows; - mThreshMsg.width = mGrayImg.cols; - mThreshMsg.encoding = sensor_msgs::image_encodings::MONO8; - mThreshMsg.step = mGrayImg.step; - mThreshMsg.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; - size_t size = mThreshMsg.step * mThreshMsg.height; - mThreshMsg.data.resize(size); - std::memcpy(mThreshMsg.data.data(), mGrayImg.data, size); - - publisher.publish(mThreshMsg); - } - } - -} // namespace mrover From 0f679903b3b436deb8c874d7fa25e9e59f60f98c Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:26:08 -0500 Subject: [PATCH 182/197] Delete src/preload/format --- src/preload/format | 17 ----------------- 1 file changed, 17 deletions(-) delete mode 100644 src/preload/format diff --git a/src/preload/format b/src/preload/format deleted file mode 100644 index 1d8d28997..000000000 --- a/src/preload/format +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once - -// macOS does not have std::format yet -// So we define it and redirect it to fmt::format - -#if defined(__APPLE__) -#include - -namespace std { - template - FMT_NODISCARD FMT_INLINE auto format(fmt::format_string fmt, T&&... args) -> std::string { - return fmt::format(fmt, std::forward(args)...); - } -} // namespace std -#else -#include_next -#endif From 63f384860047c3cffaa95fc3174644221e6ea44c Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:26:56 -0500 Subject: [PATCH 183/197] Delete src/perception/tag_detector/zed/tag_detector.processing.cpp --- .../zed/tag_detector.processing.cpp | 167 ------------------ 1 file changed, 167 deletions(-) delete mode 100644 src/perception/tag_detector/zed/tag_detector.processing.cpp diff --git a/src/perception/tag_detector/zed/tag_detector.processing.cpp b/src/perception/tag_detector/zed/tag_detector.processing.cpp deleted file mode 100644 index 2cc6c0e58..000000000 --- a/src/perception/tag_detector/zed/tag_detector.processing.cpp +++ /dev/null @@ -1,167 +0,0 @@ -#include "tag_detector.hpp" - -#include "point.hpp" - -namespace mrover { - - /** - * @brief Retrieve the pose of the tag in camera space - * @param msg 3D Point Cloud with points stored relative to the camera - * @param u X Pixel Position - * @param v Y Pixel Position - */ - auto TagDetectorNodelet::getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) -> std::optional { - assert(cloudPtr); - - if (u >= cloudPtr->width || v >= cloudPtr->height) { - NODELET_WARN("Tag center out of bounds: [%zu %zu]", u, v); - return std::nullopt; - } - - Point point = reinterpret_cast(cloudPtr->data.data())[u + v * cloudPtr->width]; - - if (!std::isfinite(point.x) || !std::isfinite(point.y) || - !std::isfinite(point.z)) { - NODELET_WARN("Tag center point not finite: [%f %f %f]", point.x, point.y, point.z); - return std::nullopt; - } - - return std::make_optional(R3{point.x, point.y, point.z}, SO3{}); - } - - /** - * For each tag we have detected so far, fuse point cloud information. - * This information is where it is in the world. - * - * @param msg Point cloud message - */ - auto TagDetectorNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void { - assert(msg); - assert(msg->height > 0); - assert(msg->width > 0); - - if (!mEnableDetections) return; - - mProfiler.beginLoop(); - - NODELET_DEBUG("Got point cloud %d", msg->header.seq); - - // OpenCV needs a dense BGR image |BGR|...| but out point cloud is - // |BGRAXYZ...|...| So we need to copy the data into the correct format - if (static_cast(msg->height) != mImg.rows || - static_cast(msg->width) != mImg.cols) { - NODELET_INFO("Image size changed from [%d %d] to [%u %u]", mImg.cols, mImg.rows, msg->width, msg->height); - mImg = cv::Mat{static_cast(msg->height), static_cast(msg->width), CV_8UC3, cv::Scalar{0, 0, 0}}; - } - auto* pixelPtr = reinterpret_cast(mImg.data); - auto* pointPtr = reinterpret_cast(msg->data.data()); - std::for_each(std::execution::par_unseq, pixelPtr, pixelPtr + mImg.total(), [&](cv::Vec3b& pixel) { - size_t i = &pixel - pixelPtr; - pixel[0] = pointPtr[i].b; - pixel[1] = pointPtr[i].g; - pixel[2] = pointPtr[i].r; - }); - mProfiler.measureEvent("Convert"); - - // Call thresholding - publishThresholdedImage(); - mProfiler.measureEvent("Threshold"); - - // Detect the tag vertices in screen space and their respective ids - // {mImmediateCorneres, mImmediateIds} are the outputs from OpenCV - cv::aruco::detectMarkers(mImg, mDictionary, mImmediateCorners, mImmediateIds, mDetectorParams); - NODELET_DEBUG("OpenCV detect size: %zu", mImmediateIds.size()); - mProfiler.measureEvent("OpenCV Detect"); - - // Update ID, image center, and increment hit count for all detected tags - for (std::size_t i = 0; i < mImmediateIds.size(); ++i) { - int id = mImmediateIds[i]; - Tag& tag = mTags[id]; - tag.hitCount = std::clamp(tag.hitCount + mTagIncrementWeight, 0, mMaxHitCount); - tag.id = id; - tag.imageCenter = std::reduce(mImmediateCorners[i].begin(), mImmediateCorners[i].end()) / static_cast(mImmediateCorners[i].size()); - tag.tagInCam = getTagInCamFromPixel(msg, std::lround(tag.imageCenter.x), std::lround(tag.imageCenter.y)); - - if (tag.tagInCam) { - // Publish tag to immediate - std::string immediateFrameId = "immediateFiducial" + std::to_string(tag.id); - SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, tag.tagInCam.value()); - } - } - - // Handle tags that were not seen this update - // Decrement their hit count and remove if they hit zero - auto it = mTags.begin(); - while (it != mTags.end()) { - auto& [id, tag] = *it; - if (std::ranges::find(mImmediateIds, id) == mImmediateIds.end()) { - tag.hitCount -= mTagDecrementWeight; - tag.tagInCam = std::nullopt; - if (tag.hitCount <= 0) { - it = mTags.erase(it); - continue; - } - } - ++it; - } - - // Publish all tags to the tf tree that have been seen enough times - for (auto const& [id, tag]: mTags) { - if (tag.hitCount >= mMinHitCountBeforePublish && tag.tagInCam) { - try { - std::string immediateFrameId = "immediateFiducial" + std::to_string(tag.id); - // Publish tag to odom - std::string const& parentFrameId = mUseOdom ? mOdomFrameId : mMapFrameId; - SE3 tagInParent = SE3::fromTfTree(mTfBuffer, parentFrameId, immediateFrameId); - SE3::pushToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), parentFrameId, tagInParent); - } catch (tf2::ExtrapolationException const&) { - NODELET_WARN("Old data for immediate tag"); - } catch (tf2::LookupException const&) { - NODELET_WARN("Expected transform for immediate tag"); - } catch (tf2::ConnectivityException const&) { - NODELET_WARN("Expected connection to odom frame. Is visual odometry running?"); - } - } - } - - if (mPublishImages && mImgPub.getNumSubscribers()) { - - cv::aruco::drawDetectedMarkers(mImg, mImmediateCorners, mImmediateIds); - // Max number of tags the hit counter can display = 10; - if (!mTags.empty()) { - // TODO: remove some magic numbers in this block - int tagCount = 1; - auto tagBoxWidth = static_cast(mImg.cols / (mTags.size() * 2)); - for (auto& [id, tag]: mTags) { - cv::Scalar color{255, 0, 0}; - cv::Point pt{tagBoxWidth * tagCount, mImg.rows / 10}; - std::string text = "id" + std::to_string(id) + ":" + std::to_string(tag.hitCount); - cv::putText(mImg, text, pt, cv::FONT_HERSHEY_COMPLEX, mImg.cols / 800.0, color, mImg.cols / 300); - - ++tagCount; - } - } - mImgMsg.header.seq = mSeqNum; - mImgMsg.header.stamp = ros::Time::now(); - mImgMsg.header.frame_id = "zed2i_left_camera_frame"; - mImgMsg.height = mImg.rows; - mImgMsg.width = mImg.cols; - mImgMsg.encoding = sensor_msgs::image_encodings::BGR8; - mImgMsg.step = mImg.step; - mImgMsg.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; - size_t size = mImgMsg.step * mImgMsg.height; - mImgMsg.data.resize(size); - std::memcpy(mImgMsg.data.data(), mImg.data, size); - mImgPub.publish(mImgMsg); - } - - size_t detectedCount = mImmediateIds.size(); - NODELET_INFO_COND(!mPrevDetectedCount.has_value() || detectedCount != mPrevDetectedCount.value(), "Detected %zu markers", detectedCount); - mPrevDetectedCount = detectedCount; - - mProfiler.measureEvent("Publish"); - - mSeqNum++; - } - -} // namespace mrover \ No newline at end of file From 7fc34860b7446b6f32f376d05d528d0dc563f154 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:27:15 -0500 Subject: [PATCH 184/197] Delete src/perception/tag_detector/zed/tag_detector.hpp --- .../tag_detector/zed/tag_detector.hpp | 70 ------------------- 1 file changed, 70 deletions(-) delete mode 100644 src/perception/tag_detector/zed/tag_detector.hpp diff --git a/src/perception/tag_detector/zed/tag_detector.hpp b/src/perception/tag_detector/zed/tag_detector.hpp deleted file mode 100644 index b062107d1..000000000 --- a/src/perception/tag_detector/zed/tag_detector.hpp +++ /dev/null @@ -1,70 +0,0 @@ -#include "pch.hpp" - -namespace mrover { - - struct Tag { - int id = -1; - int hitCount = 0; - cv::Point2f imageCenter{}; - std::optional tagInCam; - }; - - class TagDetectorNodelet : public nodelet::Nodelet { - private: - ros::NodeHandle mNh, mPnh; - - ros::Publisher mImgPub; - std::unordered_map mThreshPubs; // Map from threshold scale to publisher - ros::ServiceServer mServiceEnableDetections; - - ros::Subscriber mPcSub; - ros::Subscriber mImgSub; - tf2_ros::Buffer mTfBuffer; - tf2_ros::TransformListener mTfListener{mTfBuffer}; - tf2_ros::TransformBroadcaster mTfBroadcaster; - - bool mEnableDetections = true; - bool mUseOdom{}; - std::string mOdomFrameId, mMapFrameId, mCameraFrameId; - bool mPublishImages{}; // If set, we publish the images with the tags drawn on top - int mMinHitCountBeforePublish{}; - int mMaxHitCount{}; - int mTagIncrementWeight{}; - int mTagDecrementWeight{}; - - cv::Ptr mDetectorParams; - cv::Ptr mDictionary; - - cv::Mat mImg; - cv::Mat mGrayImg; - sensor_msgs::Image mImgMsg; - sensor_msgs::Image mThreshMsg; - uint32_t mSeqNum{}; - std::optional mPrevDetectedCount; // Log spam prevention - std::vector> mImmediateCorners; - std::vector mImmediateIds; - std::unordered_map mTags; - dynamic_reconfigure::Server mConfigServer; - dynamic_reconfigure::Server::CallbackType mCallbackType; - - LoopProfiler mProfiler{"Tag Detector"}; - - void onInit() override; - - void publishThresholdedImage(); - - std::optional getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v); - - public: - TagDetectorNodelet() = default; - - ~TagDetectorNodelet() override = default; - - void pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg); - - void configCallback(mrover::TagDetectorParamsConfig& config, uint32_t level); - - bool enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); - }; - -} // namespace mrover From 29ac5310080c385bb385e4d6e1de1e4cfe07404c Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:27:35 -0500 Subject: [PATCH 185/197] Delete src/perception/tag_detector/zed/tag_detector.cpp --- .../tag_detector/zed/tag_detector.cpp | 144 ------------------ 1 file changed, 144 deletions(-) delete mode 100644 src/perception/tag_detector/zed/tag_detector.cpp diff --git a/src/perception/tag_detector/zed/tag_detector.cpp b/src/perception/tag_detector/zed/tag_detector.cpp deleted file mode 100644 index 66130f1a6..000000000 --- a/src/perception/tag_detector/zed/tag_detector.cpp +++ /dev/null @@ -1,144 +0,0 @@ -#include "tag_detector.hpp" - -namespace mrover { - - void TagDetectorNodelet::onInit() { - mNh = getMTNodeHandle(); - mPnh = getMTPrivateNodeHandle(); - mDetectorParams = cv::makePtr(); - auto defaultDetectorParams = cv::makePtr(); - int dictionaryNumber; - - mNh.param("use_odom_frame", mUseOdom, false); - mNh.param("odom_frame", mOdomFrameId, "odom"); - mNh.param("world_frame", mMapFrameId, "map"); - mNh.param("camera_frame", mCameraFrameId, "zed2i_left_camera_frame"); - - mPnh.param("publish_images", mPublishImages, true); - mPnh.param("dictionary", dictionaryNumber, static_cast(cv::aruco::DICT_4X4_50)); - mPnh.param("min_hit_count_before_publish", mMinHitCountBeforePublish, 5); - mPnh.param("max_hit_count", mMaxHitCount, 5); - mPnh.param("tag_increment_weight", mTagIncrementWeight, 2); - mPnh.param("tag_decrement_weight", mTagDecrementWeight, 1); - - mImgPub = mNh.advertise("tag_detection", 1); - mDictionary = cv::makePtr(cv::aruco::getPredefinedDictionary(dictionaryNumber)); - - mPcSub = mNh.subscribe("camera/left/points", 1, &TagDetectorNodelet::pointCloudCallback, this); - mServiceEnableDetections = mNh.advertiseService("enable_detections", &TagDetectorNodelet::enableDetectionsCallback, this); - - // Lambda handles passing class pointer (implicit first parameter) to configCallback - mCallbackType = [this](mrover::TagDetectorParamsConfig& config, uint32_t level) { configCallback(config, level); }; - mConfigServer.setCallback(mCallbackType); - - mPnh.param("adaptiveThreshConstant", - mDetectorParams->adaptiveThreshConstant, defaultDetectorParams->adaptiveThreshConstant); - mPnh.param("adaptiveThreshWinSizeMax", - mDetectorParams->adaptiveThreshWinSizeMax, defaultDetectorParams->adaptiveThreshWinSizeMax); - mPnh.param("adaptiveThreshWinSizeMin", - mDetectorParams->adaptiveThreshWinSizeMin, defaultDetectorParams->adaptiveThreshWinSizeMin); - mPnh.param("adaptiveThreshWinSizeStep", - mDetectorParams->adaptiveThreshWinSizeStep, defaultDetectorParams->adaptiveThreshWinSizeStep); - mPnh.param("cornerRefinementMaxIterations", - mDetectorParams->cornerRefinementMaxIterations, - defaultDetectorParams->cornerRefinementMaxIterations); - mPnh.param("cornerRefinementMinAccuracy", - mDetectorParams->cornerRefinementMinAccuracy, - defaultDetectorParams->cornerRefinementMinAccuracy); - mPnh.param("cornerRefinementWinSize", - mDetectorParams->cornerRefinementWinSize, defaultDetectorParams->cornerRefinementWinSize); - - bool doCornerRefinement = true; - mPnh.param("doCornerRefinement", doCornerRefinement, true); - if (doCornerRefinement) { - bool cornerRefinementSubPix = true; - mPnh.param("cornerRefinementSubPix", cornerRefinementSubPix, true); - if (cornerRefinementSubPix) { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; - } else { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR; - } - } else { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; - } - - mPnh.param("errorCorrectionRate", - mDetectorParams->errorCorrectionRate, defaultDetectorParams->errorCorrectionRate); - mPnh.param("minCornerDistanceRate", - mDetectorParams->minCornerDistanceRate, defaultDetectorParams->minCornerDistanceRate); - mPnh.param("markerBorderBits", - mDetectorParams->markerBorderBits, defaultDetectorParams->markerBorderBits); - mPnh.param("maxErroneousBitsInBorderRate", - mDetectorParams->maxErroneousBitsInBorderRate, - defaultDetectorParams->maxErroneousBitsInBorderRate); - mPnh.param("minDistanceToBorder", - mDetectorParams->minDistanceToBorder, defaultDetectorParams->minDistanceToBorder); - mPnh.param("minMarkerDistanceRate", - mDetectorParams->minMarkerDistanceRate, defaultDetectorParams->minMarkerDistanceRate); - mPnh.param("minMarkerPerimeterRate", - mDetectorParams->minMarkerPerimeterRate, defaultDetectorParams->minMarkerPerimeterRate); - mPnh.param("maxMarkerPerimeterRate", - mDetectorParams->maxMarkerPerimeterRate, defaultDetectorParams->maxMarkerPerimeterRate); - mPnh.param("minOtsuStdDev", mDetectorParams->minOtsuStdDev, defaultDetectorParams->minOtsuStdDev); - mPnh.param("perspectiveRemoveIgnoredMarginPerCell", - mDetectorParams->perspectiveRemoveIgnoredMarginPerCell, - defaultDetectorParams->perspectiveRemoveIgnoredMarginPerCell); - mPnh.param("perspectiveRemovePixelPerCell", - mDetectorParams->perspectiveRemovePixelPerCell, - defaultDetectorParams->perspectiveRemovePixelPerCell); - mPnh.param("polygonalApproxAccuracyRate", - mDetectorParams->polygonalApproxAccuracyRate, - defaultDetectorParams->polygonalApproxAccuracyRate); - - NODELET_INFO("Tag detection ready, use odom frame: %s, min hit count: %d, max hit count: %d, hit increment weight: %d, hit decrement weight: %d", mUseOdom ? "true" : "false", mMinHitCountBeforePublish, mMaxHitCount, mTagIncrementWeight, mTagDecrementWeight); - } - - void TagDetectorNodelet::configCallback(mrover::TagDetectorParamsConfig& config, uint32_t level) { - // Don't load initial config, since it will overwrite the rosparam settings - if (level == std::numeric_limits::max()) return; - - mDetectorParams->adaptiveThreshConstant = config.adaptiveThreshConstant; - mDetectorParams->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin; - mDetectorParams->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax; - mDetectorParams->adaptiveThreshWinSizeStep = config.adaptiveThreshWinSizeStep; - mDetectorParams->cornerRefinementMaxIterations = config.cornerRefinementMaxIterations; - mDetectorParams->cornerRefinementMinAccuracy = config.cornerRefinementMinAccuracy; - mDetectorParams->cornerRefinementWinSize = config.cornerRefinementWinSize; - if (config.doCornerRefinement) { - if (config.cornerRefinementSubpix) { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; - } else { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR; - } - } else { - mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; - } - mDetectorParams->errorCorrectionRate = config.errorCorrectionRate; - mDetectorParams->minCornerDistanceRate = config.minCornerDistanceRate; - mDetectorParams->markerBorderBits = config.markerBorderBits; - mDetectorParams->maxErroneousBitsInBorderRate = config.maxErroneousBitsInBorderRate; - mDetectorParams->minDistanceToBorder = config.minDistanceToBorder; - mDetectorParams->minMarkerDistanceRate = config.minMarkerDistanceRate; - mDetectorParams->minMarkerPerimeterRate = config.minMarkerPerimeterRate; - mDetectorParams->maxMarkerPerimeterRate = config.maxMarkerPerimeterRate; - mDetectorParams->minOtsuStdDev = config.minOtsuStdDev; - mDetectorParams->perspectiveRemoveIgnoredMarginPerCell = config.perspectiveRemoveIgnoredMarginPerCell; - mDetectorParams->perspectiveRemovePixelPerCell = config.perspectiveRemovePixelPerCell; - mDetectorParams->polygonalApproxAccuracyRate = config.polygonalApproxAccuracyRate; - } - - bool TagDetectorNodelet::enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { - mEnableDetections = req.data; - if (mEnableDetections) { - res.message = "Enabled tag detections."; - NODELET_INFO("Enabled tag detections."); - } else { - res.message = "Disabled tag detections."; - NODELET_INFO("Disabled tag detections."); - } - - res.success = true; - return true; - } - -} // namespace mrover From bf460bc53f9e2c065d9c8f041e273e717b9d76e6 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:28:09 -0500 Subject: [PATCH 186/197] Delete src/perception/tag_detector/zed/pch.hpp --- src/perception/tag_detector/zed/pch.hpp | 34 ------------------------- 1 file changed, 34 deletions(-) delete mode 100644 src/perception/tag_detector/zed/pch.hpp diff --git a/src/perception/tag_detector/zed/pch.hpp b/src/perception/tag_detector/zed/pch.hpp deleted file mode 100644 index 47c69dcae..000000000 --- a/src/perception/tag_detector/zed/pch.hpp +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include From 098f0073c2bc9946cf1d96b04c3f4719f5d9b961 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:28:57 -0500 Subject: [PATCH 187/197] Delete config/DetectorParams.cfg --- config/DetectorParams.cfg | 92 --------------------------------------- 1 file changed, 92 deletions(-) delete mode 100755 config/DetectorParams.cfg diff --git a/config/DetectorParams.cfg b/config/DetectorParams.cfg deleted file mode 100755 index 38165dc1e..000000000 --- a/config/DetectorParams.cfg +++ /dev/null @@ -1,92 +0,0 @@ -#!/usr/bin/env python3 -PACKAGE = "mrover" - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -gen.add("adaptiveThreshConstant", double_t, 0, - "Constant for adaptive thresholding before finding contours", - 7, 0, 255) - -gen.add("adaptiveThreshWinSizeMin", int_t, 0, - "Minimum window size for adaptive thresholding before finding contours", - 3, 1, 100) - -gen.add("adaptiveThreshWinSizeMax", int_t, 0, - "Maximum window size for adaptive thresholding before finding contours", - 23, 1, 100) - -gen.add("adaptiveThreshWinSizeStep", int_t, 0, - "Increments from adaptiveThreshWinSizeMin to adaptiveThreshWinSizeMax during the thresholding", - 10, 1, 23) - -gen.add("cornerRefinementMaxIterations", int_t, 0, - "Maximum number of iterations for stop criteria of the corner refinement process", - 30, 1, 100) - -gen.add("cornerRefinementMinAccuracy", double_t, 0, - "Minimum error for the stop criteria of the corner refinement process", - 0.1, 0, 1) - -gen.add("cornerRefinementWinSize", int_t, 0, - "Window size for the corner refinement process (in pixels)", - 5, 1, 100) - -gen.add("doCornerRefinement", bool_t, 0, - "Whether to do subpixel corner refinement", - True) - -gen.add("cornerRefinementSubpix", bool_t, 0, - "Whether to do subpixel corner refinement (true) or contour (false)", - True) - -gen.add("errorCorrectionRate", double_t, 0, - "Error correction rate respect to the maximum error correction capability for each dictionary", - 0.6, 0, 1) - -gen.add("minCornerDistanceRate", double_t, 0, - "Minimum distance between corners for detected markers relative to its perimeter", - 0.05, 0, 1) - -gen.add("markerBorderBits", int_t, 0, - "Number of bits of the marker border, i.e. marker border width", - 1, 0, 100) - -gen.add("maxErroneousBitsInBorderRate", double_t, 0, - "Maximum number of accepted erroneous bits in the border (i.e. number of allowed white bits in the border)", - 0.35, 0, 1) - -gen.add("minDistanceToBorder", int_t, 0, - "Minimum distance of any corner to the image border for detected markers (in pixels)", - 3, 0, 100) - -gen.add("minMarkerDistanceRate", double_t, 0, - "minimum mean distance beetween two marker corners to be considered similar, so that the smaller one is removed. The rate is relative to the smaller perimeter of the two markers", - 0.05, 0, 1) - -gen.add("minMarkerPerimeterRate", double_t, 0, - "Determine minumum perimeter for marker contour to be detected. This is defined as a rate respect to the maximum dimension of the input image", - 0.03, 0, 1) - -gen.add("maxMarkerPerimeterRate", double_t, 0, - "Determine maximum perimeter for marker contour to be detected. This is defined as a rate respect to the maximum dimension of the input image", - 4.0, 0, 1) - -gen.add("minOtsuStdDev", double_t, 0, - "Minimun standard deviation in pixels values during the decodification step to apply Otsu thresholding (otherwise, all the bits are set to 0 or 1 depending on mean higher than 128 or not)", - 5.0, 0, 255) - -gen.add("perspectiveRemoveIgnoredMarginPerCell", double_t, 0, - "Width of the margin of pixels on each cell not considered for the determination of the cell bit. Represents the rate respect to the total size of the cell, i.e. perpectiveRemovePixelPerCell", - 0.13, 0, 1) - -gen.add("perspectiveRemovePixelPerCell", int_t, 0, - "Number of bits (per dimension) for each cell of the marker when removing the perspective", - 4, 1, 100) - -gen.add("polygonalApproxAccuracyRate", double_t, 0, - "Minimum accuracy during the polygonal approximation process to determine which contours are squares", - 0.08, 0, 1) - -exit(gen.generate(PACKAGE, "zed_tag_detector", "DetectorParams")) From 1c188d7ab082cdeea7b521df2776e9983395f4a9 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:29:17 -0500 Subject: [PATCH 188/197] Delete config/navigation.yaml --- config/navigation.yaml | 54 ------------------------------------------ 1 file changed, 54 deletions(-) delete mode 100644 config/navigation.yaml diff --git a/config/navigation.yaml b/config/navigation.yaml deleted file mode 100644 index 153a40dc7..000000000 --- a/config/navigation.yaml +++ /dev/null @@ -1,54 +0,0 @@ -drive: - map: - max_driving_effort: 1.0 - min_driving_effort: -1.0 - max_turning_effort: 0.7 - min_turning_effort: -0.7 - turning_p: 3.0 - driving_p: 20.0 - odom: - max_driving_effort: 0.7 - min_driving_effort: -0.7 - max_turning_effort: 0.5 - min_turning_effort: -0.5 - turning_p: 3.0 - driving_p: 10.0 - lookahead_distance: 1.0 - -search: - stop_thresh: 0.5 - drive_fwd_thresh: 0.34 - coverage_radius: 20 - segments_per_rotation: 8 - distance_between_spirals: 3 - -single_fiducial: - stop_thresh: 1.0 - fiducial_stop_threshold: 1.75 - post_avoidance_multiplier: 1.42 - post_radius: 0.7 - -waypoint: - stop_thresh: 0.5 - drive_fwd_thresh: 0.34 - no_fiducial: -1 - -recovery: - stop_thresh: 0.2 - drive_fwd_thresh: 0.34 - recovery_distance: 2.0 # distance the rover moves back in each leg of j-turn - give_up_time: 10.0 - -failure_identification: - dataframe_max_size: 200 - test_recovery_state: False - post_recovery_grace_period: 5.0 - -watchdog: - window_size: 100 #size of window we are looking at for being stuck - angular_threshold: 0.1 - linear_threshold: 0.2 - -long_range: - distance_ahead: 20 - time_threshold: 5 From 28723907cc43b0c06b6a046b4c649f171a1cc575 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:29:53 -0500 Subject: [PATCH 189/197] Delete config/TagDetectorParams.cfg --- config/TagDetectorParams.cfg | 92 ------------------------------------ 1 file changed, 92 deletions(-) delete mode 100755 config/TagDetectorParams.cfg diff --git a/config/TagDetectorParams.cfg b/config/TagDetectorParams.cfg deleted file mode 100755 index 390d6903a..000000000 --- a/config/TagDetectorParams.cfg +++ /dev/null @@ -1,92 +0,0 @@ -#!/usr/bin/env python3 -PACKAGE = "mrover" - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -gen.add("adaptiveThreshConstant", double_t, 0, - "Constant for adaptive thresholding before finding contours", - 7, 0, 255) - -gen.add("adaptiveThreshWinSizeMin", int_t, 0, - "Minimum window size for adaptive thresholding before finding contours", - 3, 1, 100) - -gen.add("adaptiveThreshWinSizeMax", int_t, 0, - "Maximum window size for adaptive thresholding before finding contours", - 23, 1, 100) - -gen.add("adaptiveThreshWinSizeStep", int_t, 0, - "Increments from adaptiveThreshWinSizeMin to adaptiveThreshWinSizeMax during the thresholding", - 10, 1, 23) - -gen.add("cornerRefinementMaxIterations", int_t, 0, - "Maximum number of iterations for stop criteria of the corner refinement process", - 30, 1, 100) - -gen.add("cornerRefinementMinAccuracy", double_t, 0, - "Minimum error for the stop criteria of the corner refinement process", - 0.1, 0, 1) - -gen.add("cornerRefinementWinSize", int_t, 0, - "Window size for the corner refinement process (in pixels)", - 5, 1, 100) - -gen.add("doCornerRefinement", bool_t, 0, - "Whether to do subpixel corner refinement", - True) - -gen.add("cornerRefinementSubpix", bool_t, 0, - "Whether to do subpixel corner refinement (true) or contour (false)", - True) - -gen.add("errorCorrectionRate", double_t, 0, - "Error correction rate respect to the maximum error correction capability for each dictionary", - 0.6, 0, 1) - -gen.add("minCornerDistanceRate", double_t, 0, - "Minimum distance between corners for detected markers relative to its perimeter", - 0.05, 0, 1) - -gen.add("markerBorderBits", int_t, 0, - "Number of bits of the marker border, i.e. marker border width", - 1, 0, 100) - -gen.add("maxErroneousBitsInBorderRate", double_t, 0, - "Maximum number of accepted erroneous bits in the border (i.e. number of allowed white bits in the border)", - 0.35, 0, 1) - -gen.add("minDistanceToBorder", int_t, 0, - "Minimum distance of any corner to the image border for detected markers (in pixels)", - 3, 0, 100) - -gen.add("minMarkerDistanceRate", double_t, 0, - "minimum mean distance beetween two marker corners to be considered similar, so that the smaller one is removed. The rate is relative to the smaller perimeter of the two markers", - 0.05, 0, 1) - -gen.add("minMarkerPerimeterRate", double_t, 0, - "Determine minumum perimeter for marker contour to be detected. This is defined as a rate respect to the maximum dimension of the input image", - 0.03, 0, 1) - -gen.add("maxMarkerPerimeterRate", double_t, 0, - "Determine maximum perimeter for marker contour to be detected. This is defined as a rate respect to the maximum dimension of the input image", - 4.0, 0, 1) - -gen.add("minOtsuStdDev", double_t, 0, - "Minimun standard deviation in pixels values during the decodification step to apply Otsu thresholding (otherwise, all the bits are set to 0 or 1 depending on mean higher than 128 or not)", - 5.0, 0, 255) - -gen.add("perspectiveRemoveIgnoredMarginPerCell", double_t, 0, - "Width of the margin of pixels on each cell not considered for the determination of the cell bit. Represents the rate respect to the total size of the cell, i.e. perpectiveRemovePixelPerCell", - 0.13, 0, 1) - -gen.add("perspectiveRemovePixelPerCell", int_t, 0, - "Number of bits (per dimension) for each cell of the marker when removing the perspective", - 4, 1, 100) - -gen.add("polygonalApproxAccuracyRate", double_t, 0, - "Minimum accuracy during the polygonal approximation process to determine which contours are squares", - 0.08, 0, 1) - -exit(gen.generate(PACKAGE, "long_range_tag_detector", "TagDetectorParams")) From 921597c99e5351772d2b5deb3670806485f01e2e Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:30:41 -0500 Subject: [PATCH 190/197] Delete plugins/tag_detector_plugin.xml --- plugins/tag_detector_plugin.xml | 6 ------ 1 file changed, 6 deletions(-) delete mode 100644 plugins/tag_detector_plugin.xml diff --git a/plugins/tag_detector_plugin.xml b/plugins/tag_detector_plugin.xml deleted file mode 100644 index babad0bed..000000000 --- a/plugins/tag_detector_plugin.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - \ No newline at end of file From 087d8a8f134a7a172e9f8bfda67e30c6cdd6ed64 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:31:21 -0500 Subject: [PATCH 191/197] Delete scripts/debug_course_publisher.py --- scripts/debug_course_publisher.py | 37 ------------------------------- 1 file changed, 37 deletions(-) delete mode 100755 scripts/debug_course_publisher.py diff --git a/scripts/debug_course_publisher.py b/scripts/debug_course_publisher.py deleted file mode 100755 index c9d2314de..000000000 --- a/scripts/debug_course_publisher.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python3 - -import numpy as np - -import rospy -from mrover.msg import Waypoint, WaypointType -from util.SE3 import SE3 -from util.course_publish_helpers import convert_waypoint_to_gps, publish_waypoints - -""" -The purpose of this file is for testing courses in the sim -without needing the auton GUI. You can add waypoints with -and without tags and these will get published to nav -""" - -if __name__ == "__main__": - rospy.init_node("debug_course_publisher") - - publish_waypoints( - [ - convert_waypoint_to_gps(waypoint) - for waypoint in [ - ( - Waypoint(tag_id=0, type=WaypointType(val=WaypointType.MALLET)), - SE3(position=np.array([4, 2, 0])), - ) - # ( - # Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)), - # SE3(position=np.array([3, 0, 0])), - # ) - # ( - # Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)), - # SE3(position=np.array([11, -10, 0])), - # ) - ] - ] - ) From 85fb4af4ece77ce6ca0c9723aa3456b3298faa37 Mon Sep 17 00:00:00 2001 From: jbrhm <134429827+jbrhm@users.noreply.github.com> Date: Sun, 11 Feb 2024 12:32:39 -0500 Subject: [PATCH 192/197] Delete src/perception/tag_detector/README.md --- src/perception/tag_detector/README.md | 13 ------------- 1 file changed, 13 deletions(-) delete mode 100644 src/perception/tag_detector/README.md diff --git a/src/perception/tag_detector/README.md b/src/perception/tag_detector/README.md deleted file mode 100644 index a80de7d8f..000000000 --- a/src/perception/tag_detector/README.md +++ /dev/null @@ -1,13 +0,0 @@ -# [Perception](https://github.com/umrover/mrover-ros/wiki/Perception) - -### Code Layout - - -## Short Range Detection -- [tag_detector.cpp](./tag_detector.cpp) Mainly ROS node setup (topics, parameters, etc.) -- [tag_detector.processing.cpp](./tag_detector.processing.cpp) Processes inputs (camera feed and pointcloud) to estimate locations of the ArUco fiducials - - -## Long Range Detection -- [long_range_tag_detector.cpp](./long_range_tag_detector.cpp) Mainly ROS node setup (topics, parameters, etc.) -- [long_range_tag_detector.processing.cpp](./long_range_tag_detector.processing.cpp) Does all the input processing (ArUco recognition) and publishes the information. From 6207ad8665811c0ea1b4cbe9c85423b5a34e65b0 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 11 Feb 2024 19:08:16 -0500 Subject: [PATCH 193/197] Revert files that should not have been changed --- config/navigation.yaml | 50 +++++++++++++++++++++++++++ launch/auton.launch | 57 ------------------------------- launch/new_sim.launch | 17 --------- plugins/tag_detector_plugin.xml | 6 ++++ scripts/debug_course_publisher.py | 32 +++++++++++++++++ src/preload/format | 17 +++++++++ 6 files changed, 105 insertions(+), 74 deletions(-) create mode 100644 config/navigation.yaml delete mode 100644 launch/auton.launch delete mode 100644 launch/new_sim.launch create mode 100644 plugins/tag_detector_plugin.xml create mode 100755 scripts/debug_course_publisher.py create mode 100644 src/preload/format diff --git a/config/navigation.yaml b/config/navigation.yaml new file mode 100644 index 000000000..fe5a6adfb --- /dev/null +++ b/config/navigation.yaml @@ -0,0 +1,50 @@ +drive: + map: + max_driving_effort: 1.0 + min_driving_effort: -1.0 + max_turning_effort: 0.7 + min_turning_effort: -0.7 + turning_p: 3.0 + driving_p: 20.0 + odom: + max_driving_effort: 0.7 + min_driving_effort: -0.7 + max_turning_effort: 0.5 + min_turning_effort: -0.5 + turning_p: 3.0 + driving_p: 10.0 + lookahead_distance: 1.0 + +search: + stop_thresh: 0.5 + drive_fwd_thresh: 0.34 + coverage_radius: 20 + segments_per_rotation: 8 + distance_between_spirals: 3 + +single_fiducial: + stop_thresh: 1.0 + fiducial_stop_threshold: 1.75 + post_avoidance_multiplier: 1.42 + post_radius: 0.7 + +waypoint: + stop_thresh: 0.5 + drive_fwd_thresh: 0.34 + no_fiducial: -1 + +recovery: + stop_thresh: 0.2 + drive_fwd_thresh: 0.34 + recovery_distance: 2.0 # distance the rover moves back in each leg of j-turn + give_up_time: 10.0 + +failure_identification: + dataframe_max_size: 200 + test_recovery_state: False + post_recovery_grace_period: 5.0 + +watchdog: + window_size: 100 #size of window we are looking at for being stuck + angular_threshold: 0.1 + linear_threshold: 0.2 diff --git a/launch/auton.launch b/launch/auton.launch deleted file mode 100644 index d642ecbf2..000000000 --- a/launch/auton.launch +++ /dev/null @@ -1,57 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/launch/new_sim.launch b/launch/new_sim.launch deleted file mode 100644 index a5b3e3eed..000000000 --- a/launch/new_sim.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/plugins/tag_detector_plugin.xml b/plugins/tag_detector_plugin.xml new file mode 100644 index 000000000..7f9242490 --- /dev/null +++ b/plugins/tag_detector_plugin.xml @@ -0,0 +1,6 @@ + + + + diff --git a/scripts/debug_course_publisher.py b/scripts/debug_course_publisher.py new file mode 100755 index 000000000..35162f32f --- /dev/null +++ b/scripts/debug_course_publisher.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python3 + +import numpy as np + +import rospy +from mrover.msg import Waypoint, WaypointType +from util.SE3 import SE3 +from util.course_publish_helpers import convert_waypoint_to_gps, publish_waypoints + +""" +The purpose of this file is for testing courses in the sim +without needing the auton GUI. You can add waypoints with +and without tags and these will get published to nav +""" + +if __name__ == "__main__": + rospy.init_node("debug_course_publisher") + + publish_waypoints([convert_waypoint_to_gps(waypoint) for waypoint in [ + ( + Waypoint(tag_id=0, type=WaypointType(val=WaypointType.NO_SEARCH)), + SE3(position=np.array([4, 4, 0])), + ), + ( + Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)), + SE3(position=np.array([-2, -2, 0])), + ), + ( + Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)), + SE3(position=np.array([11, -10, 0])), + ) + ]]) diff --git a/src/preload/format b/src/preload/format new file mode 100644 index 000000000..3176bfefe --- /dev/null +++ b/src/preload/format @@ -0,0 +1,17 @@ +#pragma once + +// macOS does not have std::format yet +// So we define it and redirect it to fmt::format + +#ifdef __APPLE__ +#include + +namespace std { + template + FMT_NODISCARD FMT_INLINE auto format(fmt::format_string fmt, T&&... args) -> std::string { + return fmt::format(fmt, std::forward(args)...); + } +} // namespace std +#else +#include_next +#endif From cc5b00d3561fb9d77951414bae36922a3247eb14 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 11 Feb 2024 19:15:53 -0500 Subject: [PATCH 194/197] Minor changes --- src/perception/object_detector/inference.cu | 22 ++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/perception/object_detector/inference.cu b/src/perception/object_detector/inference.cu index ad554ca71..5a8c5097a 100644 --- a/src/perception/object_detector/inference.cu +++ b/src/perception/object_detector/inference.cu @@ -133,7 +133,7 @@ namespace mrover { assert(mContext); //Get the binding id for the input tensor - int const inputId = getBindingInputIndex(mContext.get()); + int inputId = getBindingInputIndex(mContext.get()); //Memcpy the input tensor from the host to the gpu cudaMemcpy(mBindings[inputId], input.data, input.total() * input.elemSize(), cudaMemcpyHostToDevice); @@ -155,23 +155,23 @@ namespace mrover { // This is essentially: element count * size of each element std::size_t const size = std::reduce(extents, extents + rank, sizeof(float), std::multiplies<>()); // Create GPU memory for TensorRT to operate on - cudaMalloc(mBindings.data() + i, size); - assert(mBindings.data() + i); + if (cudaError_t result = cudaMalloc(mBindings.data() + i, size); result != cudaSuccess) + throw std::runtime_error{"Failed to allocate GPU memory: " + std::string{cudaGetErrorString(result)}}; } assert(mContext); // Create an appropriately sized output tensor - Dims const outputTensorDims = mEngine->getTensorShape(OUTPUT_BINDING_NAME); - for (int i = 0; i < outputTensorDims.nbDims; i++) { - char message[512]; - std::snprintf(message, sizeof(message), "Size %d %d", i, outputTensorDims.d[i]); - mLogger.log(ILogger::Severity::kINFO, message); + auto const [nbDims, d] = mEngine->getTensorShape(OUTPUT_BINDING_NAME); + for (int i = 0; i < nbDims; i++) { + std::array message; + std::snprintf(message.data(), message.size(), "Size %d %d", i, d[i]); + mLogger.log(ILogger::Severity::kINFO, message.data()); } // Create the mat wrapper around the output matrix for ease of use - assert(outputTensorDims.nbDims == 3); - assert(outputTensorDims.d[0] == 1); - mOutputTensor = cv::Mat::zeros(outputTensorDims.d[1], outputTensorDims.d[2], CV_32FC1); + assert(nbDims == 3); + assert(d[0] == 1); + mOutputTensor = cv::Mat::zeros(d[1], d[2], CV_32FC1); } auto Inference::getBindingInputIndex(IExecutionContext const* context) -> int { From e9996e339f6916065ffb17df17cdd66ffd182a84 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 16 Feb 2024 09:42:04 -0500 Subject: [PATCH 195/197] Fixed Frames --- src/perception/object_detector/object_detector.processing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 0bda1ef6a..9209c5fa4 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -289,7 +289,7 @@ namespace mrover { //Grab the object inside of the camera frame and push it into the map frame SE3 objectInsideCamera = SE3::fromTfTree(mTfBuffer, mMapFrameId, immediateFrameId); - SE3::pushToTfTree(mTfBroadcaster, permanentFrameId, mCameraFrameId, objectInsideCamera); + SE3::pushToTfTree(mTfBroadcaster, permanentFrameId, mMapFrameId, objectInsideCamera); } } catch (tf2::ExtrapolationException const&) { From 35f529ed706343b80cdd037131fe9c12ab06d55d Mon Sep 17 00:00:00 2001 From: John Date: Sun, 18 Feb 2024 11:46:38 -0500 Subject: [PATCH 196/197] baja.onnx --- src/perception/object_detector/object_detector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index 9daeddf27..c91000bf2 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -21,7 +21,7 @@ namespace mrover { mNh.param("obj_hitcount_max", mObjMaxHitcount, 10); //Model Params - mNh.param("model_name", mModelName, "yolov8n_mallet_bottle_better"); + mNh.param("model_name", mModelName, "baja"); //Init Model Path std::filesystem::path packagePath = ros::package::getPath("mrover"); From eb6781c6a0cd26dc198b5c31b7d7f25daf7a8a76 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Mon, 19 Feb 2024 21:51:25 -0500 Subject: [PATCH 197/197] Changes from Baja, rename class names to be lowercase, publish to correct frame, use manif --- CMakeLists.txt | 2 + .../image_capture/image_capturepy.py | 2 - .../object_detector/object_detector.cpp | 6 +- .../object_detector/object_detector.hpp | 12 +- .../object_detector.processing.cpp | 212 +++++++++--------- src/perception/object_detector/pch.hpp | 2 +- 6 files changed, 113 insertions(+), 123 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 05e7bd936..050c768c9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -178,6 +178,8 @@ if (ZED_FOUND) mrover_nodelet_defines(object_detector __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__ # Eigen includes some files it should not, ignore ) + # TODO(john): Update object detector to use new API + target_compile_options(object_detector_nodelet PRIVATE -Wno-deprecated-declarations) # Temporary mrover_nodelet_link_libraries(object_detector PRIVATE opencv_imgcodecs opencv_highgui) diff --git a/src/perception/image_capture/image_capturepy.py b/src/perception/image_capture/image_capturepy.py index f3916d1d5..98132c825 100755 --- a/src/perception/image_capture/image_capturepy.py +++ b/src/perception/image_capture/image_capturepy.py @@ -23,7 +23,6 @@ def on_press(key): if key == keyboard.Key.enter: - msg = rospy.wait_for_message("/camera/left/image", Image, timeout=5) data = np.empty(msg.height * msg.width * 4, dtype=np.uint8) for x in range(msg.height * msg.width * 4): @@ -46,7 +45,6 @@ def on_release(key): class image_capturepy: - def __init__(self): while False: continue diff --git a/src/perception/object_detector/object_detector.cpp b/src/perception/object_detector/object_detector.cpp index c91000bf2..7da75334d 100644 --- a/src/perception/object_detector/object_detector.cpp +++ b/src/perception/object_detector/object_detector.cpp @@ -11,8 +11,8 @@ namespace mrover { mNh.param("enable_loop_profiler", mEnableLoopProfiler, false); //TF Params - mNh.param("camera_frame", mCameraFrameId, "zed2i_left_camera_frame"); - mNh.param("world_frame", mMapFrameId, "map"); + mNh.param("camera_frame", mCameraFrameId, "zed_left_camera_frame"); + mNh.param("world_frame", mMapFrame, "map"); //Hit count params mNh.param("obj_increment_weight", mObjIncrementWeight, 2); @@ -21,7 +21,7 @@ namespace mrover { mNh.param("obj_hitcount_max", mObjMaxHitcount, 10); //Model Params - mNh.param("model_name", mModelName, "baja"); + mNh.param("model_name", mModelName, "yolov8n_mallet_bottle_better"); //Init Model Path std::filesystem::path packagePath = ros::package::getPath("mrover"); diff --git a/src/perception/object_detector/object_detector.hpp b/src/perception/object_detector/object_detector.hpp index 05a8acc51..b15bcce25 100644 --- a/src/perception/object_detector/object_detector.hpp +++ b/src/perception/object_detector/object_detector.hpp @@ -5,9 +5,9 @@ namespace mrover { //Data type for detection struct Detection { - int classId{0}; + int classId{}; std::string className; - float confidence{0.0}; + float confidence{}; cv::Rect box; }; @@ -20,7 +20,7 @@ namespace mrover { cv::Mat mImg; - std::vector classes{"Bottle", "Hammer"}; + std::vector classes{"bottle", "hammer"}; ros::NodeHandle mNh, mPnh; @@ -41,7 +41,7 @@ namespace mrover { tf2_ros::TransformListener mTfListener{mTfBuffer}; tf2_ros::TransformBroadcaster mTfBroadcaster; std::string mCameraFrameId; - std::string mMapFrameId; + std::string mMapFrame; std::vector mObjectHitCounts{0, 0}; @@ -53,10 +53,10 @@ namespace mrover { auto onInit() -> void override; auto getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, - size_t u, size_t v, size_t width, size_t height) -> std::optional; + size_t u, size_t v, size_t width, size_t height) -> std::optional; auto spiralSearchInImg(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, - size_t xCenter, size_t yCenter, size_t width, size_t height) -> std::optional; + size_t xCenter, size_t yCenter, size_t width, size_t height) -> std::optional; static auto convertPointCloudToRGBA(sensor_msgs::PointCloud2ConstPtr const& msg, cv::Mat& img) -> void; diff --git a/src/perception/object_detector/object_detector.processing.cpp b/src/perception/object_detector/object_detector.processing.cpp index 9209c5fa4..8e5979100 100644 --- a/src/perception/object_detector/object_detector.processing.cpp +++ b/src/perception/object_detector/object_detector.processing.cpp @@ -1,4 +1,5 @@ #include "object_detector.hpp" +#include "lie.hpp" namespace mrover { @@ -32,7 +33,7 @@ namespace mrover { cv::cvtColor(sizedImage, sizedImage, cv::COLOR_BGRA2BGR); // Create the blob from the resized image - cv::dnn::blobFromImage(sizedImage, mImageBlob, 1.0 / 255.0, imgSize, cv::Scalar(), true, false); + cv::dnn::blobFromImage(sizedImage, mImageBlob, 1.0 / 255.0, imgSize, cv::Scalar{}, true, false); if (mEnableLoopProfiler) { mLoopProfiler.measureEvent("Convert Image"); @@ -64,64 +65,56 @@ namespace mrover { output = output.reshape(1, dimensions); cv::transpose(output, output); - //Reinterpret data from the output to be in a usable form - auto data = reinterpret_cast(output.data); - //Model Information + // Model Information auto modelInputCols = static_cast(imgSize.width); auto modelInputRows = static_cast(imgSize.height); auto modelShapeWidth = static_cast(imgSize.width); auto modelShapeHeight = static_cast(imgSize.height); - //Set model thresholds - float modelScoreThreshold = 0.50; + // Set model thresholds + float modelScoreThreshold = 0.75; float modelNMSThreshold = 0.50; - //Get x and y scale factors - float x_factor = modelInputCols / modelShapeWidth; - float y_factor = modelInputRows / modelShapeHeight; + // Get x and y scale factors + float xFactor = modelInputCols / modelShapeWidth; + float yFactor = modelInputRows / modelShapeHeight; - //Init storage containers - std::vector class_ids; + // Init storage containers + std::vector classIds; std::vector confidences; std::vector boxes; - //Each of the possibilities do interpret the data - for (int i = 0; i < rows; ++i) { - //This is because the first 4 points are box[x,y,w,h] - float* classes_scores = data + 4; + // Each row of the output is a detection with a bounding box and associated class scores + for (int r = 0; r < rows; ++r) { + // Skip first four values as they are the box data + cv::Mat scores = output.row(r).colRange(4, dimensions); - //Create a mat to store all of the class scores - cv::Mat scores(1, static_cast(classes.size()), CV_32FC1, classes_scores); - cv::Point class_id; + cv::Point classId; double maxClassScore; + cv::minMaxLoc(scores, nullptr, &maxClassScore, nullptr, &classId); - //Find the max class score for the associated row - cv::minMaxLoc(scores, nullptr, &maxClassScore, nullptr, &class_id); - - //Determine if the class is an acceptable confidence level else disregard - if (maxClassScore > modelScoreThreshold) { - //Push back data points into storage containers - confidences.push_back(static_cast(maxClassScore)); - class_ids.push_back(class_id.x); - - //Get the bounding box data - float x = data[0]; - float y = data[1]; - float w = data[2]; - float h = data[3]; - - //Cast the corners into integers to be used on pixels - int left = static_cast((x - 0.5 * w) * x_factor); - int top = static_cast((y - 0.5 * h) * y_factor); - int width = static_cast(w * x_factor); - int height = static_cast(h * y_factor); - - //Push abck the box into storage - boxes.emplace_back(left, top, width, height); - } + // Determine if the class is an acceptable confidence level, otherwise disregard + if (maxClassScore <= modelScoreThreshold) continue; + + confidences.push_back(static_cast(maxClassScore)); + classIds.push_back(classId.x); + + // Get the bounding box data + cv::Mat box = output.row(r).colRange(0, 4); + auto x = box.at(0); + auto y = box.at(1); + auto w = box.at(2); + auto h = box.at(3); - data += dimensions; + // Cast the corners into integers to be used on pixels + auto left = static_cast((x - 0.5 * w) * xFactor); + auto top = static_cast((y - 0.5 * h) * yFactor); + auto width = static_cast(w * xFactor); + auto height = static_cast(h * yFactor); + + // Push abck the box into storage + boxes.emplace_back(left, top, width, height); } //Coalesce the boxes into a smaller number of distinct boxes @@ -130,17 +123,17 @@ namespace mrover { //Storage for the detection from the model std::vector detections{}; - for (int idx: nmsResult) { + for (int i: nmsResult) { //Init the detection Detection result; //Fill in the id and confidence for the class - result.classId = class_ids[idx]; - result.confidence = confidences[idx]; + result.classId = classIds[i]; + result.confidence = confidences[i]; //Fill in the class name and box information result.className = classes[result.classId]; - result.box = boxes[idx]; + result.box = boxes[i]; //Push back the detection into the for storagevector detections.push_back(result); @@ -151,32 +144,32 @@ namespace mrover { } std::vector seenObjects{false, false}; - //If there are detections locate them in 3D + // If there are detections locate them in 3D for (Detection const& detection: detections) { - //Increment Object hit counts if theyre seen + // Increment Object hit counts if theyre seen updateHitsObject(msg, detection, seenObjects); - //Decrement Object hit counts if they're not seen - for (size_t i = 0; i < seenObjects.size(); i++) { - if (!seenObjects.at(i)) { - mObjectHitCounts.at(i) = std::max(0, mObjectHitCounts.at(i) - mObjDecrementWeight); - } + // Decrement Object hit counts if they're not seen + for (std::size_t i = 0; i < seenObjects.size(); i++) { + if (seenObjects[i]) continue; + + assert(i < mObjectHitCounts.size()); + mObjectHitCounts[i] = std::max(0, mObjectHitCounts[i] - mObjDecrementWeight); } - //Draw the detected object's bounding boxes on the image for each of the objects detected - std::vector fontColors{cv::Scalar{232, 115, 5}, - cv::Scalar{0, 4, 227}}; + // Draw the detected object's bounding boxes on the image for each of the objects detected + std::array fontColors{cv::Scalar{232, 115, 5}, cv::Scalar{0, 4, 227}}; for (std::size_t i = 0; i < detections.size(); i++) { - //Font color will change for each different detection - cv::Scalar fontColor = fontColors.at(detections.at(i).classId); + // Font color will change for each different detection + cv::Scalar fontColor = fontColors.at(detections[i].classId); cv::rectangle(sizedImage, detections[i].box, fontColor, 1, cv::LINE_8, 0); - //Put the text on the image + // Put the text on the image cv::Point textPosition(80, static_cast(80 * (i + 1))); constexpr int fontSize = 1; constexpr int fontWeight = 2; - putText(sizedImage, detections[i].className, textPosition, cv::FONT_HERSHEY_COMPLEX, fontSize, fontColor, fontWeight); //Putting the text in the matrix// + putText(sizedImage, detections[i].className, textPosition, cv::FONT_HERSHEY_COMPLEX, fontSize, fontColor, fontWeight); // Putting the text in the matrix } } @@ -184,18 +177,18 @@ namespace mrover { mLoopProfiler.measureEvent("Push to TF"); } - //We only want to publish the debug image if there is something lsitening, to reduce the operations going on - if (mDebugImgPub.getNumSubscribers() > 0 || true) { - //Publishes the image to the debug publisher + // We only want to publish the debug image if there is something lsitening, to reduce the operations going on + if (mDebugImgPub.getNumSubscribers()) { + // Publishes the image to the debug publisher publishImg(sizedImage); } if (mEnableLoopProfiler) { mLoopProfiler.measureEvent("Publish Debug Img"); } - } // namespace mrover + } - auto ObjectDetectorNodelet::getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v, size_t width, size_t height) -> std::optional { + auto ObjectDetectorNodelet::getObjectInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v, size_t width, size_t height) -> std::optional { assert(cloudPtr); if (u >= cloudPtr->width || v >= cloudPtr->height) { NODELET_WARN("Tag center out of bounds: [%zu %zu]", u, v); @@ -206,17 +199,17 @@ namespace mrover { return spiralSearchInImg(cloudPtr, u, v, width, height); } - auto ObjectDetectorNodelet::spiralSearchInImg(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t xCenter, size_t yCenter, size_t width, size_t height) -> std::optional { - size_t currX = xCenter; - size_t currY = yCenter; - size_t radius = 0; + auto ObjectDetectorNodelet::spiralSearchInImg(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t xCenter, size_t yCenter, size_t width, size_t height) -> std::optional { + std::size_t currX = xCenter; + std::size_t currY = yCenter; + std::size_t radius = 0; int t = 0; constexpr int numPts = 16; bool isPointInvalid = true; Point point{}; // Find the smaller of the two box dimensions so we know the max spiral radius - size_t smallDim = std::min(width / 2, height / 2); + std::size_t smallDim = std::min(width / 2, height / 2); while (isPointInvalid) { // This is the parametric equation to spiral around the center pnt @@ -243,7 +236,7 @@ namespace mrover { } } - return std::make_optional(R3{point.x, point.y, point.z}, SO3{}); + return std::make_optional(R3{point.x, point.y, point.z}, SO3d::Identity()); } auto ObjectDetectorNodelet::convertPointCloudToRGBA(sensor_msgs::PointCloud2ConstPtr const& msg, cv::Mat& img) -> void { @@ -259,48 +252,45 @@ namespace mrover { } auto ObjectDetectorNodelet::updateHitsObject(sensor_msgs::PointCloud2ConstPtr const& msg, Detection const& detection, std::vector& seenObjects, cv::Size const& imgSize) -> void { - - cv::Rect box = detection.box; + cv::Rect const& box = detection.box; auto center = std::pair(box.x + box.width / 2, box.y + box.height / 2); // Resize from {640, 640} image space to {720,1280} image space - auto centerWidth = static_cast(center.first * static_cast(msg->width) / imgSize.width); - auto centerHeight = static_cast(center.second * static_cast(msg->height) / imgSize.height); - - if (!seenObjects.at(detection.classId)) { - seenObjects.at(detection.classId) = true; - - //Get the object's position in 3D from the point cloud and run this statement if the optional has a value - if (std::optional objectLocation = getObjectInCamFromPixel(msg, centerWidth, centerHeight, box.width, box.height); objectLocation) { - try { - std::string immediateFrameId = "immediateDetectedObject" + classes.at(detection.classId); - - //Push the immediate detections to the zed frame - SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, objectLocation.value()); - - - //Since the object is seen we need to increment the hit counter - mObjectHitCounts.at(detection.classId) = std::min(mObjMaxHitcount, mObjectHitCounts.at(detection.classId) + mObjIncrementWeight); - - //Only publish to permament if we are confident in the object - if (mObjectHitCounts.at(detection.classId) > mObjHitThreshold) { - - std::string permanentFrameId = "detectedObject" + classes.at(detection.classId); - - - //Grab the object inside of the camera frame and push it into the map frame - SE3 objectInsideCamera = SE3::fromTfTree(mTfBuffer, mMapFrameId, immediateFrameId); - SE3::pushToTfTree(mTfBroadcaster, permanentFrameId, mMapFrameId, objectInsideCamera); - } - - } catch (tf2::ExtrapolationException const&) { - NODELET_WARN("Old data for immediate tag"); - } catch (tf2::LookupException const&) { - NODELET_WARN("Expected transform for immediate tag"); - } catch (tf::ConnectivityException const&) { - NODELET_WARN("Expected connection to odom frame. Is visual odometry running?"); - } catch (tf::LookupException const&) { - NODELET_WARN("LOOK UP NOT FOUND"); + auto centerWidth = static_cast(center.first * static_cast(msg->width) / imgSize.width); + auto centerHeight = static_cast(center.second * static_cast(msg->height) / imgSize.height); + + assert(static_cast(detection.classId) < seenObjects.size()); + assert(static_cast(detection.classId) < classes.size()); + assert(static_cast(detection.classId) < mObjectHitCounts.size()); + + if (seenObjects[detection.classId]) return; + + seenObjects[detection.classId] = true; + + // Get the object's position in 3D from the point cloud and run this statement if the optional has a value + if (std::optional objectInCamera = getObjectInCamFromPixel(msg, centerWidth, centerHeight, box.width, box.height)) { + try { + std::string objectImmediateFrame = std::format("immediate{}", classes[detection.classId]); + // Push the immediate detections to the camera frame + SE3Conversions::pushToTfTree(mTfBroadcaster, objectImmediateFrame, mCameraFrameId, objectInCamera.value()); + // Since the object is seen we need to increment the hit counter + mObjectHitCounts[detection.classId] = std::min(mObjMaxHitcount, mObjectHitCounts[detection.classId] + mObjIncrementWeight); + + // Only publish to permament if we are confident in the object + if (mObjectHitCounts[detection.classId] > mObjHitThreshold) { + std::string objectPermanentFrame = classes[detection.classId]; + // Grab the object inside of the camera frame and push it into the map frame + SE3d objectInMap = SE3Conversions::fromTfTree(mTfBuffer, mMapFrame, objectImmediateFrame); + SE3Conversions::pushToTfTree(mTfBroadcaster, objectPermanentFrame, mMapFrame, objectInMap); } + + } catch (tf2::ExtrapolationException const&) { + NODELET_WARN("Old data for immediate tag"); + } catch (tf2::LookupException const&) { + NODELET_WARN("Expected transform for immediate tag"); + } catch (tf::ConnectivityException const&) { + NODELET_WARN("Expected connection to odom frame. Is visual odometry running?"); + } catch (tf::LookupException const&) { + NODELET_WARN("LOOK UP NOT FOUND"); } } } diff --git a/src/perception/object_detector/pch.hpp b/src/perception/object_detector/pch.hpp index 18c32f3a5..6f7e6116b 100644 --- a/src/perception/object_detector/pch.hpp +++ b/src/perception/object_detector/pch.hpp @@ -40,8 +40,8 @@ #include +#include #include -#include #include #include