From 1a274edf44c169d9a9effdc745743d5e84bd9957 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Tue, 26 Sep 2023 17:44:48 -0400 Subject: [PATCH 001/145] 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/145] 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/145] 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/145] 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/145] 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/145] 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/145] 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/145] 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/145] 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/145] 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/145] 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/145] 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 7629bcee86892a2c2aa6841f5b0be1738a948431 Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Tue, 3 Oct 2023 16:00:09 -0400 Subject: [PATCH 013/145] 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 014/145] 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 015/145] 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 e70dc0c7832c77547a4f389ff0ac11edc7a8ec09 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 10 Oct 2023 20:11:11 -0400 Subject: [PATCH 016/145] 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 017/145] 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 b74bf908c19ee56677afc6fd82a136df8c725af8 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Sun, 22 Oct 2023 02:11:47 -0400 Subject: [PATCH 018/145] 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 019/145] 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 488d9f70cde2ab69ee8c407a682e3ba23db6c6d2 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Wed, 25 Oct 2023 15:31:47 -0400 Subject: [PATCH 020/145] 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 021/145] 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 3d5e10be228964ddd9b45a8862d2be7c6340d8be Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Mon, 30 Oct 2023 12:07:55 -0400 Subject: [PATCH 022/145] 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 023/145] 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 ac14193813cad0bd71b07617d71b64524347c6b2 Mon Sep 17 00:00:00 2001 From: lorraine Date: Thu, 2 Nov 2023 19:55:54 -0400 Subject: [PATCH 024/145] 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 28fc946260a7349417c07992438bf57f6164bdb8 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Mon, 6 Nov 2023 11:13:25 -0500 Subject: [PATCH 025/145] 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 026/145] 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 cccf7b0249d7d36387772da3410d144185eddb7b Mon Sep 17 00:00:00 2001 From: lorraine Date: Sun, 12 Nov 2023 13:59:15 -0500 Subject: [PATCH 027/145] 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 028/145] 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 f996b2d1ba959187005edfb6af99d330df3950fb Mon Sep 17 00:00:00 2001 From: Emerson-Ramey Date: Sun, 19 Nov 2023 06:48:34 +0000 Subject: [PATCH 029/145] 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 e7f7e9e268992529149da7cd1ef5e211d3cdbdd3 Mon Sep 17 00:00:00 2001 From: emmalinhart Date: Thu, 30 Nov 2023 20:03:18 -0500 Subject: [PATCH 030/145] 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 031/145] 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 52d57636b729c9a0202aa14439b73cbcee65a8ca Mon Sep 17 00:00:00 2001 From: Emerson-Ramey Date: Tue, 9 Jan 2024 06:34:42 +0000 Subject: [PATCH 032/145] 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 fa6a6f0f2e0112cf1eedb9e09fbe9fc40466c1aa Mon Sep 17 00:00:00 2001 From: Marvin Jirapongsuwan Date: Thu, 18 Jan 2024 15:00:16 -0500 Subject: [PATCH 033/145] 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 704e3145ee7fde6c26f18d5b44d81ab48df9234e Mon Sep 17 00:00:00 2001 From: Emerson-Ramey Date: Sun, 21 Jan 2024 01:18:49 +0000 Subject: [PATCH 034/145] update todos --- msg/WaypointType.msg | 2 + src/navigation/approach_object.py | 2 - src/navigation/approach_post.py | 84 +++++++++++++++---------------- src/navigation/context.py | 6 ++- src/navigation/waypoint.py | 2 +- 5 files changed, 48 insertions(+), 48 deletions(-) diff --git a/msg/WaypointType.msg b/msg/WaypointType.msg index df2088fbd..a7995aec5 100644 --- a/msg/WaypointType.msg +++ b/msg/WaypointType.msg @@ -1,3 +1,5 @@ int32 NO_SEARCH = 0 int32 POST = 1 +int32 MALLET = 2 +int32 WATER_BOTTLE = 3 int32 val \ No newline at end of file diff --git a/src/navigation/approach_object.py b/src/navigation/approach_object.py index 400d627bc..c4e2c089e 100644 --- a/src/navigation/approach_object.py +++ b/src/navigation/approach_object.py @@ -15,8 +15,6 @@ class ApproachObjectState(ApproachTargetBaseState): 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): diff --git a/src/navigation/approach_post.py b/src/navigation/approach_post.py index e4aa1ec6b..5eaae0bc8 100644 --- a/src/navigation/approach_post.py +++ b/src/navigation/approach_post.py @@ -8,14 +8,12 @@ from navigation.approach_target_base import ApproachTargetBaseState -class NewApproachPostState(ApproachTargetBaseState): +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 - -Arrived at the waypoint where the fiducial should be but have not seen it yet: SearchState - -Stuck? """ def on_enter(self, context): @@ -36,48 +34,48 @@ def determine_next(self, context, finished: bool) -> State: 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 +# 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_enter(self, context) -> None: +# pass - def on_exit(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() +# 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 - pass +# 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 +# return self diff --git a/src/navigation/context.py b/src/navigation/context.py index abcab5884..723cd5842 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -62,8 +62,8 @@ class Environment: ctx: Context NO_FIDUCIAL: ClassVar[int] = -1 - arrived_at_post: bool = False - last_post_location: Optional[np.ndarray] = None + arrived_at_post: bool = False # TODO change to arrived_at_target + last_post_location: Optional[np.ndarray] = None # TODO change to last_target_location # 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]: @@ -156,6 +156,8 @@ def look_for_post(self) -> bool: return waypoint.type.val == mrover.msg.WaypointType.POST else: return False + + # TODO add look_for_object() def is_complete(self) -> bool: return self.waypoint_index == len(self.course_data.waypoints) diff --git a/src/navigation/waypoint.py b/src/navigation/waypoint.py index 8aa3398c2..d07780563 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -61,7 +61,7 @@ def on_loop(self, context) -> State: # 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() + # return water_bottle_search.WaterBottleSearchState() if context.rover.stuck: context.rover.previous_state = self From ca1b3a6164775019f799ff705447b185e008780b Mon Sep 17 00:00:00 2001 From: Ankith Udupa Date: Sun, 21 Jan 2024 13:28:09 -0500 Subject: [PATCH 035/145] updated jetson yaml --- ansible/roles/jetson_networks/tasks/main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ansible/roles/jetson_networks/tasks/main.yml b/ansible/roles/jetson_networks/tasks/main.yml index 9f1f2c588..bef0a6aee 100644 --- a/ansible/roles/jetson_networks/tasks/main.yml +++ b/ansible/roles/jetson_networks/tasks/main.yml @@ -6,7 +6,7 @@ type: ethernet ifname: eth0 autoconnect: yes - ip4: 10.0.0.10/24 + ip4: 10.0.0.10/8 gw4: 10.0.0.2 dns4: [10.0.0.2] method6: disabled From af28e7daf8b80b5e38eb1375f6627247ca45c63f Mon Sep 17 00:00:00 2001 From: dllliu Date: Sun, 21 Jan 2024 14:11:11 -0500 Subject: [PATCH 036/145] gps testing config --- config/esw.yaml | 3 +-- launch/rtk_basestation.launch | 19 +++++++++++++++++++ 2 files changed, 20 insertions(+), 2 deletions(-) create mode 100644 launch/rtk_basestation.launch diff --git a/config/esw.yaml b/config/esw.yaml index d18dc4954..8aca0663d 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -14,9 +14,8 @@ left_gps_driver: baud: 38400 frame_id: "base_link" - basestation_gps_driver: - port: "/dev/ttyACM1" + port: "/dev/ttyACM0" baud: 38400 frame_id: "base_link" diff --git a/launch/rtk_basestation.launch b/launch/rtk_basestation.launch new file mode 100644 index 000000000..a8df3946d --- /dev/null +++ b/launch/rtk_basestation.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + \ No newline at end of file From 6bfcae6a024f9d4328b59732bd4138a90ca9be76 Mon Sep 17 00:00:00 2001 From: Stephen Barstys Date: Sun, 21 Jan 2024 14:21:00 -0500 Subject: [PATCH 037/145] progress on TODOs --- src/navigation/approach_target_base.py | 4 +- src/navigation/context.py | 41 +++++++++++++----- src/navigation/long_range_state.py | 60 ++++++++++++++++++++++++++ src/navigation/search.py | 38 ++++++++++------ 4 files changed, 117 insertions(+), 26 deletions(-) create mode 100644 src/navigation/long_range_state.py diff --git a/src/navigation/approach_target_base.py b/src/navigation/approach_target_base.py index ab28f4b76..f0dbb4650 100644 --- a/src/navigation/approach_target_base.py +++ b/src/navigation/approach_target_base.py @@ -48,8 +48,8 @@ def on_loop(self, context) -> State: ) 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.env.arrived_at_target = True + context.env.last_target_location = self.get_target_pos() context.course.increment_waypoint() else: context.rover.send_drive_command(cmd_vel) diff --git a/src/navigation/context.py b/src/navigation/context.py index 28250a3bb..8c7c75597 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -62,10 +62,11 @@ class Environment: ctx: Context NO_FIDUCIAL: ClassVar[int] = -1 - arrived_at_post: bool = False # TODO change to arrived_at_target - last_post_location: Optional[np.ndarray] = None # TODO change to last_target_location + arrived_at_target: bool = None + last_target_location: Optional[np.ndarray] = None # TODO add dictionary for long range tag id : (time received, our hit counter, bearing) - + tag_data_dict = {} + 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 @@ -74,9 +75,7 @@ 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}" - ) + 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"EXPIRED {id}!") @@ -88,7 +87,7 @@ def get_target_pos(self, id: str, in_odom_frame: bool = True) -> Optional[np.nda ) as e: return None return target_pose.position - + def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray]: """ Retrieves the position of the current fiducial or object (and we are looking for it) @@ -156,8 +155,20 @@ def look_for_post(self) -> bool: return waypoint.type.val == mrover.msg.WaypointType.POST else: return False - - # TODO add look_for_object() + + 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 == mrover.msg.WaypointType.MALLET + or waypoint.type.val == mrover.msg.WaypointType.WATER_BOTTLE + ) + else: + return False def is_complete(self) -> bool: return self.waypoint_index == len(self.course_data.waypoints) @@ -213,7 +224,7 @@ class Context: vis_publisher: rospy.Publisher course_listener: rospy.Subscriber stuck_listener: rospy.Subscriber - # TODO create listener for long range cam + long_range_cam_listener: rospy.Subscriber # Use these as the primary interfaces in states course: Optional[Course] @@ -243,6 +254,8 @@ def __init__(self): 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("tag_data", list, self.tag_data_callback) + self.long_range_cam_listener = rospy.Subscriber("long_range_cam", Bool, self.long_range_cam_listener) def recv_enable_auton(self, req: mrover.srv.PublishEnableAutonRequest) -> mrover.srv.PublishEnableAutonResponse: enable_msg = req.enableMsg @@ -255,4 +268,10 @@ 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 + def tag_data_callback(self, tags: list) -> None: + for tag in tags: + tag_id = tag["id"] + hit_count = tag["hitCount"] + bearing = tag["bearing"] + time_received = rospy.Time() + self.env.tag_data_dict[tag_id] = (time_received, hit_count, bearing) diff --git a/src/navigation/long_range_state.py b/src/navigation/long_range_state.py new file mode 100644 index 000000000..13df495af --- /dev/null +++ b/src/navigation/long_range_state.py @@ -0,0 +1,60 @@ +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 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 state import ApproachTargetBaseState +from state import approach_post +import numpy as np + +TAG_SEARCH_EXPIRATION = 3 + + +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.current_waypoint()["tagid"] + bearing = context.env.tag_data_dict[tag_id][2] + rover_position = context.rover.get_pose().position + target_pos_x = 90 - bearing + target_pos_y = bearing + target_pos_z = rover_position[2] + target_coordinates = np.ndarray + context.tag_data_callback() + return target_pos + + def determine_next(self, context, finished: bool) -> State: + fid_pos = context.env.current_fid_pos() # Need to pass in the fid_id + tag_time = context.env.tag_data_dict[context.current_tag_id][0] + if fid_pos is None: + if rospy.Time() - tag_time > TAG_SEARCH_EXPIRATION: + return state.SearchState() + else: + return state.LongRangeState() + else: + return approach_post.ApproachPostState() diff --git a/src/navigation/search.py b/src/navigation/search.py index 76aec55e4..5e31537aa 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -5,6 +5,7 @@ import numpy as np from mrover.msg import GPSPointList +import mrover.msg from util.ros_utils import get_rosparam from util.state_lib.state import State @@ -93,15 +94,26 @@ 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, - self.DISTANCE_BETWEEN_SPIRALS, - self.SEGMENTS_PER_ROTATION, - search_center.tag_id, - ) + if search_center.type == mrover.msg.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, + ) + + if mrover.msg.WaypointType.RUBBER_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.prev_target = None def on_exit(self, context) -> None: @@ -137,13 +149,13 @@ def on_loop(self, context) -> State: # 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 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 + # 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 From 45d85f301f6d777004f5a626477a233529c0f65e Mon Sep 17 00:00:00 2001 From: Ankith Udupa Date: Sun, 21 Jan 2024 15:46:55 -0500 Subject: [PATCH 038/145] changes made during rtk bearing testing --- ansible/jetson.yml | 6 +++--- ansible/roles/jetson_networks/tasks/main.yml | 2 +- config/esw.yaml | 4 ++-- launch/rtk.launch | 12 +++++++++--- 4 files changed, 15 insertions(+), 9 deletions(-) diff --git a/ansible/jetson.yml b/ansible/jetson.yml index 22391b58d..01bc688a6 100644 --- a/ansible/jetson.yml +++ b/ansible/jetson.yml @@ -4,7 +4,7 @@ ros_distro: noetic ubuntu_release: focal roles: - - jetson_build - - dev + # - jetson_build + # - dev - jetson_networks - - jetson_services + # - jetson_services diff --git a/ansible/roles/jetson_networks/tasks/main.yml b/ansible/roles/jetson_networks/tasks/main.yml index bef0a6aee..cd107525a 100644 --- a/ansible/roles/jetson_networks/tasks/main.yml +++ b/ansible/roles/jetson_networks/tasks/main.yml @@ -4,7 +4,7 @@ conn_name: MRover state: present type: ethernet - ifname: eth0 + ifname: enp1s0 autoconnect: yes ip4: 10.0.0.10/8 gw4: 10.0.0.2 diff --git a/config/esw.yaml b/config/esw.yaml index 8aca0663d..4b0ed3352 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -5,7 +5,7 @@ gps_driver: frame_id: "base_link" right_gps_driver: - port: "/dev/ttyACM0" + port: "/dev/ttyACM1" baud: 38400 frame_id: "base_link" @@ -15,7 +15,7 @@ left_gps_driver: frame_id: "base_link" basestation_gps_driver: - port: "/dev/ttyACM0" + port: "/dev/ttyUSB1" baud: 38400 frame_id: "base_link" diff --git a/launch/rtk.launch b/launch/rtk.launch index e7a8a841a..dfab8fc1e 100644 --- a/launch/rtk.launch +++ b/launch/rtk.launch @@ -3,16 +3,22 @@ - + + + + - + + + + From f51c02d567a24e47dd605acadbb39600e8c5867e Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Mon, 22 Jan 2024 18:30:18 -0500 Subject: [PATCH 039/145] add udev rule for usb cam --- src/perception/long_range_cam/long_range_cam.cpp | 11 +++-------- .../long_range_cam/long_range_tag_detector.cpp | 2 +- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/src/perception/long_range_cam/long_range_cam.cpp b/src/perception/long_range_cam/long_range_cam.cpp index 8b7fde3eb..16388d0e3 100644 --- a/src/perception/long_range_cam/long_range_cam.cpp +++ b/src/perception/long_range_cam/long_range_cam.cpp @@ -10,12 +10,9 @@ 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 { mGrabThread = std::jthread(&LongRangeCamNodelet::grabUpdate, this); @@ -26,8 +23,6 @@ namespace mrover { } void fillImageMessage(cv::Mat& bgra, sensor_msgs::ImagePtr const& msg) { - // TODO: Uncomment this - // assert(img.channels() == 4); assert(msg); msg->height = bgra.rows; @@ -52,7 +47,7 @@ namespace mrover { 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}; + 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"); } 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 777b3ba92..29688efaa 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 @@ -22,7 +22,7 @@ namespace mrover { // topic: long_range_cam/image //todo - mImgSub = mNh.subscribe(, 1, &LongRangeTagDetectorNodelet::imageCallback, this); + 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); From d6abfe9acdff7a51620c6015e57f34fcb7629d19 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Mon, 22 Jan 2024 21:08:51 -0500 Subject: [PATCH 040/145] Update name --- src/navigation/context.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/navigation/context.py b/src/navigation/context.py index c0d72c76d..ee22f54cb 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -101,7 +101,7 @@ def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray] 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) + return self.get_target_pos("hammer", in_odom) elif current_waypoint.type == WaypointType.WATER_BOTTLE: return self.get_target_pos("water_bottle", in_odom) else: From c35561f71a5fc00b1b71fb61d41e3c0890890dc5 Mon Sep 17 00:00:00 2001 From: Emerson-Ramey Date: Tue, 23 Jan 2024 19:16:24 -0500 Subject: [PATCH 041/145] change to MALLET --- src/navigation/approach_object.py | 2 +- src/navigation/context.py | 25 ++++++++++++------------- src/navigation/post_backup.py | 4 ++-- src/navigation/search.py | 12 ++++++------ src/navigation/waypoint.py | 6 +++--- 5 files changed, 24 insertions(+), 25 deletions(-) diff --git a/src/navigation/approach_object.py b/src/navigation/approach_object.py index c4e2c089e..55faf80ed 100644 --- a/src/navigation/approach_object.py +++ b/src/navigation/approach_object.py @@ -10,7 +10,7 @@ class ApproachObjectState(ApproachTargetBaseState): """ - State for when we see an object (rubber mallet or water bottle). + State for when we see an object (mallet or water bottle). We are only using the ZED camera. Transitions: -If arrived at target: DoneState diff --git a/src/navigation/context.py b/src/navigation/context.py index 0bb07e2ef..4c6b17312 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -63,7 +63,7 @@ class Environment: arrived_at_target: bool = None last_target_location: Optional[np.ndarray] = None # TODO add dictionary for long range tag id : (time received, our hit counter, bearing) - tag_data_dict = {} + # tag_data_dict = {} def get_target_pos(self, id: str, in_odom_frame: bool = True) -> Optional[np.ndarray]: """ @@ -162,8 +162,8 @@ def look_for_object(self) -> bool: waypoint = self.current_waypoint() if waypoint is not None: return ( - waypoint.type.val == mrover.msg.WaypointType.MALLET - or waypoint.type.val == mrover.msg.WaypointType.WATER_BOTTLE + waypoint.type.val == WaypointType.MALLET + or waypoint.type.val == WaypointType.WATER_BOTTLE ) else: return False @@ -222,7 +222,7 @@ class Context: vis_publisher: rospy.Publisher course_listener: rospy.Subscriber stuck_listener: rospy.Subscriber - long_range_cam_listener: rospy.Subscriber + # tag_data_listener: rospy.Subscriber # Use these as the primary interfaces in states course: Optional[Course] @@ -252,8 +252,7 @@ def __init__(self): 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("tag_data", list, self.tag_data_callback) - self.long_range_cam_listener = rospy.Subscriber("long_range_cam", Bool, self.long_range_cam_listener) + # self.tag_data_listener = rospy.Subscriber("tag_data", list, self.tag_data_callback) def recv_enable_auton(self, req: EnableAutonRequest) -> EnableAutonResponse: if req.enable: @@ -265,10 +264,10 @@ 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: list) -> None: - for tag in tags: - tag_id = tag["id"] - hit_count = tag["hitCount"] - bearing = tag["bearing"] - time_received = rospy.Time() - self.env.tag_data_dict[tag_id] = (time_received, hit_count, bearing) + # def tag_data_callback(self, tags: list) -> None: + # for tag in tags: + # tag_id = tag["id"] + # hit_count = tag["hitCount"] + # bearing = tag["bearing"] + # time_received = rospy.Time() + # self.env.tag_data_dict[tag_id] = (time_received, hit_count, bearing) diff --git a/src/navigation/post_backup.py b/src/navigation/post_backup.py index b7c51246f..d205cd5c9 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_post_location is None: + if context.env.last_target_location is None: self.traj = None else: self.traj = AvoidPostTrajectory.avoid_post_trajectory( context.rover.get_pose(), - context.env.last_post_location, + context.env.last_target_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 5e31537aa..f0afce082 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -4,8 +4,8 @@ from typing import Optional import numpy as np -from mrover.msg import GPSPointList -import mrover.msg +from mrover.msg import GPSPointList, WaypointType + from util.ros_utils import get_rosparam from util.state_lib.state import State @@ -96,8 +96,8 @@ 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) - if search_center.type == mrover.msg.WaypointType.POST: + # TODO give different parameters to spiral_traj based on if search_center.type is POST or MALLET (water bottle has diff search state) + if search_center.type == WaypointType.POST: self.traj = SearchTrajectory.spiral_traj( context.rover.get_pose().position[0:2], self.SPIRAL_COVERAGE_RADIUS, @@ -106,7 +106,7 @@ def on_enter(self, context) -> None: search_center.tag_id, ) - if mrover.msg.WaypointType.RUBBER_MALLET: + if WaypointType.MALLET: self.traj = SearchTrajectory.spiral_traj( context.rover.get_pose().position[0:2], self.SPIRAL_COVERAGE_RADIUS / 2, @@ -155,7 +155,7 @@ def on_loop(self, context) -> State: # if tag id has hit count > 3: # return long_range.LongRangeState() - # elif current waypoint type is RUBBER MALLET + # elif current waypoint type is 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 d07780563..122d3b6fe 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -30,8 +30,8 @@ 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_post: - context.env.arrived_at_post = False + if context.env.arrived_at_target: + context.env.arrived_at_target = False return post_backup.PostBackupState() if context.course.look_for_post(): @@ -57,7 +57,7 @@ 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: # TODO: elif looking for post or rubber mallet + else: # TODO: elif looking for post or 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: From 8ee12e7a1865968d761803b9445f0900504c9c89 Mon Sep 17 00:00:00 2001 From: Emerson-Ramey Date: Tue, 23 Jan 2024 19:54:33 -0500 Subject: [PATCH 042/145] edit search and waypoint --- src/navigation/search.py | 24 +++++++++++------------- src/navigation/waypoint.py | 17 ++++++++--------- 2 files changed, 19 insertions(+), 22 deletions(-) diff --git a/src/navigation/search.py b/src/navigation/search.py index f0afce082..95ec83ba1 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -10,7 +10,7 @@ from util.ros_utils import get_rosparam from util.state_lib.state import State -from navigation import approach_post, recovery, waypoint +from navigation import approach_post, approach_object, recovery, waypoint from navigation.context import convert_cartesian_to_gps from navigation.trajectory import Trajectory @@ -96,7 +96,6 @@ 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 MALLET (water bottle has diff search state) if search_center.type == WaypointType.POST: self.traj = SearchTrajectory.spiral_traj( context.rover.get_pose().position[0:2], @@ -147,15 +146,14 @@ def on_loop(self, context) -> State: ) context.rover.send_drive_command(cmd_vel) - # 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 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 + current_waypoint = context.course.current_waypoint() + if current_waypoint.type == "POST": + 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 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 == "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 122d3b6fe..de930f02e 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 +from navigation import search, recovery, approach_post, post_backup, state, approach_object class WaypointState(State): @@ -36,13 +36,12 @@ def on_loop(self, context) -> State: if context.course.look_for_post(): if context.env.current_target_pos() is not None: - return approach_post.ApproachPostState() - # TODO: - # elif tag id has hit count > 3: + 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() + 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: @@ -57,8 +56,8 @@ 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: # TODO: elif looking for post or mallet - # We finished a waypoint associated with a tag id, but we have not seen it yet. + 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. return search.SearchState() # TODO elif looking for water bottle: # return water_bottle_search.WaterBottleSearchState() From 9df940a9f8ba2c454959a5e4617eb0198b4d65f5 Mon Sep 17 00:00:00 2001 From: Rahul Date: Thu, 25 Jan 2024 18:19:29 -0500 Subject: [PATCH 043/145] added plotting file --- src/localization/gps_plotting.py | 47 ++++++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 src/localization/gps_plotting.py diff --git a/src/localization/gps_plotting.py b/src/localization/gps_plotting.py new file mode 100644 index 000000000..6b9da1471 --- /dev/null +++ b/src/localization/gps_plotting.py @@ -0,0 +1,47 @@ +import matplotlib.pyplot as plt +from pymap3d.enu import geodetic2enu +import pandas as pd +import numpy as np + +ref_lat = 42.293195 +ref_lon = -83.7096706 +ref_alt = 234.1 +linearized_pose = pd.read_csv("/home/rahul/catkin_ws/src/mrover/rtk_linearized_pose.csv") +#right_gps = pd.read_csv("/Users/nehakankanala/PycharmProjects/plot_rtk_data/rtk_right_gps.csv") + +lin_pose_x = linearized_pose["field.pose.pose.orientation.x"].to_numpy() +lin_pose_y = linearized_pose["field.pose.pose.orientation.y"].to_numpy() +lin_pose_z = linearized_pose["field.pose.pose.orientation.z"].to_numpy() +lin_pose_w = linearized_pose["field.pose.pose.orientation.w"].to_numpy() + +time = linearized_pose["%time"].to_numpy() + + +rec_yaw = [] +rec_time = [] + +for x, y, z, w, time in zip(lin_pose_x,lin_pose_y,lin_pose_z, lin_pose_w, time): + siny_cosp = 2 * (w * z + x * y) + cosy_cosp = 1 - 2 * (y * y + z * z) + yaw = np.arctan2(siny_cosp, cosy_cosp) + + rec_yaw.append(yaw) + rec_time.append(time) + print(yaw, time) + + +# rec_time = rec_time[20:] +# rec_yaw = rec_yaw[20:] + +# for i in range(len(rec_time)): +# rec_time[i] = rec_time[i] - rec_time[0] + + +plt.figure(figsize=(10, 10)) +plt.plot(rec_time, rec_yaw, color='red', label='right') +plt.xlabel('time (s)') +plt.ylabel('yaw (rad)') +plt.title('yaw vs time') +plt.legend() +plt.savefig("rtk_plot.png") +plt.show() From 95cfc4cd529978580f267fc2cf85edb2f66e9b95 Mon Sep 17 00:00:00 2001 From: Rahul Date: Thu, 25 Jan 2024 18:34:14 -0500 Subject: [PATCH 044/145] normalize time --- src/localization/gps_plotting.py | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/localization/gps_plotting.py b/src/localization/gps_plotting.py index 6b9da1471..81f291e84 100644 --- a/src/localization/gps_plotting.py +++ b/src/localization/gps_plotting.py @@ -27,21 +27,17 @@ rec_yaw.append(yaw) rec_time.append(time) - print(yaw, time) +for i in range(1, len(rec_time)): + rec_time[i] = rec_time[i] - rec_time[0] -# rec_time = rec_time[20:] -# rec_yaw = rec_yaw[20:] - -# for i in range(len(rec_time)): -# rec_time[i] = rec_time[i] - rec_time[0] - +rec_time[0] = 0 plt.figure(figsize=(10, 10)) plt.plot(rec_time, rec_yaw, color='red', label='right') plt.xlabel('time (s)') plt.ylabel('yaw (rad)') -plt.title('yaw vs time') +plt.title('yaw vs time rtk') plt.legend() plt.savefig("rtk_plot.png") plt.show() From b97bc64ada490ec04f24ed46be16815e4594f0d1 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Thu, 25 Jan 2024 19:08:17 -0500 Subject: [PATCH 045/145] approach object (mallet) working --- scripts/debug_course_publisher.py | 33 +++++++++++++++----------- src/navigation/approach_target_base.py | 4 ++-- src/navigation/context.py | 11 ++++----- src/navigation/navigation.py | 19 ++++++++++++--- src/navigation/search.py | 6 ++--- src/navigation/waypoint.py | 8 +++---- 6 files changed, 48 insertions(+), 33 deletions(-) diff --git a/scripts/debug_course_publisher.py b/scripts/debug_course_publisher.py index 35162f32f..b329e8662 100755 --- a/scripts/debug_course_publisher.py +++ b/scripts/debug_course_publisher.py @@ -16,17 +16,22 @@ 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])), - ) - ]]) + publish_waypoints( + [ + 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([-2, -2, 0])), + # ), + # ( + # Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)), + # SE3(position=np.array([11, -10, 0])), + # ) + ] + ] + ) diff --git a/src/navigation/approach_target_base.py b/src/navigation/approach_target_base.py index f0dbb4650..135acc1f9 100644 --- a/src/navigation/approach_target_base.py +++ b/src/navigation/approach_target_base.py @@ -46,10 +46,10 @@ def on_loop(self, context) -> State: self.DRIVE_FWD_THRESH, in_odom=context.use_odom, ) - next_state = self.determine_next(arrived) + 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.env.last_target_location = self.get_target_pos(context) context.course.increment_waypoint() else: context.rover.send_drive_command(cmd_vel) diff --git a/src/navigation/context.py b/src/navigation/context.py index 4c6b17312..d9f9b8ea6 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -98,14 +98,14 @@ def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray] print("CURRENT WAYPOINT IS NONE") return None - if current_waypoint.type == WaypointType.POST: + if current_waypoint.type.val == WaypointType.POST: return self.get_target_pos(f"fiducial{current_waypoint.tag_id}", in_odom) - elif current_waypoint.type == WaypointType.MALLET: + elif current_waypoint.type.val == WaypointType.MALLET: return self.get_target_pos("hammer", 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") + # print("CURRENT WAYPOINT IS NOT A POST OR OBJECT") return None @@ -161,10 +161,7 @@ def look_for_object(self) -> bool: """ waypoint = self.current_waypoint() if waypoint is not None: - return ( - waypoint.type.val == WaypointType.MALLET - or waypoint.type.val == WaypointType.WATER_BOTTLE - ) + return waypoint.type.val == WaypointType.MALLET or waypoint.type.val == WaypointType.WATER_BOTTLE else: return False diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py index 82eeee331..300d87c08 100755 --- a/src/navigation/navigation.py +++ b/src/navigation/navigation.py @@ -14,6 +14,7 @@ from navigation.search import SearchState from navigation.state import DoneState, OffState, off_check from navigation.waypoint import WaypointState +from navigation.approach_object import ApproachObjectState class Navigation(threading.Thread): @@ -30,13 +31,25 @@ def __init__(self, context: Context): ) self.state_machine.add_transitions(PostBackupState(), [WaypointState(), RecoveryState()]) self.state_machine.add_transitions( - RecoveryState(), [WaypointState(), SearchState(), PostBackupState(), ApproachPostState()] + RecoveryState(), + [WaypointState(), SearchState(), PostBackupState(), ApproachPostState(), ApproachObjectState()], + ) + self.state_machine.add_transitions( + SearchState(), [ApproachPostState(), ApproachObjectState(), WaypointState(), RecoveryState()] ) - self.state_machine.add_transitions(SearchState(), [ApproachPostState(), WaypointState(), RecoveryState()]) self.state_machine.add_transitions(DoneState(), [WaypointState()]) self.state_machine.add_transitions( - WaypointState(), [PostBackupState(), ApproachPostState(), SearchState(), RecoveryState(), DoneState()] + WaypointState(), + [ + PostBackupState(), + ApproachPostState(), + ApproachObjectState(), + SearchState(), + RecoveryState(), + DoneState(), + ], ) + self.state_machine.add_transitions(ApproachObjectState(), [DoneState(), SearchState()]) 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/search.py b/src/navigation/search.py index 95ec83ba1..f94d36664 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -147,13 +147,13 @@ def on_loop(self, context) -> State: context.rover.send_drive_command(cmd_vel) current_waypoint = context.course.current_waypoint() - if current_waypoint.type == "POST": + if current_waypoint.type.val == WaypointType.POST: 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 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 == "MALLET": + elif current_waypoint.type.val == WaypointType.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 approach_object.ApproachObjectState() # if we see the object return self diff --git a/src/navigation/waypoint.py b/src/navigation/waypoint.py index de930f02e..bad8b6cf4 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -36,11 +36,11 @@ def on_loop(self, context) -> State: if context.course.look_for_post(): if context.env.current_target_pos() is not None: - return approach_post.ApproachPostState() + 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: + 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 @@ -53,14 +53,14 @@ def on_loop(self, context) -> State: self.DRIVE_FWD_THRESH, ) if arrived: - if not context.course.look_for_post(): + if not context.course.look_for_post() and not context.course.look_for_object(): # 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. return search.SearchState() # TODO elif looking for water bottle: - # return water_bottle_search.WaterBottleSearchState() + # return water_bottle_search.WaterBottleSearchState() if context.rover.stuck: context.rover.previous_state = self From aa58caf09053a66672fdada4a119ebad2c14c867 Mon Sep 17 00:00:00 2001 From: Rahul Date: Thu, 25 Jan 2024 19:28:22 -0500 Subject: [PATCH 046/145] changed driver to use gps time --- src/localization/gps_driver.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index 029ac96cb..07cb577f6 100755 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -17,7 +17,6 @@ from sensor_msgs.msg import NavSatFix from rtcm_msgs.msg import Message - class GPS_Driver(): def __init__(self): @@ -64,7 +63,6 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: print("RTCM message successfully used by receiver\n") - # rospy.loginfo(vars(rover_gps_data)) if rover_gps_data.identity == "NAV-PVT": @@ -72,7 +70,10 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: parsed_latitude = msg.lat parsed_longitude = msg.lon parsed_altitude = msg.hMSL - message_header = Header(stamp=rospy.Time.now(), frame_id="base_link") + time = msg.nano + + message_header = Header(stamp=rospy.Time(nsecs=time), frame_id="base_link") + self.gps_pub.publish( NavSatFix(header=message_header, latitude=parsed_latitude, longitude=parsed_longitude, altitude=parsed_altitude) From 987155a846f34921e8fd983d9394177277cb2f65 Mon Sep 17 00:00:00 2001 From: Ajay Sumanth Date: Thu, 25 Jan 2024 16:51:54 -0800 Subject: [PATCH 047/145] added target coord calculations --- .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 +++++++++ 5 files changed, 46 insertions(+) create mode 100644 .catkin_tools/CATKIN_IGNORE create mode 100644 .catkin_tools/README create mode 100644 .catkin_tools/VERSION create mode 100644 .catkin_tools/profiles/default/build.yaml create mode 100644 .catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml diff --git a/.catkin_tools/CATKIN_IGNORE b/.catkin_tools/CATKIN_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/.catkin_tools/README b/.catkin_tools/README new file mode 100644 index 000000000..4706f4756 --- /dev/null +++ b/.catkin_tools/README @@ -0,0 +1,13 @@ +# 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 new file mode 100644 index 000000000..f76f91317 --- /dev/null +++ b/.catkin_tools/VERSION @@ -0,0 +1 @@ +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 new file mode 100644 index 000000000..910ef4283 --- /dev/null +++ b/.catkin_tools/profiles/default/build.yaml @@ -0,0 +1,22 @@ +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 new file mode 100644 index 000000000..134c59ade --- /dev/null +++ b/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml @@ -0,0 +1,10 @@ + + catkin_tools_prebuild + + This package is used to generate catkin setup files. + + 0.0.0 + BSD + jbohren + catkin + From 6e9249693f61a1706f44a2ed5f3f66b457c76dd6 Mon Sep 17 00:00:00 2001 From: Ajay Sumanth Date: Thu, 25 Jan 2024 16:55:23 -0800 Subject: [PATCH 048/145] added target coord calculations --- src/navigation/long_range_state.py | 25 +++++++++++++++++++++---- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/src/navigation/long_range_state.py b/src/navigation/long_range_state.py index 13df495af..2bd9737ea 100644 --- a/src/navigation/long_range_state.py +++ b/src/navigation/long_range_state.py @@ -17,6 +17,7 @@ from state import ApproachTargetBaseState from state import approach_post import numpy as np +import math TAG_SEARCH_EXPIRATION = 3 @@ -40,10 +41,26 @@ def on_exit(self, context): def get_target_pos(self, context) -> Optional[int]: tag_id = context.current_waypoint()["tagid"] bearing = context.env.tag_data_dict[tag_id][2] - rover_position = context.rover.get_pose().position - target_pos_x = 90 - bearing - target_pos_y = bearing - target_pos_z = rover_position[2] + rover_position = context.rover.get_pose().rover_position + #target_pos = context.rover.get_target_pos().target_pos + + #Get x and y coords from pose + rover_position_x = rover_position[1][0][0] #if not, try 0,0 on 2d coords + rover_position_y = rover_position[0][1][0] + + #target_pos_x: target_pos[1][0][0] + #target_pos_y: target_pos[0][1][0] + + + target_position_x= math.sin(bearing)*25 + rover_position_x + target_position_y= math.cos(bearing)*25 + rover_position_y + target_pos = target_position_x,target_position_y + #Bearing and dist to coord: + + # target_pos_x = 90 - bearing + # target_pos_y = bearing + # target_pos_z = rover_position[2] + target_coordinates = np.ndarray context.tag_data_callback() return target_pos From 1cdc56160653080315e3a4afcbed8e1a26df2f0d Mon Sep 17 00:00:00 2001 From: Emerson-Ramey Date: Sat, 27 Jan 2024 04:52:51 +0000 Subject: [PATCH 049/145] update to approach water bottle too --- src/navigation/context.py | 2 +- src/navigation/search.py | 7 +++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/navigation/context.py b/src/navigation/context.py index d9f9b8ea6..42b2cf09c 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -103,7 +103,7 @@ def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray] elif current_waypoint.type.val == WaypointType.MALLET: return self.get_target_pos("hammer", in_odom) elif current_waypoint.type == WaypointType.WATER_BOTTLE: - return self.get_target_pos("water_bottle", in_odom) + return self.get_target_pos("bottle", in_odom) else: # print("CURRENT WAYPOINT IS NOT A POST OR OBJECT") return None diff --git a/src/navigation/search.py b/src/navigation/search.py index f94d36664..ab71505e7 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -96,7 +96,7 @@ def on_enter(self, context) -> None: search_center = context.course.current_waypoint() if not self.is_recovering: - if search_center.type == WaypointType.POST: + if search_center.type.val == WaypointType.POST: self.traj = SearchTrajectory.spiral_traj( context.rover.get_pose().position[0:2], self.SPIRAL_COVERAGE_RADIUS, @@ -104,8 +104,7 @@ def on_enter(self, context) -> None: self.SEGMENTS_PER_ROTATION, search_center.tag_id, ) - - if WaypointType.MALLET: + else: # water bottle or mallet self.traj = SearchTrajectory.spiral_traj( context.rover.get_pose().position[0:2], self.SPIRAL_COVERAGE_RADIUS / 2, @@ -153,7 +152,7 @@ def on_loop(self, context) -> State: # 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.val == WaypointType.MALLET: + 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 return self From e006721adf5ccec392bfce689e4fc86b746155b4 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Sat, 27 Jan 2024 14:10:06 -0500 Subject: [PATCH 050/145] fix bearing calc --- msg/LongRangeTag.msg | 7 ++---- .../long_range_tag_detector.cpp | 1 + .../long_range_tag_detector.hpp | 5 ++-- .../long_range_tag_detector.processing.cpp | 25 +++++++++---------- 4 files changed, 18 insertions(+), 20 deletions(-) diff --git a/msg/LongRangeTag.msg b/msg/LongRangeTag.msg index 6a8ea2cb2..ae4d49ab8 100644 --- a/msg/LongRangeTag.msg +++ b/msg/LongRangeTag.msg @@ -1,7 +1,4 @@ # 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 +# Bearing is in degrees from center of image uint8 id -float32 xOffset -float32 yOffset \ No newline at end of file +float32 bearing \ No newline at end of file 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 29688efaa..e85126d8a 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,7 @@ namespace mrover { 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); 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 4826554a8..cb0b4bf3d 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 @@ -32,6 +32,7 @@ namespace mrover { int mTagIncrementWeight{}; int mTagDecrementWeight{}; int mTagRemoveWeight{}; + float mLongRangeFov{}; cv::Ptr mDetectorParams; cv::Ptr mDictionary; @@ -126,7 +127,7 @@ namespace mrover { * @param tagCorners reference to tag corners, passed to @see getTagCenterPixels * @return float of tag bearing */ - float getTagBearing(std::vector& tagCorners) const; + float getTagBearing(cv::Point2f& tagCenter) const; /** * @see updateHitCounts() @@ -142,7 +143,7 @@ namespace mrover { * Publish the tags which have been detected for more than * mMinHitCountBeforePublish */ - void publishThresholdTags(); + void publishPermanentTags(); /** * publishes the thresholded tags onto an image using OpenCV 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 30f36cecb..e54a78150 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,5 +1,6 @@ #include "long_range_tag_detector.hpp" #include +#include #include namespace mrover { @@ -28,7 +29,7 @@ namespace mrover { } //Publish all tags that meet threshold - //publishThresholdTags(); + publishPermanentTags(); //PUBLISH TAGS } @@ -50,7 +51,7 @@ namespace mrover { } void LongRangeTagDetectorNodelet::runTagDetection() { - std::cout << mDetectorParams->adaptiveThreshConstant << std::endl; + // std::cout << mDetectorParams->adaptiveThreshConstant << std::endl; cv::aruco::detectMarkers(mImg, mDictionary, mImmediateCorners, mImmediateIds, mDetectorParams); } @@ -59,7 +60,8 @@ 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; + cv::Point2f center = getTagCenterPixels(mImmediateCorners[i]); + // std::cout << "bearing: " << getTagBearing(center) << "!!!" << std::endl; } //Now decrement all the hitcounts for tags that were not updated @@ -95,6 +97,7 @@ namespace mrover { lrt.updated = true; lrt.imageCenter = getNormedTagCenterOffset(tagCorners); + std::cout << "lrt image center " << lrt.imageCenter.x << std::endl; return lrt; } @@ -152,18 +155,16 @@ namespace mrover { return offsetCenterPoint; } - float LongRangeTagDetectorNodelet::getTagBearing(std::vector& tagCorners) const { + float LongRangeTagDetectorNodelet::getTagBearing(cv::Point2f& tagCenter) const { //for HD720 resolution - cv::Point2f center = getTagCenterPixels(tagCorners); auto imageWidth = (float) mImgMsg.width; - std::cout << "width: " << imageWidth << std::endl; - float angleOfFOV = 101; - float bearing = ((center.x - (imageWidth / 2)) / imageWidth) * angleOfFOV; - + 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::publishThresholdTags() { + void LongRangeTagDetectorNodelet::publishPermanentTags() { //Loop through all the tagsj LongRangeTags tagsMessage; // @@ -174,9 +175,7 @@ namespace mrover { //Fill in fields newTagMessage.id = tag.second.id; - newTagMessage.xOffset = tag.second.imageCenter.x; - newTagMessage.yOffset = tag.second.imageCenter.y; - + newTagMessage.bearing = getTagBearing(tag.second.imageCenter); //Add to back of tagsMessage tagsMessage.longRangeTags.push_back(newTagMessage); } From 3eb039b5b79e1bfdf63d318affb9cbe30c370fac Mon Sep 17 00:00:00 2001 From: ankithu Date: Sat, 27 Jan 2024 14:33:30 -0500 Subject: [PATCH 051/145] navigation ready for long range testing --- src/navigation/context.py | 62 ++++++++++++++++++++++++------ src/navigation/long_range_state.py | 49 ++++++++++------------- src/navigation/search.py | 11 +++--- 3 files changed, 77 insertions(+), 45 deletions(-) diff --git a/src/navigation/context.py b/src/navigation/context.py index 42b2cf09c..5a9d4cce6 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -9,7 +9,15 @@ import rospy import tf2_ros from geometry_msgs.msg import Twist -from mrover.msg import Waypoint, GPSWaypoint, WaypointType, GPSPointList, Course as CourseMsg +from mrover.msg import ( + Waypoint, + GPSWaypoint, + WaypointType, + GPSPointList, + Course as CourseMsg, + LongRangeTag, + LongRangeTags, +) from mrover.srv import EnableAuton, EnableAutonRequest, EnableAutonResponse from navigation.drive import DriveController from std_msgs.msg import Time, Bool @@ -59,11 +67,10 @@ class Environment: """ ctx: Context + long_range_tags: LongRangeTagStore NO_FIDUCIAL: ClassVar[int] = -1 arrived_at_target: bool = None last_target_location: Optional[np.ndarray] = None - # TODO add dictionary for long range tag id : (time received, our hit counter, bearing) - # tag_data_dict = {} def get_target_pos(self, id: str, in_odom_frame: bool = True) -> Optional[np.ndarray]: """ @@ -109,6 +116,41 @@ def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray] return None +class LongRangeTagStore: + @dataclass + class TagData: + hit_count: int + tag: LongRangeTag + + ctx: Context + __data: dict[int, TagData] + min_hits: int + + def __init__(self, ctx: Context, min_hits: int) -> None: + self.ctx = ctx + self.__data = {} + self.min_hits = min_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 + + for tag in tags: + if tag.id not in self.__data: + self.__data[tag.id] = self.TagData(hit_count=1, tag=tag) + + def get(self, tag_id: int) -> Optional[LongRangeTag]: + if tag_id in self.__data and self.__data[tag_id].hit_count > self.min_hits: + return self.__data[tag_id].tag + else: + return None + + @dataclass class Course: ctx: Context @@ -243,13 +285,14 @@ def __init__(self): self.stuck_listener = rospy.Subscriber("nav_stuck", Bool, self.stuck_callback) self.course = None self.rover = Rover(self, False, "") - self.env = Environment(self) + # TODO update min_hits to be a param + self.env = Environment(self, long_range_tags=LongRangeTagStore(self, 3)) 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("tag_data", list, self.tag_data_callback) + self.tag_data_listener = rospy.Subscriber("tags", LongRangeTags, self.tag_data_callback) def recv_enable_auton(self, req: EnableAutonRequest) -> EnableAutonResponse: if req.enable: @@ -261,10 +304,5 @@ 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: list) -> None: - # for tag in tags: - # tag_id = tag["id"] - # hit_count = tag["hitCount"] - # bearing = tag["bearing"] - # time_received = rospy.Time() - # self.env.tag_data_dict[tag_id] = (time_received, hit_count, bearing) + def tag_data_callback(self, tags: LongRangeTags) -> None: + self.env.long_range_tags.push_frame(tags.longRangeTags) diff --git a/src/navigation/long_range_state.py b/src/navigation/long_range_state.py index 2bd9737ea..74149daf2 100644 --- a/src/navigation/long_range_state.py +++ b/src/navigation/long_range_state.py @@ -6,6 +6,7 @@ from context import Context from util.state_lib.state import State from util.state_lib.state_machine import StateMachine +from util.np_utils import normalize from abc import ABC, abstractmethod from state import DoneState from search import SearchState @@ -19,8 +20,6 @@ import numpy as np import math -TAG_SEARCH_EXPIRATION = 3 - class LongRangeState(ApproachTargetBaseState): """ @@ -39,39 +38,33 @@ def on_exit(self, context): pass def get_target_pos(self, context) -> Optional[int]: - tag_id = context.current_waypoint()["tagid"] - bearing = context.env.tag_data_dict[tag_id][2] - rover_position = context.rover.get_pose().rover_position - #target_pos = context.rover.get_target_pos().target_pos - - #Get x and y coords from pose - rover_position_x = rover_position[1][0][0] #if not, try 0,0 on 2d coords - rover_position_y = rover_position[0][1][0] + 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 - #target_pos_x: target_pos[1][0][0] - #target_pos_y: target_pos[0][1][0] + rover_position = pose.position + rover_direction = pose.rotation.direction_vector() + bearing_to_tag = np.radians(tag.bearing) - target_position_x= math.sin(bearing)*25 + rover_position_x - target_position_y= math.cos(bearing)*25 + rover_position_y - target_pos = target_position_x,target_position_y - #Bearing and dist to coord: + # 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)]] + ) - # target_pos_x = 90 - bearing - # target_pos_y = bearing - # target_pos_z = rover_position[2] + direction_to_tag = bearing_rotation_mat @ rover_direction[:2] - target_coordinates = np.ndarray - context.tag_data_callback() - return target_pos + direction_to_tag = normalize(direction_to_tag) + distance = 20 # TODO replace with rosparam + tag_position = rover_position + direction_to_tag * distance def determine_next(self, context, finished: bool) -> State: - fid_pos = context.env.current_fid_pos() # Need to pass in the fid_id - tag_time = context.env.tag_data_dict[context.current_tag_id][0] + fid_pos = context.env.current_fid_pos() if fid_pos is None: - if rospy.Time() - tag_time > TAG_SEARCH_EXPIRATION: - return state.SearchState() - else: - return state.LongRangeState() + return state.LongRangeState() else: return approach_post.ApproachPostState() diff --git a/src/navigation/search.py b/src/navigation/search.py index ab71505e7..e08f32c28 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -104,7 +104,7 @@ def on_enter(self, context) -> None: self.SEGMENTS_PER_ROTATION, search_center.tag_id, ) - else: # water bottle or mallet + else: # water bottle or mallet self.traj = SearchTrajectory.spiral_traj( context.rover.get_pose().position[0:2], self.SPIRAL_COVERAGE_RADIUS / 2, @@ -147,11 +147,12 @@ def on_loop(self, context) -> State: 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 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() + 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 approach_post.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 From 16e4b13d96ec102e3c39205396b512c6088010b7 Mon Sep 17 00:00:00 2001 From: ankithu Date: Sun, 28 Jan 2024 12:50:09 -0500 Subject: [PATCH 052/145] max hit count --- src/navigation/context.py | 7 +++++-- src/navigation/long_range_state.py | 1 + 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/navigation/context.py b/src/navigation/context.py index 5a9d4cce6..d86b4a94b 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -121,15 +121,17 @@ class LongRangeTagStore: 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) -> None: + 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(): @@ -139,13 +141,14 @@ def push_frame(self, tags: List[LongRangeTag]) -> None: 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) def get(self, tag_id: int) -> Optional[LongRangeTag]: - if tag_id in self.__data and self.__data[tag_id].hit_count > self.min_hits: + if tag_id in self.__data and self.__data[tag_id].hit_count >= self.min_hits: return self.__data[tag_id].tag else: return None diff --git a/src/navigation/long_range_state.py b/src/navigation/long_range_state.py index 74149daf2..a9ee40e27 100644 --- a/src/navigation/long_range_state.py +++ b/src/navigation/long_range_state.py @@ -61,6 +61,7 @@ def get_target_pos(self, context) -> Optional[int]: direction_to_tag = normalize(direction_to_tag) distance = 20 # TODO replace with rosparam 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() From 6265aa4cb3679e06598496d08cdee242e7f62fd7 Mon Sep 17 00:00:00 2001 From: dllliu Date: Sun, 28 Jan 2024 13:01:34 -0500 Subject: [PATCH 053/145] new msg type for rtk fix status --- msg/rtkStatus.msg | 5 ++++ src/localization/gps_driver.py | 51 ++++++++++++++++++---------------- 2 files changed, 32 insertions(+), 24 deletions(-) create mode 100644 msg/rtkStatus.msg diff --git a/msg/rtkStatus.msg b/msg/rtkStatus.msg new file mode 100644 index 000000000..bca134e45 --- /dev/null +++ b/msg/rtkStatus.msg @@ -0,0 +1,5 @@ +uint8 NO_RTK = 0 +uint8 FLOATING_RTK = 1 +uint8 RTK_FIX = 2 + +uint8 RTK_FIX_TYPE \ No newline at end of file diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index 029ac96cb..2914de873 100755 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -1,43 +1,45 @@ #!/usr/bin/env python3 -''' +""" Reads and parses NMEA messages from the onboard GPS to provide location data to the rover over LCM (/gps). Subscribes to /rtcm and passes RTCM messages to the onboard gps to acquire an RTK fix. -''' +""" import serial import asyncio import rospy import threading import numpy as np from os import getenv + # from rover_msgs import GPS, RTCM from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol, UBXMessage from std_msgs.msg import Header from sensor_msgs.msg import NavSatFix from rtcm_msgs.msg import Message +from mrover.msg import rtkStatus -class GPS_Driver(): - +class GPS_Driver: def __init__(self): - rospy.init_node('gps_driver') + rospy.init_node("gps_driver") self.port = rospy.get_param("port") self.baud = rospy.get_param("baud") - self.base_station_sub = rospy.Subscriber('/rtcm', Message, self.process_rtcm) - self.gps_pub = rospy.Publisher('fix', NavSatFix, queue_size=1) - self.lock = threading.Lock() + self.base_station_sub = rospy.Subscriber("/rtcm", Message, self.process_rtcm) + self.gps_pub = rospy.Publisher("fix", NavSatFix, queue_size=1) + self.rtk_fix_pub = rospy.Publisher("rtk_fix_status", rtkStatus, queue_size=1) + self.lock = threading.Lock() def connect(self): - #open connection to rover GPS + # open connection to rover GPS self.ser = serial.Serial(self.port, self.baud) self.reader = UBXReader(self.ser, protfilter=(UBX_PROTOCOL | RTCM3_PROTOCOL)) return self def exit(self) -> None: - #close connection + # close connection self.ser.close() def process_rtcm(self, data) -> None: @@ -51,7 +53,7 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: # skip if message could not be parsed if not msg: return - + if rover_gps_data.identity == "RXM-RTCM": print("RXM") msg_used = msg.msgUsed @@ -63,10 +65,8 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: elif msg_used == 2: print("RTCM message successfully used by receiver\n") - - # rospy.loginfo(vars(rover_gps_data)) - + if rover_gps_data.identity == "NAV-PVT": print("PVT") parsed_latitude = msg.lat @@ -75,12 +75,17 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: message_header = Header(stamp=rospy.Time.now(), frame_id="base_link") self.gps_pub.publish( - NavSatFix(header=message_header, latitude=parsed_latitude, longitude=parsed_longitude, altitude=parsed_altitude) + NavSatFix( + header=message_header, + latitude=parsed_latitude, + longitude=parsed_longitude, + altitude=parsed_altitude, + ) ) + self.rtk_fix_pub.publish(rtkStatus(msg_used)) if msg.difSoln == 1: print("Differemtial Corrections Applied") - # publidh to navsatstatus in navsatfix if msg.carrSoln == 0: @@ -88,15 +93,13 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: elif msg.carrSoln == 1: print("Floating RTK Fix\n") elif msg.carrSoln == 2: - print("RTK FIX\n") + print("RTK FIX\n") if rover_gps_data.identity == "NAV-STATUS": pass - - def gps_data_thread(self) -> None: - #TODO: add more message checks if needed + # TODO: add more message checks if needed while not rospy.is_shutdown(): with self.lock: if self.ser.in_waiting: @@ -105,14 +108,14 @@ def gps_data_thread(self) -> None: def main(): - #change values + # change values rtk_manager = GPS_Driver() rtk_manager.connect() - #rover_gps_thread = threading.Thread(rtk_manager.gps_data_thread) - #rover_gps_thread.start() + # rover_gps_thread = threading.Thread(rtk_manager.gps_data_thread) + # rover_gps_thread.start() rtk_manager.gps_data_thread() rtk_manager.exit() if __name__ == "__main__": - main() \ No newline at end of file + main() From bf08fc524cee12c3bbcd8cd19cefd4561c246977 Mon Sep 17 00:00:00 2001 From: Stephen Barstys Date: Sun, 28 Jan 2024 13:13:52 -0500 Subject: [PATCH 054/145] time threshold check for LongRangeTagData --- config/navigation.yaml | 6 +++++- src/navigation/context.py | 10 +++++++++- src/navigation/long_range_state.py | 4 +++- 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/config/navigation.yaml b/config/navigation.yaml index fe5a6adfb..153a40dc7 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -46,5 +46,9 @@ failure_identification: watchdog: window_size: 100 #size of window we are looking at for being stuck - angular_threshold: 0.1 + angular_threshold: 0.1 linear_threshold: 0.2 + +long_range: + distance_ahead: 20 + time_threshold: 5 diff --git a/src/navigation/context.py b/src/navigation/context.py index d86b4a94b..ba963f426 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -23,9 +23,12 @@ 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") @@ -148,7 +151,12 @@ def push_frame(self, tags: List[LongRangeTag]) -> None: self.__data[tag.id] = self.TagData(hit_count=1, tag=tag) def get(self, tag_id: int) -> Optional[LongRangeTag]: - if tag_id in self.__data and self.__data[tag_id].hit_count >= self.min_hits: + 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 diff --git a/src/navigation/long_range_state.py b/src/navigation/long_range_state.py index a9ee40e27..6504dd17d 100644 --- a/src/navigation/long_range_state.py +++ b/src/navigation/long_range_state.py @@ -20,6 +20,8 @@ import numpy as np import math +DIST_AHEAD = get_rosparam("long_range/distance_ahead", 20) + class LongRangeState(ApproachTargetBaseState): """ @@ -59,7 +61,7 @@ def get_target_pos(self, context) -> Optional[int]: direction_to_tag = bearing_rotation_mat @ rover_direction[:2] direction_to_tag = normalize(direction_to_tag) - distance = 20 # TODO replace with rosparam + distance = DIST_AHEAD tag_position = rover_position + direction_to_tag * distance return tag_position From 0a5cef58fbfd28b90d99eba2df8089a3ea3251a6 Mon Sep 17 00:00:00 2001 From: ankithu Date: Sun, 28 Jan 2024 13:52:52 -0500 Subject: [PATCH 055/145] fix mypy errors --- src/navigation/context.py | 4 +- src/navigation/long_range.py | 57 +++++++++++++++++------ src/navigation/long_range_state.py | 73 ------------------------------ src/navigation/search.py | 4 +- src/navigation/waypoint.py | 4 +- 5 files changed, 50 insertions(+), 92 deletions(-) delete mode 100644 src/navigation/long_range_state.py diff --git a/src/navigation/context.py b/src/navigation/context.py index ba963f426..fa68db9c5 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -72,7 +72,7 @@ class Environment: ctx: Context long_range_tags: LongRangeTagStore NO_FIDUCIAL: ClassVar[int] = -1 - arrived_at_target: bool = None + arrived_at_target: bool = False last_target_location: Optional[np.ndarray] = None def get_target_pos(self, id: str, in_odom_frame: bool = True) -> Optional[np.ndarray]: @@ -148,7 +148,7 @@ def push_frame(self, tags: List[LongRangeTag]) -> None: for tag in tags: if tag.id not in self.__data: - self.__data[tag.id] = self.TagData(hit_count=1, tag=tag) + self.__data[tag.id] = self.TagData(hit_count=1, tag=tag, time=rospy.get_time()) def get(self, tag_id: int) -> Optional[LongRangeTag]: time_difference = rospy.get_time() - self.__data[tag_id].time diff --git a/src/navigation/long_range.py b/src/navigation/long_range.py index 76c0d4949..500511517 100644 --- a/src/navigation/long_range.py +++ b/src/navigation/long_range.py @@ -1,12 +1,26 @@ 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 abc import abstractmethod +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 state import ApproachTargetBaseState +from state import approach_post +import numpy as np +import math -from navigation import approach_post -from navigation.approach_target_base import ApproachTargetBaseState +DIST_AHEAD = get_rosparam("long_range/distance_ahead", 20) class LongRangeState(ApproachTargetBaseState): @@ -26,17 +40,34 @@ def on_exit(self, context): pass def get_target_pos(self, context) -> Optional[int]: - 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 + 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: - # if we see the tag in the ZED transition to approach post - fid_pos = context.env.current_target_pos() + fid_pos = context.env.current_fid_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 + return state.LongRangeState() else: return approach_post.ApproachPostState() diff --git a/src/navigation/long_range_state.py b/src/navigation/long_range_state.py deleted file mode 100644 index 6504dd17d..000000000 --- a/src/navigation/long_range_state.py +++ /dev/null @@ -1,73 +0,0 @@ -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 normalize -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 state import ApproachTargetBaseState -from state import approach_post -import numpy as np -import math - -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 = normalize(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 state.LongRangeState() - else: - return approach_post.ApproachPostState() diff --git a/src/navigation/search.py b/src/navigation/search.py index 0500c74a6..3db5ede04 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -10,7 +10,7 @@ from util.ros_utils import get_rosparam from util.state_lib.state import State -from navigation import approach_post, approach_object, recovery, waypoint +from navigation import approach_post, approach_object, recovery, waypoint, long_range from navigation.context import convert_cartesian_to_gps from navigation.trajectory import Trajectory @@ -152,7 +152,7 @@ def on_loop(self, context) -> State: # 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 approach_post.LongRangeState() + 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 diff --git a/src/navigation/waypoint.py b/src/navigation/waypoint.py index a03b2fa84..62181a5c6 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 +from navigation import search, recovery, approach_post, post_backup, state, approach_object, long_range class WaypointState(State): @@ -39,7 +39,7 @@ def on_loop(self, context) -> State: # 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 approach_post.LongRangeState() + return long_range.LongRangeState() elif context.course.look_for_object(): if context.env.current_target_pos() is not None: return approach_object.ApproachObjectState() From 5b5f4f11309dc4a1d5d0a9275e0c50a17cce4223 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Mon, 29 Jan 2024 21:25:29 -0500 Subject: [PATCH 056/145] merge long range with new states --- launch/perception.launch | 9 +- plugins/tag_detector_plugin.xml | 2 +- src/perception/long_range_cam/main.cpp | 23 ++ src/perception/long_range_cam/pch.hpp | 2 - .../tag_detector/long_range_cam/main.cpp | 22 ++ .../tag_detector/long_range_cam/pch.hpp | 2 - .../tag_detector/{ => zed}/main.cpp | 0 .../zed/tag_detector.processing.cpp | 346 +++++++++--------- 8 files changed, 226 insertions(+), 180 deletions(-) create mode 100644 src/perception/long_range_cam/main.cpp create mode 100644 src/perception/tag_detector/long_range_cam/main.cpp rename src/perception/tag_detector/{ => zed}/main.cpp (100%) diff --git a/launch/perception.launch b/launch/perception.launch index 0fcce3c20..67f0e5aeb 100644 --- a/launch/perception.launch +++ b/launch/perception.launch @@ -2,7 +2,12 @@ + + + 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 057/145] 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 058/145] 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 059/145] 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 060/145] 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 594f7795b0508a263407f84fdd0bcd9797a44e59 Mon Sep 17 00:00:00 2001 From: Rahul Date: Tue, 30 Jan 2024 19:18:29 -0500 Subject: [PATCH 061/145] changed gps time --- src/localization/gps_driver.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index a7fc5d51b..c1f875a2f 100755 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -18,6 +18,7 @@ from sensor_msgs.msg import NavSatFix from rtcm_msgs.msg import Message from mrover.msg import rtkStatus +import datetime class GPS_Driver: @@ -72,9 +73,10 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: parsed_latitude = msg.lat parsed_longitude = msg.lon parsed_altitude = msg.hMSL - time = msg.nano + time = datetime.datetime(year=msg.year, month=msg.month, day=msg.day, hour= msg.hour, second=msg.second, microsecond= int((msg.nano/1000))) + time=time.timestamp() - message_header = Header(stamp=rospy.Time(nsecs=time), frame_id="base_link") + message_header = Header(stamp=time, frame_id="base_link") self.gps_pub.publish( NavSatFix( From 31f0702cc75ab1be764f9ac007e58156bed79f5b Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Tue, 30 Jan 2024 19:55:12 -0500 Subject: [PATCH 062/145] testing long range --- config/navigation.yaml | 2 + launch/perception.launch | 4 +- scripts/debug_course_publisher.py | 2 +- src/navigation/context.py | 31 ++-- src/navigation/long_range.py | 5 +- src/navigation/navigation.py | 2 +- urdf/rover/rover.urdf.xacro | 240 +++++++++++++++--------------- 7 files changed, 150 insertions(+), 136 deletions(-) diff --git a/config/navigation.yaml b/config/navigation.yaml index 153a40dc7..96ba22aa2 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -52,3 +52,5 @@ watchdog: long_range: distance_ahead: 20 time_threshold: 5 + increment_weight: 100 + decrement_weight: 1 diff --git a/launch/perception.launch b/launch/perception.launch index 67f0e5aeb..76141dc77 100644 --- a/launch/perception.launch +++ b/launch/perception.launch @@ -2,9 +2,9 @@ - + args="load mrover/TagDetectorNodelet nodelet_manager" output="screen" /> None: + def __init__(self, ctx: Context, min_hits: int, max_hits: int = 100) -> 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 + for _, cur_tag in list(self.__data.items()): + tags_ids = [tag.id for tag in tags] + if cur_tag.tag.id not in tags_ids: + cur_tag.hit_count -= DECREMENT_WEIGHT + print(f"dec {cur_tag.tag.id} to {cur_tag.hit_count}") if cur_tag.hit_count <= 0: del self.__data[cur_tag.tag.id] + print("deleted from dict") else: - cur_tag.hit_count += 1 - cur_tag.hit_count = min(cur_tag.hit_count, self.max_hits) + cur_tag.hit_count += INCREMENT_WEIGHT + if cur_tag.hit_count > self.max_hits: + cur_tag.hit_count = self.max_hits + print("TAG ID", cur_tag.tag.id, "HIT COUNT:", cur_tag.hit_count) 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()) + self.__data[tag.id] = self.TagData(hit_count=INCREMENT_WEIGHT, tag=tag, time=rospy.get_time()) def get(self, tag_id: int) -> Optional[LongRangeTag]: if len(self.__data) == 0: return None - + if tag_id not in self.__data: + print(f"{tag_id} not seen.") + return None time_difference = rospy.get_time() - self.__data[tag_id].time + print(f"tag id hc: {self.__data[tag_id].hit_count}") if ( tag_id in self.__data and self.__data[tag_id].hit_count >= self.min_hits @@ -275,7 +286,7 @@ class Context: vis_publisher: rospy.Publisher course_listener: rospy.Subscriber stuck_listener: rospy.Subscriber - # tag_data_listener: rospy.Subscriber + tag_data_listener: rospy.Subscriber # Use these as the primary interfaces in states course: Optional[Course] @@ -300,7 +311,7 @@ def __init__(self): 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, long_range_tags=LongRangeTagStore(self, 50)) self.disable_requested = False self.use_odom = rospy.get_param("use_odom_frame") self.world_frame = rospy.get_param("world_frame") diff --git a/src/navigation/long_range.py b/src/navigation/long_range.py index 2f13bc4b0..9c7184ae5 100644 --- a/src/navigation/long_range.py +++ b/src/navigation/long_range.py @@ -30,7 +30,7 @@ def on_exit(self, context): pass def get_target_pos(self, context) -> Optional[int]: - tag_id = context.course.get_current_waypoint().tag_id + tag_id = context.course.current_waypoint().tag_id tag = context.env.long_range_tags.get(tag_id) if tag is None: return None @@ -52,11 +52,12 @@ def get_target_pos(self, context) -> Optional[int]: direction_to_tag = normalized(direction_to_tag) distance = DIST_AHEAD + direction_to_tag = np.array([direction_to_tag[0], direction_to_tag[1], 0.0]) 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() + fid_pos = context.env.current_target_pos() if fid_pos is None: return self else: diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py index 056ac5cfc..44d4db391 100755 --- a/src/navigation/navigation.py +++ b/src/navigation/navigation.py @@ -60,7 +60,7 @@ def __init__(self, context: Context): ], ) self.state_machine.add_transitions(ApproachObjectState(), [DoneState(), SearchState()]) - self.state_machine.add_transitions(LongRangeState(), [ApproachPostState()]) + self.state_machine.add_transitions(LongRangeState(), [ApproachPostState(), SearchState()]) 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/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 7869e4836..85db94adf 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -3,43 +3,43 @@ - - - - - - - - + + + + + + + + - + - + + iyy="${m*(3*r*r+h*h)/12}" iyz="0" + izz="${m*r*r/2}" /> - + + iyy="${m / 12.0 * (d*d + h*h)}" iyz="0" + izz="${m / 12.0 * (d*d + w*w)}" /> - + + iyy="${2.0*m*(r*r)/5.0}" iyz="0" + izz="${2.0*m*(r*r)/5.0}" /> @@ -47,16 +47,16 @@ - + - + - + - + @@ -64,229 +64,229 @@ - - - - - + + + + + - + - + - + - - - + + + - + - + - + - + - + - + - + - + - + - + - + - + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - + - + - + - + - + - + - + - - - + + + - - - - - + + + + + - - - - - + + + + + + x="0.034346" y="0" z="0.049024" + lower="${-TAU / 8}" upper="0" /> + x="0.534365" y="0" z="0.009056" + lower="-0.959931" upper="2.87979" /> + x="0.546033" y="0" z="0.088594" + lower="-2.35619" upper="2.35619" /> + x="0.044886" y="0" z="0" + lower="-2.35619" upper="2.35619" /> - - + + - - - + + + - + - - - + + + - + - - - + + + - + - - - + + + - + - - - + + + - + From 9be5063329d9d4341453d8439b15f8d983e07eff Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Wed, 31 Jan 2024 21:18:35 -0500 Subject: [PATCH 063/145] update time and add transitions to approach post --- config/navigation.yaml | 2 +- scripts/debug_course_publisher.py | 10 +++---- src/navigation/approach_post.py | 47 ------------------------------- src/navigation/context.py | 18 ++++++------ src/navigation/search.py | 4 +-- src/navigation/waypoint.py | 4 +-- 6 files changed, 20 insertions(+), 65 deletions(-) diff --git a/config/navigation.yaml b/config/navigation.yaml index 96ba22aa2..cc54f3043 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -52,5 +52,5 @@ watchdog: long_range: distance_ahead: 20 time_threshold: 5 - increment_weight: 100 + increment_weight: 5 decrement_weight: 1 diff --git a/scripts/debug_course_publisher.py b/scripts/debug_course_publisher.py index 6afda25dc..248af2777 100755 --- a/scripts/debug_course_publisher.py +++ b/scripts/debug_course_publisher.py @@ -27,11 +27,11 @@ ( Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)), SE3(position=np.array([5, 0, 0])), - ) - # ( - # Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)), - # SE3(position=np.array([11, -10, 0])), - # ) + ), + ( + Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)), + SE3(position=np.array([11, -10, 0])), + ), ] ] ) diff --git a/src/navigation/approach_post.py b/src/navigation/approach_post.py index 5eaae0bc8..53c2acfe3 100644 --- a/src/navigation/approach_post.py +++ b/src/navigation/approach_post.py @@ -32,50 +32,3 @@ def determine_next(self, context, finished: bool) -> State: 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 diff --git a/src/navigation/context.py b/src/navigation/context.py index 016288834..7b6e90611 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -28,7 +28,7 @@ TAG_EXPIRATION_TIME_SECONDS = 60 TIME_THRESHOLD = get_rosparam("long_range/time_threshold", 5) -INCREMENT_WEIGHT = get_rosparam("long_range/increment_weight", 100) +INCREMENT_WEIGHT = get_rosparam("long_range/increment_weight", 5) DECREMENT_WEIGHT = get_rosparam("long_range/decrement_weight", 1) REF_LAT = rospy.get_param("gps_linearization/reference_point_latitude") @@ -133,7 +133,7 @@ class TagData: min_hits: int max_hits: int - def __init__(self, ctx: Context, min_hits: int, max_hits: int = 100) -> None: + def __init__(self, ctx: Context, min_hits: int, max_hits: int = 10) -> None: self.ctx = ctx self.__data = {} self.min_hits = min_hits @@ -144,28 +144,30 @@ def push_frame(self, tags: List[LongRangeTag]) -> None: tags_ids = [tag.id for tag in tags] if cur_tag.tag.id not in tags_ids: cur_tag.hit_count -= DECREMENT_WEIGHT - print(f"dec {cur_tag.tag.id} to {cur_tag.hit_count}") + # print(f"DEC {cur_tag.tag.id} to {cur_tag.hit_count}") if cur_tag.hit_count <= 0: del self.__data[cur_tag.tag.id] - print("deleted from dict") + # print("DELETED from dict") else: cur_tag.hit_count += INCREMENT_WEIGHT + cur_tag.time = rospy.get_time() if cur_tag.hit_count > self.max_hits: cur_tag.hit_count = self.max_hits - print("TAG ID", cur_tag.tag.id, "HIT COUNT:", cur_tag.hit_count) + # print(f"INC {cur_tag.tag.id} to {cur_tag.hit_count}") for tag in tags: if tag.id not in self.__data: + # print(f"INC {tag.id} to {INCREMENT_WEIGHT}") self.__data[tag.id] = self.TagData(hit_count=INCREMENT_WEIGHT, tag=tag, time=rospy.get_time()) def get(self, tag_id: int) -> Optional[LongRangeTag]: if len(self.__data) == 0: return None if tag_id not in self.__data: - print(f"{tag_id} not seen.") + # print(f"{tag_id} not seen.") return None time_difference = rospy.get_time() - self.__data[tag_id].time - print(f"tag id hc: {self.__data[tag_id].hit_count}") + # print(f"HIT COUNT: {self.__data[tag_id].hit_count}") if ( tag_id in self.__data and self.__data[tag_id].hit_count >= self.min_hits @@ -311,7 +313,7 @@ def __init__(self): 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, 50)) + self.env = Environment(self, long_range_tags=LongRangeTagStore(self, 3)) self.disable_requested = False self.use_odom = rospy.get_param("use_odom_frame") self.world_frame = rospy.get_param("world_frame") diff --git a/src/navigation/search.py b/src/navigation/search.py index 3db5ede04..18db30adc 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -148,8 +148,8 @@ def on_loop(self, context) -> State: 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 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() diff --git a/src/navigation/waypoint.py b/src/navigation/waypoint.py index 62181a5c6..723f90cd3 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -35,8 +35,8 @@ def on_loop(self, context) -> State: return post_backup.PostBackupState() if context.course.look_for_post(): - # if context.env.current_target_pos() is not None: - # return approach_post.ApproachPostState() + 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() From 908443b846d9ee71623aaa7a7badc46b4f223e19 Mon Sep 17 00:00:00 2001 From: Rahul Date: Thu, 1 Feb 2024 18:22:38 -0500 Subject: [PATCH 064/145] updated timestamps --- src/localization/gps_driver.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index c1f875a2f..98a88240e 100755 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -17,7 +17,7 @@ from std_msgs.msg import Header from sensor_msgs.msg import NavSatFix from rtcm_msgs.msg import Message -from mrover.msg import rtkStatus +# from mrover.msg import rtkStatus import datetime @@ -28,7 +28,7 @@ def __init__(self): self.baud = rospy.get_param("baud") self.base_station_sub = rospy.Subscriber("/rtcm", Message, self.process_rtcm) self.gps_pub = rospy.Publisher("fix", NavSatFix, queue_size=1) - self.rtk_fix_pub = rospy.Publisher("rtk_fix_status", rtkStatus, queue_size=1) + # self.rtk_fix_pub = rospy.Publisher("rtk_fix_status", rtkStatus, queue_size=1) self.lock = threading.Lock() @@ -73,8 +73,10 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: parsed_latitude = msg.lat parsed_longitude = msg.lon parsed_altitude = msg.hMSL - time = datetime.datetime(year=msg.year, month=msg.month, day=msg.day, hour= msg.hour, second=msg.second, microsecond= int((msg.nano/1000))) - time=time.timestamp() + time = datetime.datetime(year=msg.year, month=msg.month, day=msg.day, hour= msg.hour, second=msg.second , milliseconds= int((msg.nano/1e6)),microsecond = (int((msg.nano/1e6) % 1e6)) / 1e3) + time= rospy.Time(secs=time.timestamp()) + print(time, rospy.Time.now(), msg.nano) + message_header = Header(stamp=time, frame_id="base_link") @@ -86,7 +88,7 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: altitude=parsed_altitude, ) ) - self.rtk_fix_pub.publish(rtkStatus(msg_used)) + # self.rtk_fix_pub.publish(rtkStatus(msg_used)) if msg.difSoln == 1: print("Differemtial Corrections Applied") From 9350c3ddc3ece53a6d3c66894964ff8e61c379fe Mon Sep 17 00:00:00 2001 From: dllliu Date: Thu, 1 Feb 2024 18:46:15 -0500 Subject: [PATCH 065/145] bag file tests --- short_range_rtk_2024-01-28-14-32-22.bag | Bin 0 -> 505402 bytes src/localization/bag.py | 95 ++++++++++++++++++++++++ 2 files changed, 95 insertions(+) create mode 100644 short_range_rtk_2024-01-28-14-32-22.bag create mode 100644 src/localization/bag.py diff --git a/short_range_rtk_2024-01-28-14-32-22.bag b/short_range_rtk_2024-01-28-14-32-22.bag new file mode 100644 index 0000000000000000000000000000000000000000..730a73badcb8154d44d0363aebf87118a33f7292 GIT binary patch literal 505402 zcmeF42V4_P*XTn>l%|4;3K67O=)HH52Lz-jRtOM^(xeC|B_JSHzy?+j3s|vNq$)NP z6#*+CAVukdbaH1l2_|8qd4JD~_kQlkEu}}EB=CNq*A~3)NDn6l{z!or;ES+GpbLIe3ISZa;O`(m ze=k>E`2fE_zu=$&aE|$~CC=L$AK>K%UdOxo1-LmWsw=3es4FO|C@CvwsAy`aXlU?& zcHX!kS5Gfr4_yUmu;=5Z8W`-Oi*t2%cT-kybyIUubHghuxGO3tXew*CDyqAxYAU8w*4Z48r*a1%mf@T*89z zSQAqt2Qytotb`lhCD?=1jP#DV)e>u6B`j9L1MiCuz>HE7lI4$1p-sQ zXKHI}ZL6z+mB0rC_ythEv&7Ke&{9`X4J+Y}3&MHJy8_9)fNjWaND?pc!h5?R@-4;V z-0%TdPvjR#CPWLKz@Py5nZ7t5JXV7A%g-GfNEEUb7C1I7uLA zjQoLU5D4VQ1&u0!^$&LO_6qdG2lDXf{+BnNWp-v-*uWq+Ct#vLd6JoVfUf}0aKi<- z0h#wG17~mzz~cg;KbV_fdB}YM4&afn5Al-)U4!=kPBi-A zBr^kH@uA4o4&=lIYJtyK2rL4WlMBSV0^@+o)YmN#D}%h|1a<=#V8Mp~5%B(guAW#K zAh}IgkSFjD*w>qJ0bV#4;920r$i?o4m4*jNFQ7WJFLG*MoUb3{^rSQX`V3}H3(rD2 zFVsoa6Z9EyIS*`Lum><8_UttAmn)j;QT#FUS;X$ z!6;6$BIf{N=oVkNg||7wHnSF-gAM$^{acfd!c$^cXM_KK6E9(0wF`C$1VO|F>JPk4 zJ^=V?kdudhpp#nwaE}0acdt+lc$y#|e|gXf=nuji49>E?et6(?vfxh_oQJHUoT8eX zf_z|rEBO=g0dlVX{_5b^Ku=r%-p$EBC_o1^*3zPUCO$w9OFCLh%NB|1_y8@fK(a)5 z3@9bTYBDSplWD-5J&p)-&1?k+fvAqyW1aYGrtBdP+EZj>XpexPjZVNOPQc}Yf&>4j zndaLPu9;tenjB=)^j~J10sj1>t4M%0^rrE~yZ=x3l-7-0Gh2XaoZtsuIXur@m@ma&sfiVCC?*D1mjHl3~nQwqva2gAm zLZ|%=Q>K`XVv2wuSD*h!^@pJrMocgtH+C5Q6TE{3Z~hvFyWj%hSOkvmsBl3u1Xr9G zf=lpB48bRc;1fe|`jP$r&>=WgjHDfcFT3kF|0IZya0q@GFoJLWcUa%RutHMEqsK}G z@D|lOApHo~j1L3(7R>qlz41Z##aNuHYj6P0H4LkZRngQ`Q=Ub83{~Wn^ACg3+Xv_6 zD+zwHf*-1ZGaT>9L-@=Ge~eaMU?WP%pA7z{R9-;4Z9o$&cRFH5aVmXh9(W5A#cqdBMRr4knKtAV0LV zH(o|*A_tNynjD-`>Oits4NxZ_f5b!yV4?&-U0L|Q9DnF8wqFGRaOIJb|7E0dBu&Cg zb-DhEK951P*}>P%vK?Hk2{1lUZGqtLGK`OLK4zNZeixADuo)2G+gR{_$cNPdQ$-dE z-CC+A@_cX8$nKFl|6Gu~Y+G7T9btUY&#g*&sBgv(hl1HM{U$%YPLm$^nfKj3I&V*V zNJx%$+AxtQR>Ki^H||ckun=-icwV?=G)OZiw8eIQ)`Q@H9dtrRgV)M1TTPrX0>zOT z7|av{%t-_A8WyzEAGve({KfOfvP_o=X7)Nr*>=v_|a0wPAKu zNMnZ-_oAXo`D`y8<3rZ`!Zr(XlkUvdKjbyCv%P9AF)~%RVOF2ORw9u&NE{?!dW7s8 z4put!n$83t1GbrBZOp0A$tlPR?Y@&v1+v|j7+?e*ovHy11X%L1Abr4s9t^g%4T#Y) zinG3O=Ah^)lRbtr7L^qV9>UBe#%i!!tl;D;`(ErM(5XGt&?o6AwqK4|O=MyJsT(g$ z0O$G5_V$IH9(a2+wnGxJgC97Yhd?05cKW29HcRCKip&R%e?@)z_P;7aG(&yr$gk?= zs-do;;_m8>!z*d1sj9guD7k6i@$PDBni?)@3eMmIP$EC9J8EF9tetG_EdcHmNFf}% z#M0W(-rUNJ{IVkXWzfiJiMbI zfPy1PGZJ!0-4+{2J{%VC0pG3dkXkb$1X5V_0oCc?0K5;Z!ODOp;CS)|W`e*7!5w^o!y5NBX{(b?l7&|K4uj9esBG4~@6mjiAIn{PlTC%+sfb#h-agl;L zDB(hqKv9u>ESNa?rRC_SkxR#;GX45cFoA`KrPakiqF^wsMXE7-`+*WaoGc;F!+c!MqnZCoJJY!8YSPy;?3LO3Xd!cMgW?*;|C;jwGn@E!qp zeBe4ctc_nFn4toa0h9THe(;3)fw>4=kY9i{))x=mkMu5_?emo5argGa1*xfEp+fLv z$cg=YJ$`#;;2=Tp#Qp)WH17i^3cYZ?wC4{2?m#;%j&dG23$PjI>h69R0m055$w1qD7H4?d1G6X);e4QIG0CRn4n z4oi^`tPZDW4h>>Xnt%NDBIQbe0SSEaMqj@W-^Fm+(HrX(1f`{{@kwNgF|@jx7yfuf2li@|ImA}|POpc{d6AXWul24?`S<^`@5Jg_kq=MScnLP@(+%E5_J@EJJZlVpAwTdy#* zH8->}HboHT`F*O<0X6c#1?Q!MG9Z0Y^Ll1mpnd1}Z`UK42~dnU{9)11v=bAzq>26!5Bq5gmES z8{!Zd1G#y*BjYcG8^~x0S&rHb6 zxbX;)0EW6>N5DDuUqcpISWyED5=bW@4OgheHT!iAurP=^}EH z!aMa_mLRIz!M8~Po$4)P>t!~ix4;r3xWR=YuHIlg6^J#$dzttJ%OMP)I>gM-(%jP2 z8k|jo6#l_`f%t&UUaoka-zPE1yD*2M9!`@km?d7JG>H^dW<;AvZ98Sc9syD%Yw!b0 z8jexw>@c5#-RT5YXKVz2m#og-n;>awP}KbNzG4{Nh!m}z>iFl^B(hnE!G6bOhnp47=-8`v@^sSn;9%@1KUmXaUB&!p5{ z>DOM+Cg+pD=fS-8S8_gPvBv+9?3{U>nM=>9>`CiY|3uv4fW5@30{$B{KPT9sm>({Y zVPt*~y1sza`QO`oB0-->&?ge~i3A-fLs14p6A3yP@sd|sP9*3P3Hrp6XOOr2f4by( zB0-->(8=>lX!-R-g8pw4^gpCA|3QXMl^oILQdMQQIH)u9-t<)D#xCj%eW=PJ9%krs zXG`T#GISO9|6+y?m*|h?ad3STC67avP>`otX_p)Kfo9M=ztU!yCBu7+;F@Gog$TS2 z#^+;i9Me=)c2U>Va8+2;O%pCQ^3TavqDdvIcEKqjx@#mqNrLzI&SP2SW#-mdx$Z03)@)LFW z|Nraq9(X@+e_cQrY2JS9r2uNH$n`h6eh^%Mq^k~08oGmtX|Un|2c}z*$}zYx5g!l) zR!s(lf#NGX8Tm?*z}k+^hK`;I2^W-!NwrWlaM_MdD}o8X0HhWVCtm#m;4NdM=HTu; zaO)Q+-Uq^(FOUTkmhf(}xKJ>ILVIR-vz|Fv0stpp7bB&7s$=0KEO=igbTKw;F?P#h zFk4Ih05D4p<_sLM@Qh<#UWNYhYUJewqZ6~rYU`+!8LvPxw(EdKhICdy3I(tuXu4}L zGT#ecb|b%!Omic19#qDb!-75poe4V;X~44_tTn)cTWsN+?XOPxTM7bEDDDXEhlGAc z%5^E?L0zD8LXm0IrF0B7bo8&V(Z9C*9r1sej-tb;yCO`ZvL~(Za1XBWAln_T903;> zWf=szMwE9$co?&MF#t8PT5GHy(bCY4beAkFG@}g{Xh$~k-)?bi1Z85lq7BYHczR#r zU;}^q;QTRHcr$;){LE;r= zR}fc}#@zK>Hy#mqK44b$5AO8q_CAT*#A-78yE5@YtR`42+8;rz!I%bzl<6M0_u0PM z27V;kb|h&tbNU_r$%3pO-9x7z^}5|tKr^&;yZ6Al-J+dM9e%Z2pAoh+4X>LevPj#C z;~3BWw)eO1eMl3D+tzBrf5Y$0$B{!#!@K=CvYmxy6B)1D)%E$voX|@v*pPL*d+CIZ z2Bx5_+l8Hix^8!gW#{c4ux|GYhpmlUXK$DP*~$Jhv09E1i*xSo_{U#Jm1{4zWRdf! z-4DuU^-Xf*iR;id!hGdnd*tzaTb_!%(e@sm)M=D;yRAYF4hP@=TDPk!;lrE?4L8{l zyC>4AK)r7FIsxc|vTk?T`7<{~Pn!0Lv@|$imyc){iLQH>#~n2o;kh`1usl{Lh;vFt zR7)ZsY2B{jAJ^?3sCA(c8U`M6Qu8ZH!HZ#SMmrmnmxZP|Y>L`^-j#MXn65!Dg@?Dj zs9}SlkUCxCowH~+OxOB%tRJ0FP2c`sOxI{cg_kI*s8TO)A`_}fblvgW300a?qfP)q zt#MDu6M!_$IT=YJw&Ed@<5G-vM)um69QTdWiCAc~#x2RCIj+usGsmS7l`6-jb%*;d zj}U0VTM3}1Po?ump_Sy?(g2}FkawDv!5CUs546s9IbEN3nA`67-M|TG0ZXE86e!bl z45~Lj1s$;}pOY0rE1AxlsiAeemNB%r9K#;@h2bB2IxLas#2WDrT@Tx^@KfOKy6SmW z8xCtD;iJ9TBu{ubb3(g# zJVcH(ph++B4ywD%{6)+q2X?69c^N*>Q4Kl!oMG25jW9HwgS+? z3^8%D$qbEeG(w|73tkL-X?@H>3pO{iU`18|EeC4A{pL+wW1tnb)_xa^zmDh9GblC2 zD{+4nT2M5DiZ=+f4$^f8^U&f1N{-3V~%ZNQdQQB#u(3=KXq|#V~bp;=}xgLN_sH%MwQF^ZIrgNkvIbPzYEObwi~P z>c9tf%&eQ!IH6~<(p;yk;FU2h_Y5>Yc&iY}v<}gEGc~j>#V~}{^Ipd-rq-Kq#Z5vQ zJEO&11bAeo+l)+V+WO+9_i>hR;sY_kwc_U|Rme!(!R(sO-W5M>5tb#1cI;PC>Dmjqo_R>hEl+!*pYYGl0Bl{t)YBdRa94}0?$_6@AJQMptZozGkjd^+ae6GD zM}pS=`(y8rR4rc0oY1lRFbK3#>4c63y-0x;>=aaJEw$`?+zFudq-x6inJGIyABZgJ zIXeH#$&!6u4{P~@#q2qkl@|7A&vYwXHhEH~cF80^L+he#%lY(@q|9y7Oo~e6`Ln(= zg4Q>z5OXSED&!HnAEr}*8d`7ifj%hEGCL1tp186+@s!FWqz_2%cGW9}AIA-BCBqw{9!p#HiRJ*Yo4(_4Ki zyqR+`Z~Z_1Tbu`9h;30VT#&FyK#DyvLGWsUNY%Ec4*m?{*(A}!_c49!+|f_4cztpv zaz8Hh?Felq;;6rx#+w)l9kzo53F^n`gpLLlp#=2@;tYbix#jE1R)@1n`OgG-#eKTo zxVy__(0YSixg8wp}?pO}ZMei;HU>nhuq5|A_FMcHY_kse$iq zRwaE(Q178DpPgUf$(#zDc3iBYK-@Gs6=40m48v|&gCVrGR?wUaHGsiBJkM)Jx`CRe zIcjKz&2^7hA_;;W_$avl=Cb~n+XSpXpWOk6#$U{m&QL-#CVIR=GsBJ+)=5NSZ91Km z{$;F%S|{`#Mq(|+3gS}_j)}FDr5Mz)7W;Q&Efm#p^-*SHEo^=%mcJ;TOOOs{`kn(yjBC-x!9wP6H_t3lxQ3Cv9<(sjQTNIWFVJ8 z=griy_Dc+UtbJmpw`G55bLgDTr_ahtE6-+Zj%^4l6D`$w{WHew{nn^CvqlaC57=}p z-xFHEh%IW zYyYu)UHu)z+B-Kt_v#o8J`J_tBZx}uAkHBO6STS$yrMaH{A#S*_dL*hvUIRYhIf~M z*@_W~hkf~LS*9!SY|<4x)17;p5^G1NDg;Pfv5q61ITd6#Z$#{Vnob3>-Io}EM~rmA zbD|8OjWf?46ir)Nx4md&^>sV-h1uzDjyhXTb}pGOZx>}(uV-5|ay}fJ4yP(W)#v^n z2hhEJI3%y8UkmE z^NY;#X9~{U`0;XnRBwI(M4h^x9hs^)OXtnhRi95TjH^DCj^PhW&FvmF21^%qN*K)u zNc<|@TW^-W8*7KHdAEwSzeV8PJN0Y{qAi%O_4otf{|xZ6$vGP;0;6j9U&?XLVH`@So_G zWtYh3m(dPSe?vULHlm+6?X*tzi-Gjg=4<*RL}CgNBlNOL7-zTViz7TY?Y7OcjwX~c z?4n$GEkuPZbQMgT!4e4AMW{6(3(KpG%>ZFp8B@O$&yIb{v#i~;`gWE4Bhi-Q&1Tc~ z9%0|mAuo94NihqT5c_+(HixKZ(NV$SBV3Ex;`8#gqi>$zvI=B`usLkL%&DNTS_?tg zIXV@n5LV2R4(x-1F#Sxao1$svd);T%eKOfk^zbe^KFIQ3fHSYCV+QuXgOQT?KUD2n zbJjzIbu)pm*1b&BfS~K7a=Rc1a{zL|{U!U?j6s<3nOk8n(I}TXmq|ew`?w_?Tcxvx^pHUEo8Yy9On^)CHq@>t*s@}UXk&-e@piX1v zk6RjpS_6Wj20FDOw1vYr#E2$MqqI#|s>r+xX*<*7c@`h|jlOAo6UqDjtvRnA-|F${ zVeJHM(anTGr7dcFdf*z;1y2$O6>>qj1{!izRZv5MtS zCWt*>l~c4i@nl}~kYwVqw5$5d>ki(Te-XR)OH{+tk-_R?#(8XQ;d*QOP7s?4t9hp; zh$mr>dPa*Qv+Z!=!VDtppvLbImR$Gl&%qPwS1{cdiT@Yr^os`8BO`1TrjQZBlC#gI ze>~E5xZwJ+oV#2%62H8OK7C-Hj7C9V;VU-lcf7LC>jhs$&WP?a=AUBl_Tm1lz+$%N#>3Se>=@%E;k*^qr4qztIfN%HY|Gbt93gI=J+gXU zf6X>}gqe}EFxzA6nd^n6a()QHF4L(%jW9t2&?Qs|Qzbv>QTMD=kLdA#LR#b+ze&ID z`(an=wEaN*9HD_*{7nVezAap9&53y<>`cImbis&jHlq<54noHaM4{cAY*!)(TLBut z{dp&B#vrU@@$n51VF&QC=P3wV8Z%x4QB;IM(egH|VisYr`33$-9I`;GY`X4X9$~0V zf~P)OFUkp*%~6bYC3F@i{jxbqX^r|WrF=k1sxNesEuS{SLnQ%8exMOj1iXS81c|g(c)pF5ch$T3uC1A>^VAD^o?ST zb3=QZa9-Dt-hMl&V?5KING>)=s*cYTm^GAdtl<1Ei7lzUHM%#)x(0JRGVtPbeP`=y zvnW{@Kv>xy5cUBZ#+-iqt3#0Bd6iB-a`1#%*eU@Rh%goc^cgJppY(L%4MK00=-u0N z;~SsY+z*x(uf@(AMY|1Lb#UP?@|+U;L@~ZVA}%uBXMm7u`#suXQD`Qvc477QNpQg! zd^+*%xI6X@w9_(#=oW z3=}F{I^yrnJa%N3`EHZcDz-)XB{!?}R}RN3{wzN&-`Vl1n|1aS?&GdcLx_urF(ds` z0^4bz1)ff92ETs@Q0?l0-L{6vO}P@61)lbU8h)%ftp9B3l2i=ukVNYSo&A}o6l&;)T5Dm5D2Wzo=!eZUzw;xZe=6B%j^dL?LqB$X;ATBeFsrgP z{moj&p}%WT^-}bXHvRi?7q5uTE1L-Yz?5hk1#L9W&k0(hdUMPr0c5m6@n-gI{bMp< zw9&@+P2{0^^DE#(F5+wmrEb!BGZm#CPc~vqsoRc`53LuNJ#;VU9G)jf)J3s;?| zs4;D#b>QCD-`tBc)un`{D;Vlm6uAze)Ga#ws3`@X-;PG9iY0mcY!Z2^iLfoa@H}TR76k`b1GcOU56|t zxJ#$Pf4Z0e`sj=My(p`pP+0<-cNX*`>%3rc3-=T1#;!Kb@4mPivZO!V<2r>UL*us2 zi=lTeopK#v$vrwN{Y#cWt-asdp|AusWWnaIg4Q8LJBrcfO|oJtWXb&9q8$`<<_uS) zXg7`AzsJ~|eiiNZcTX+hFu-8CPqU7I%Y(sYaN&{d^RvX`^^JZ$6h}1G>Nv30ziTo_XqJ|s&_)ZmZ%_oNhr5+W}5m5Eoxy>f? zfNJ=bb3JM^W`;_w#~!@EzTlPsms7H~b95Q2>d=g(&aNH>O{caB;RlmET7KT8%6MMO z{+=alqDTK6)1ZDDZS#?^oI|HyG_W3}XeXb+plG)_+3!~ByV)-@CDuE~e&%{BAYEpB z7BkXGjIOQA9+{TVr*lAZ)fIwj4CZ>z0_(R2Sy(q4_dVx2f|+{=7VSd8ypvUx@E5z6 zt|&!2qqb(|RQMq!iKy@ZT?G?ojKE{qMW|%4w%o^Gy+Ud#VR_`?i0#}Z`wY8q4PpxM z-b(+NWL^!ZlVNFE^ew3RmOU${9(Vc2&iLxer8$M0erQjb@8z6Q9B!`Ld0d;GEav3= zXP{M|ITf;%B@v?M(y2fu`VyG`7+^t{P!;Vc-_D|XTKb^qQOo`pou6wnQv}vT+s)sf zr(rPk!1af2R0<2>raMQthPclRViZ_9QIQuZOaSNk&GxYJ5RK5NOoFbnmX*Y$Rp4ZB z|D8|!n6<|#!9+*MB!kG0cSe~M`&Wy0P_%5eeiYg=yyz1xdl7`?(RBy&2;&4c88eKA zl4uWVB21zfP0L?-43p3%lE&}0P1MW{iaJeM3x&3*nHy~G8^07m*mSZxDDE;IMOgme zjw%~~Fha-Cun&w8_Bi67#Kafv`XQ|E3YM+QnLrqW%anP9y#XB)WIY8*hw|yXnHpgW zV;CbW&5>Z-E~s175Y$_mtQoUp^K5UWS-4~iv5t2It@quH=CrN(bZPihhx9z>)LR3s z^52D)YIxER#Qy?1{i1>OC1Sn^>OGX&+YK(jrs8TxE-Xi_Xo%4mm3< zJIywJZBDG%LtHJ)liadU-9O@?eu-08z525}KMKMc0K(epP!RU=Q#^AjT#;Ces8EQm z0)jBuMW_g~u?!IV3=rn88@R&^fAzJSMVV@5QM|tJCJ7hEhf@tGLRYEFytAQGDztvrKOhxbV?HBftcn=2a z9#DvPW!#9z@S+n7gK8R&>xt=tI09?^L-HNv#S86zykkx*31qFLmw(x&mnz{rm6ESF&`C#N@0 zs8r2PeXYHDE_@Fq>Xn_P2MEXJuv|^$l;*0wj9qk+@L7*#Rv*um>NnBb?^M?In(f>- zTG1mROzU?D!^+mu#39tH73w#&*c6HX#dP{b1Jh9uW>w1oVWEQ7#rrDe_@2XOG<;>d zmX6OE2_A~-Os;X-Av$by_%e@upVZE5S&s1QUBtm%#47oIGq!Z65vBXR{SaYcaP=QR z*jhfcEG&8|6Gcg=Lj2SvhzgI-vfksF7V$n&v>Bxr>$~>*P4$Z z1@c2!PPMD8$(S=q+|IA$*z^+c16BUYlCM4&6TQoa>r;q63T)-W2e1Duoy-VfzTJw< z^#Wnp5(HsS=v1Ian3FT;5)_2VXJqz>9<~y25as_`GRw14BE7NRa4>-D*~ac#n^|iL zE*9}!Y0_yAB__fw?7<(iFzlNjG(w{y4BGv{Io}EBR6(7nC}L9T{^Wx`8wO@=&{e+MZZd(fP;qDe(`S@ju%Bw;V^~ z{G7lA7%^X8I496DK%QEMT+t>oto8JOaxsuwDLgF(zZ>C0=c_Cwj zZFh_=y4n%>YiV_qa=x zCpZPQ5`!uy#XXcXg@wc&B!uDY<`taQ`gz@Pt2i0$k2X+Ldn+=Rj=HXUM1 zSLxkvg*pk7HmspX7-b#B!dTduKv*8vB^seonFKEl{#~<>v}!Go z3+`Vjo;D_}G7sNx0hx3pRqi2$Ni_-M#UxbPLeZ3-&q7HU8D11csgVf6Uea~PzeE_6 z#3SZ93c@&n3CBcA*gQ|52#J&w8}Es`#8jko9Jfe`N?R!EU9%#Dwlm1?aJl`#C~XZ= z9$2`t1KJ*&RW*k(ZEqe)-OAF{u74~>+n4X#r3u;sQ=)B@(FDzPit5efpkpHYx)Fqx z(s?sA!dAI3Mp%?%jInRCVj=!*CGRF1mraqm5mD3qI_j!V6kWPk+?2pJDPbB{C$45e z5B4+%QTOYRBSvnke)*fIIO4Z%ziT2Vb0^2Hv*OvLAhRWmx{xS)5M*IrY|s>huiXk; z!<>F8s&kPP<`tcO(cmN$g!zgyK-d;R`Io!Pj`Q6g51oHY|l}@En%kkNEc_w|K`V&vO{;VH@{V&wQFKkG{9(?HJU^DG=}aBL8zg;pjljneN_B?aDoOm_^uw*;&ll zeM^=ovNWWOPUzn(4MD{ryv!a(VGtay13JR}CwhZpQnbK9gJlqh{kQ!eQE;g6cX0?s zJA6umSscRV(`u>^9KNNq67x9Z1U4DNH+bsh&qxoVqZrM9YXnn#8^0U0Q0u2q)KjSk zQOJ#oL)hHy+d%|}Gs*7oJ|k%qhYCX*J}+fAz+ife_Np+(VUWZYQI4i|{T1i36rb~&D1#b0%4BP{ak*IF-2DIWef2)o`&L9#l-Gjo_eZD z&RyY|mEh2-dqv%u!xjVkX0rM(5+h_?isxKCBdofD*YJW0=f~~U4W)67kv9$vT-h|D zzP)$FT@!EE@DkW`Juy9d*_z%}E86EgQp6bU#`JV}cjt*+#pr}$uxZpdltr6Y z{UiDmGo#`LwC1B(7h? z>cDZek$hrLGJoyE2iAH${kO|ANgTu2BH{4cypFh*N@zELyJzKTGf%vq{U7(m#&1D7IdXKM%*A0G9wk&Pyg7o>TcSF z9V~3myI8G@^XQ6_0AY;3wc>6x8%-Pn5q#{Z5bD?HWP!y0k97J)g99iC+vCCrVafaz z39pY|zS?d#Y`S5oZSsP0_4Df0Pfsc};BT;9R^&T-aX~>+f_u!ou%4^g6}O1ZT1GwJ zCZ`F=Uf};D!c(Uixy7WKj7-+oIBSxN4xA*&~ zwAr7{srW^-7VDM~v6P3mh+=E?_w6KdzE#`W@g1x4wCl&JIjX0UK3&RJ*~IeCukWw` z2)haI?jkP@@inbsP6f3L3j|@G=~SRb*zp?BB`64!Ov@@4O<6v4@MnL^N72sLNyfR! z`a23o*tYkrN)#R3b~1rQU%SiZFtZ2?D85c3G%6RM>wN2v#CjYl5Lor5&ojxt?p^%SL;;S%jws?Z#$gJ``AEoqu~m{ zxb2*iUOCF{sL3eYzT)-Q(#(V1{nS|)xFz{_2)i-JM-zuo5eD^JIaLlpSPh+i(ZFC7 zgdGP*qdpiaJej{L_SFf#yHod>JlGlWFkEn9m&5b5>dEW!w|Yc(mv9xxf4ePKy^9#h zHCcSl;?Qh-x?aiBUJm7t#)$x7hvB^1Pf(OZ&BAs!eP>REUQ3b+U(i)R5C*#l6=5qZ zl~%q02vaX!u2^`Tn=k3nOew{bgs9nFn(Tz@pF9eN!<(knV%G9Rmk)DK&zfQD@4H1$ z{Ayp$ObIoGef>F~`G2&eQ5Pkt5w@^)h&dI$tdTHNq|tK$oB(OyWdVk5r0n zW6=u}-z4W^fzLjn`Z;asKT>owb+FEa9I+cc%^rET4-?biyxO0$uuR)4G(w{?3A&Ey z!Ziq!oPk_$zv|11F-&@=wsi$$lI{I_xfCYNjQp$fYEZOKTi2n`mf=NV$WKC89bI=Y zk1*6M3`(MQ>@c%jfX$n?UqHA3zqS|dKPf*uTAk~V-yU4fZh*mT%I-hGm<#IXPem>w zw(A>7_P&gh-jhd(f8dP&(e}UPI2z~Y1ST3Y6oDP#UwsuBia3J?a6eb*z?h+kZu;YG z(D|vChIo$KgeR&uzXzhFa#SG*tEcm3YJ{cKGDeuSW1P{;yJGoYr)S+=9u`y3!@FT# z-GyTHogpI%iVbT(5%G$m=j2{zc~;CF%>mb=O>Jo}=BF((4gXjc)UAOgvQy>Q_}e!g3IF!&f@}qQPqvgk8*FfUt<9_J){JQL8&@ z*j*gwmDSvPFBhxY^9}=$8&%%>8mFs*B(}!?((&N(vLM& zy-ySc#g*g3crinjjdi@%skd@wnRd>*-zV1InN8dpJ|dZW5;f2D4>2W)kD4gDm;zXW09tO@B{= z&f~L1uO?^hnPXhx-gC3_>)&QdD#jIl zV9nxKrhZ5}HF&$_xPXWTjj&GaU5WQtl56kEkS=|eKzq(uKdD&)kTaDHXO~w z0!`>;ft&P^EUbl21!{zq3&iRpOG8@eg#OLa5Y(b1JXa)tF0$Ak zL(}D`^n}gLtaFjY{w}~IaNp+HoiWpJ4mY-~f%KF&9J)rKXQI-GUh^9jBIQ=)CCd_x_lKpzs#--9wvLG|X3 zpn2Z5Z%9VhPUp?kIJ_;+7>DYP@rAx8XXLxxIGh~4udt`Xq$Xza(W?^9kw;s%oxlcu z!MI>P%yQ>elFxtG9x`&WdUfjDIU^N626rYm-LE{Sp0V7$u{F_p5=G zlC8hNVIb!xn!v(PzjHU#5tek&=@$(wO~K*wS_U|bN-Av&e<9jd9JOrjqqbbh@K1}a z9Pht*x<=1_Xphc|nLosJWQW_oekP{aPTr~_`>Fb4SD|fgzr9mEK_1}n2rNp1jBd?K zG#sku4Kk-f^>>mAo#-kcID}nuwOxPbHs6_AN?IN zvzkdb{Bu$Ao5?*Ip;4IxFAZhfdxS}@KrXn?CHQ&_lPE<=>Zc<5`;HfrP-zQAyL93` z%B^(_FAA<1*$BdZ&~?YZL>QFh&GH=x!YD@5`WQO~VOcdDF%ab8TAuk7kaLe4!cZe6 z6t(rjF_cJ&N<7$H^3HLDc%o!?u+Ta*N<3`Ef-Mptqa*NkAKu59c#;yv*qKk;^kuYi zRq8jto**7DCEA8cJXCM~1Ug3ONhN}?ZaQzKMp$tnV}!{&Za1<@7R;*;N*SJ)UL>en zJNTeFT`J2W?7O{0EvLb*Zj+`H81W9}y7cD-BQI7C987$q>U{A%YpufEdsz{x>9Lat zXP5P;dz2ld$>?OkQ$vCnXLRx3nOIi`_1nP9k07jvPQPeiIts!*#xOt_L7?>KmJ%_m zf>T8v>xGq1c)Vtv$sS^3X3C=M@{h6b#VJeu+BMsCkL&z2$8_4T-Ar3GKX(zH{iVTF zkLo`nY*`=^NhwrelP*bxpXe$e2!maOim=s|Q=^Ii!nlI=u&-WOvh47(*{K?Cb&9Wb z)3mMiyu zy}6#;S9bNJFCZc8&$nDZ>@TMg8kGys?gAW7QMiCgJlK3d|0zN|H=qXGSJf^WL%fVj z?Vd1rC&=m^p#*OQxA7t#!|qtN`v8KlKDzE;9$~0lfLi-_#iDS56W9bAj^^RXu=$5; zr1kt1qouG$jUgUo`i7c#8skPhRD?lM2Ok_p5GF?M-{npRMiGW7wSJt>WdNq2{%Ol# zT$G%r_io7%p6_{)`%GfB=X}wpglcfc|7iQ)avaTdit5duLC35de2gHhpU#`95mw>C z7-7SgSkrnuw6 zLT?j%+(%+wZPDVKdaq`=ieZFL{6KZ{+|pCBcd4^5-rpna{4P!A^h;%1fFNv;PQPg2 zGXP;o{D=CX#{V@IU5N!NB|C{u$%FHaZn|yUGL$^9NNaD@oG?xH&Kb4V16HrF_Lx0y zhUHfDY-SrgQc6tIX)65j*7~tUl*F=ddF@WsC`$bQszSRoP`GX_b1FouETv$HR0Ssy%ST0y`C2>eou=3 ze}3?xApSm$(5Uer`c)>20?JGeYW#=I&w%Fedphvo2k!4FTR3J8t62JS5RCtpo38Am z#D5jsUyc8WXqD@c_&-e79se@^!&8fttzY*L=v^w0{y z2Xz+x(p^ir?g+-tbKb?`)&EBG-LdH!YP{!?)OY58ST*ExvQwBKbTZxX8B1~v*Zn?8 zjoc8~Ve6Q=Ts8q^R8_c8JC1{Tp92}+uN;1riAQcg{dQerLE=A=PQPeiI!X#NP{<&K zi4n*i33{5=R@z!U6uD9F`0#Vtw`<=1(>}$jd$@#nA!+33VCR>pR^Pmv$&+g?$_Fag zwLkbw94wvDwh`W-1*&ki)?UE7?ANh#P1MS`pd!dU23phDQ=$p%0- z6ogGXvQSlO|H=(t%SOVgThfwuM`XASuTtqTOwxN({e4QF=wO$miNJ#+B!ubzaZ)Pr zBy9?V$|NYX2QTT78}2|ZxX-a&U<{Ke-x(><#hOeBuWy~ki%F=og`%1H%tfIsH1?Rp z0XEMjR3ZptrR$D=i7ql^)DW$}s4S8ST{j%#mxar8y~1j2wR(KhB0Rt-Algu)22 zXp5cBo5{h&7^^CQpQ>i}OYrkQ`?tHD9t!6j) z?#>BmznPh9Jnx9jz@qz2mz;T?5EfqD=C_pZyr(7?{6Z?H8V(iBJ-6Rh(SFs(P^!zH zgE|!Y+#+p+OB{4UQwQ~)3kMnZ5l$Vy&KiL}s(cCtG=RbTtSSs03c=5trgPC%~H-;B;gbNFKc&z1gvhqpp{YjP`8%raa?{j4Q{U zPDEWsml0~z^pG~A10$#uQqmHnm4b&pDb=X(XQq0HWp#e5e|o>N}H*2C2| z+t^V%G5yy!Pyhh1kh|6O&{OS}J4ZBi=N|c}rB;pq z@a$&o%*yRA^Uiv*w;h^xr}EZ?$PbExlJmYktUkCYj|4!w`*ErE4YUB%nEUe2ffx#1 zBS=~;IGIjpDgYv#f|}K=w|J9%7XYwQ+~$i5&y}k#v&0nioec$cMNcY}*fDqNl0Mft zd}pti&DCKU>3HZZs;RJZmLcXXyNbVnS(2GcjH4#f z%&8D4qr(~s4w^!zf*W{^{Edu{7ZSkE*l%Q1bG7vnN|s=I{8Q&VbsEn`acI_@m+wHba; z7vwxwutOp{W)8a&`|1G8+RNcDR&yMRdeht8-oQHRj&6Sb4@E*FC+pxHzFwA(yS5*n z*6^zGWFm`)62>~Q?`vbNRX>GN=D{iDst0p_w>&&ae;Q33VyNFqt5s|;TJqEBM~;?A znzkSVQVK(W*2jYX1Dm~`EcJ9W=fX6+eC^Cyb9RX6lTt4Kw41B0&zAe7>5KRh$&n|X zE{EssXeMeI4iTb=QN%tDy9e&7v?aIa5pS83((?E!Hh3SF0G*W7Sc1KPiX|tDH@9yC zES2)fez-r_|F+u=Go58->N)MG`m~SQb-gXU`Ds-OPI1N~=1=?Ut`o5xymw=qDkohz zIy|gDA{1G@d9ds@f~9@}2Ighd_u)SKRLqwZ0IEh;Jd!Guuw-9`Y#HW z!vQRv$m1V2wDH#-!Ti*aVR=!rV5yEssc@5qD z{e9BokG#N}aDT_%v5!CMIJ;&u{fy6naT`CP5)_IiwD=4PL7{a~HA@bEfM97Vo&A}| z5^6|+k~ljhA|Zu^#uHG{0GlgbPeaf!pKRll>La6Qcs2d>JAZ%%!k#7Z-i*))r9mMJkJtnnfF)5kR5Wmcrl>A*UHlX>yr4M$+wO!h$rEMfj{3&i8{;;-K=tNY z;6sC31&~4RbUJT#1G<4hE)upijZnk(DlQiH%dg@K>Pj!8XWjDmhtZ>|H)vA4b>*~CmbZ>G*B zRKyu)6V;Bp4f!;FJaC=9wC|b_Ti=7gfiJSG>r(H|5>-0$AV_n5T91Acf0=;Mx|1AP zBm1iRFJPQ)aR~=Gm&*w>zccU7TYo)#@FBKICdTnF9MQs>7tAI=&FgndlP|pUqlrTd z^;4ZgS`<5rPCx3J*V0<_nwN;7!Kw`5)`3sGA2!>cx@)6*Z*~}m_;*}+Q9sV3zQgcE zcHCT<>78QtVyl{uvBu5#Zoze7NA(c+7L6K?Anmw2E9fSDaLr6Srw(=s+ZxhhWLS)& z-l!=BI|Vhlz*$^-e0}Aa*SEfAm*mw9 zd{{n3GB3YPetO#jvDofxvG?Bk8%r@-0WA7yq~u~OrDT+tcv>t}p>`|D?z8Dspq}(u zz&)4Eig6NP`ddL1zHAP ze-&C#w0*}5kX&O9U3V}KE!2PmPrXI*B}&$V$~f5ENTd|08&ZsBW%hJT-H^gK>IW(P zWyT>!>#k>qUsVO22k!6QR?QB5#|P(+NgDB7sSFs0SsQUYk1^x89+!uI1B`q7cK^$t z_7jW))NZR6@k`VI&Y?imT@6tXjMAy&Tms_TD=AI!lUv!uF zAp@3mWoD;C{Z1O;@TM=P^-A~ry!p!H`4@htxD9iJxIHJ$B|6CtUr6WZHxN}OtYK%f z<(}T2U>gieU}R{e+Wi4qe4m+s7SylkoHR0HE=s2#HMAUK(4i%2Xb`q@$%E^kHt}Su^66I_wDlzpB~Vg$(qo|9W6Ert<)8C6E(aQ zOhkMrbWE)@l3Iz;2~7R?r|!@rC=4j}K9-Ufzn#+*|D!jP5wr?M5^3T;6cE*~ z-VaG5lm7GQRNw&zlwsH{YcPb?)(V<)!SkS=%V6zS1PUBP zZ}KP{K8+h3P_YL^d-($`S7T@;_g$pN3y9NM>0h!0N}?A;x;31Mx)p5haUc~*LZrzy zzG%L0Gzqb>*|2XR4A3IjnCFZGbdnK=y!g+?JejLm-se@nPAs(orbOFN12ih+VaK%f z97hsait~R9BaBI8(dt%~<5sjq_2#cY)O#zQAS{uf^JZ$6*tsxf$z#Vo`7g2Gb6jp{ zzI~%(V%k@}>2&$4r7ek#1!A8~9Rs!|aPSYzJ??B~GjEpfkWqApSLW4`0@e2%oT~8S~4E2k8+`ux{9GnAO&Kl9od5cEXU{l*SgNmw6+J#8B{ffmaM4FEG2ampK)lZ6z&imPA*< z#2F*-1a=WBSzImlRbN_p=IV)@llj}8>#d8}#CC*xdwUG?mnq+%&EW=OnT<2 z6rBpx=}>4oF#p(eD3*9|(Lt#_YmZ1NHF2vvxVpyXlhhhxy}T)X0vv7W_nyaGzrc2k zU13)rsEsm}4qe~>nnq|;F2GCU;HP)Y*6?8SJ|)uE;rakI;Qr2xGKR#XuHmJ6|J8H| zik9hj0ZE6X>AK@zA`G57kHs0K{(@RZgUz$PZ$c0zLpEAX%dd57Nw+3dFN1ZolaG)8 z24P*vhghfAHRc@&&KD82m_Qh?B2y|G$f{1|X^z zOBM1!%>{JcOdaAw3K@j>9U#Oz=V$nSkqq#0=G)mQyy|no{Sw`u{#DXLvV|fDy$z@+Q%l|RNXRpA~gc>N|n9(v+Ve)Fy_aiMtR{;s} zuHZe?5btKOukiv1@rSb3=R_2m?MN)Hk9g*`{M|((d3N8s8-7gh-dWe;mX<42aBCUY z!OXhzVrkWe#;-UGEXA`rl8SPd2=)8=(S~@3UUTv{GlVQOLeO=@8CiqNG<@x~XZ0rx#|dSi+=i(H$Kq-Jx+ zx&xGomyOR~4e?Mk?pP-z#4o1n4(3BVCnIMykjp|s1H}sKw`Gh84IW~_aWJ)`o|RoX zZlM9SI0vn8sXdEX8o}oEQaK2X7Lfaw?Zx%cgvvQyz8=yDv*Cwh9pi+G?`WvoKvQGh zvr7*fBTOb}1T2ZVq0$KY>X`W02s-Am3u)1pES)z~XDI<0jI)&6j)?}o+;#VBRphR@ zc$gf&VcZltQlqlgz1S^c>agjqN`IxARo@BemiG+hh5P&D8x1_!=NZ&Bu;p(3s?mN_ zq1yIi{K}M0X}=-|&0kqcD5!XW+bKAd<2+309==7sT*mf5N2-_*3 zAh$Lz<8qBvdhR>x{H@X3l;3(ZKEOJ&OpD<#@HS*QAzdI(;QD}(>*igXr0VLF^8$0n zgWceXfMO{?SOPgqS<6eisDRXMp}U#LQeYQFH<45TPX?#isOw*_i%=1Uw+QGx2M`v& z|Htjecm2Ua*$`@k9i;klq0$dgG&Fz$qI%ht0VAaDs6eLzHNsMAz|klO8{B_UNNU&m zPX3y#DMwA-*GMc4dz~xN+vnNAJv&POTT!k9?^5{^6-UyS3mN_qVV8GZpb;9CNzm?> z37!a(;8(H3eHmBXF-*!A&x?f7c=8pMgA^uZja%A*N?RzJ<-$M|+A{18K||64F-5xW zU>;$pX$O=1ebEQAvXo=Fpd z9#|4}Lk;w(z=j<&<+>>fuu+F1usP4m<;YNk;`}uqBV=SoR#25jD6)!yHvG0sEi3q~^kaczJMK<3vR0bZHAmV?KqNVTg&y}0 zwu*C3>$W}E6UVziLA?Yc6RWrKW><#T)B3eUP%`<{PQ;H15y0J83IBi&rD26ON zNL|TEd$UbfF%x$PL7@qG%Mc8!&#Af9oQyc!u7?tyPW34tEcTP+5S>OAj);` zwjGsx^V6QH+QcM=n^>`m+lx${L`uW{yzt<1rUs4Bs9=D0@2?j@0R!|aRNr#jIu}76 zy!8hjekD|B4Du**OVkSwlEw{r(AcP=p1?du(l9l;?qHr_sId=f{Y*+4`O;R371B+_ zIVS-*TyTMSCvPubxWWJ}4I94-4Qf<}qDp*`M!t@giFqy9+%ZKB`Ht5`CD;CjVVM>4C(prD^B%oC*D!4Q0$c0TgULNPA*SqX|A)OZkE`kX{{L;BC!$g+ zm6D-CrFlddiwtQ-GHa61JP^_(b7??@Mk+(oZ4eDoNkvH`il{V?_jmTG+xvF!zMSvx z^WZP%pZEK`ZRfpSYpuJUXYI4kK0|(aqR-b|k8O`N;`l5;kjY{-&{~uqH6ZWvyt1cI ze#v^Pb#wAm%a|T#iP37s<Ga9L-RZz*jR^uq%@N8$-jF z!5vKX>5b!>D8tmxP%!MF^m=SYbN5Vsr(BuLXi|oO7(p4fe}f~_9fV;g4yh-bKBQx2 ze8IGqYt;00e$MlT8$CB$U4PyRs(+pAlIwXjn~qP3@s#Hh;qhZ<`oFE}Wc7HwE^Ej7 zo@CWYh9NJEPBDzQ4bCxVR%{abc;r!rsgOw^W!Mu@Bo84B>%X+rNygubX`J&C4_C=m zn`KYb^*x6*=vwGm4ay5uvq~-vOjLd89R>{RtNDE&7WWau;tBM(AtaA3o=_z-nia8= z%Hjzq57BE4aA4&j7#^(1iSp1M`CozaDm}_~QHj%%q-!mf9{#J-lEAbA?Q)cdYGhu* z&O<0}nl_9MIv;_pi*N5n%E#T5Ar@gtQJKX|mHFiaS#S@;)GHZX)0-kJWIXKq; z1s|zQ%U}I(@dV;X_yL6s^u7iJMgX@C?dgcwtgpP1qNju1*PQ&jJ>6Ht)H>(+Xiuk3 zmd&I*jQ0Ko4`XU1HY}O=7+{(tsMW;3_|4X^+MS6O36VD)()X#c_6ZA0TBz|{(iI!y z>}E~AyLffb8yft|@sVq`gkcf+XJcP&XxH3jS1*@R8%s`NEM%W&+ettQ+Xv9o&983U z;J6D*Pxn|u_xE)f$hERCN9(c%nOssHKGuQraD`^>@Ye1h;qj^tJZZdE%XRlkzM^{? zSTFQOP)Pr99Zro7cXu@%hm!4`yc|B7`VXA1^_8#w$HS-OJ>3D}GHgZ<2b-fkohF&l zq&x&Mg7Wad1|zO$goifU_arV%U|(@#N&EcD@^pja-=kHxueLnT&@+^_B$+!?N2kS- zeNFwB!X6`KUwTa$j*B9lPM!xy>8 zZx6!5-jIZJne$e?KA&!cnsOYwzo$!z!Tq5A@NLJ^V~tFsZd=URKH4dK1fSc(^l0sL z+DxON3Vihx1hJoR9*O^gJ1W{K;>Y0P(jmtnxPwgq$y%MdfQ z$-;r1VbIzKF1?`82(5iYFSHwM##H;l|5)vV;ue^CCt@2`+=AhG0*6rC0>^>#pilqp zY?{J}iIVKTCPzF8J)pP+xk@oZ@eK(?0&OeW z)9H|9GbzI!iv5CN(KQl>RmU0v1Y`NMnC34%vOOhLU|(>{=@m4#-WmOydKzQ(-f;6Z z$>O@k*=B!OQf-$w;eF>P-NEv=>A9jOa5v1yY8(f=lVe9bKJek{`Du$1{y!LY_X!3Z z0J({sA}9`Y$>dJqKo{(SSJ*=3Nn1cr_u8$Q;y=3k-mvYm8TfY0kljo`Fw<^!R>^x8 z?I-6MOGS$Dn#T@~7cZvsh~ntNXCBv*9qnB-TKcAOB9d@z@=05>>G!?Hl#v^6ow;W; zFc2CsVIfge!fLn#bg>AC7?fxSHyDXTB1E&$ihR&`zu)RxK(Tme0vET`n1=xOs>R2m zdW<4R!@EX?vt@4gpZHQ_MOcxYtIS9Cv$+QzSbo?$fbIn?iTTbt1(?Z8MN;{2|66bt-_TY0TLk{3=yRQMl5 z6coO|w7i&R$kIis85M}jLA$caCl83cspEen8n8rO(jO%n(P`N)C_gtueBWulpyc%C z+vol+(SWIk1TJDF8W>)AJQ^jM4B{+sUKJNM-9Q>HVu+3fL|f`3ro*L0^e6d0HKbhw zla_wLOLgw{oWt|^-|BmE2Th%0Ct4>Gh>dRDDADxDiVG>xa`S&dw8$EX_zgo1?DY|| zHGhc9%rgEKBsE>7_G>W`wCL`#do7;R*D|3*>&cS&ZU9pG+<((W@Hi8muIm^izm6fW5y2q0e*1;v9L<13n67A3iBdIXsg&~`l znmu@SSL5<{uk^_F@Pqnw1*T_9iwAfWx|2mpFN>(uXu52ixPMOCSnlz4n#diDNe2}C zn?7vzK9suzL|J7SOaBSIgW1Zf^ClxXY8B#;vAGe~C$(Y^;S zng;)pTEopw zlxQ2s!hxM=(1}BE=}AJc_FP(db;j6UG)8mext`Lur`W%-M@L`b4S6>Pk8TH z6PYh-zx=EF;D-S@pmiVGk%GX)G{~X+rk#=ejL3BsNm_ z)wIs}G@e}C7$y|!KHeZ%9tm4=v z6z2DH1XYEMkr8bhN3D9FCnSU<|7k}YhL4c~x$hY#_dMN1CYO|GpTwvTEhwmB^Twxl z0|(d1q~92%mG!MwdSTz5srU1C;wSB#-8#b#T(6I&i22}d1ajVBR@rWt`Cjv<0_(F6 zi{~03M7xgkXof!&ksb}U8dBXB!%{*ZVXe-jgw1dXDA7Q~phP>oVWsR1glKE|q92II zy)5N6Vy)x5Tfh_Gt=#69l|Ee2&0-~ZDe81NO_4rR!*SNlaw?9xjEmXV<%-*Mhi2e~ z2FHp=sS&MMGknJH0~iQRx)VFpP@)--NgyR!e-!e!Aw>HYv`j(9Z|@0}uOH^w)!>ik zhp*Og&yF7Hwc~87s7T9cA7KqXCsGDJ35$tn!fQ{>Fd9lUG<9?}u+BPy;XXQ==&Yj? zA_JUDuTY|xbrjjfdI=CMr>yb@k!Wpy_j`K4G@E;qt04`^!tp;64P3hJKqg8wxhXIB zYo|{Wt>U(%LOs&S;Jy_46Q~ny`2B@P(u7ug{e~igi=*xTh=#Zlc3>wO2#mgQ7FMEx z;csx+Xs=DoW{I5kl z&Gd;q!R~)gVF#C#a^CKhTvA!8qe29v7)?JpTd@le?RTpoZP#y^QI>!hidqWfN(-^0 zJ=#_>xuitv%cnxL;GmQp>z~|xJ>l5E+OqaZMfW_;0pV?`0&Pd5O&6&>cK`XJle=2h zH7tjwn|`4c_gOnNIo&FafwT4R*K1A@tpXt$e0{%VL=QFz{$H1(5{%#yP@;i|L5X%` z!%EF-#xWs^w4C>|n%-$Do(WomPTd${YrsSZa=cP3K>licj<9`?M1Q_0^;G0!@tZfe73^lD*kbY01I zGNXU7t^}kYGz~=pW}&z{feZj?dDM)R0(a})JqabpUDVz`)xbeP4oowsoCN_n@Bvhr z*otVh-`hdvCG2#BwiUn<|IJ)ji!d0zE0_x{!bBhQw3npq(r*RVh#e z<^J-ibg)yLH_d&~-C-lG2=}#YnJ@5gYat_`hu-y@198Qz_zx;3KWQa9{&64bB^(aQ zeP}j!A%R)jQH0WQCs{U=(y{CIFX(u!X5kW_?mGX`KsNQ(?JI@$bc;vnZF#x+=9tCF zB;lMj-{LeDi>;)$&N{`yp1o~_3r>Vy>i`pD^3WpY#`3H9?afRg7e(Td<+)MiH3h+grUMaTY%U37NU)~1?Xy2G1>(`r9I_sFW$OgE)9STI<(o_p3C*-qqR6pq~j;~YGgljWRDK! z$y8Js8Nw#vjNVuD`(P$y5=iOD+k{*cLdQ=5iRlO(19l565*{^c9AC!svVi8cJEI>z zkG#l^Si;D>zWoPOYtiqgV|jsFg=c1y3dICSo!N#kOh5q$hU4~50?-+e0nS4OIVb?g z%%^r00Ps=5*XbF|wm3lxWc3X*UuW*F)d1K4}2MhYNq8Pa437Fu=KP<0pzI4dFGj zu8}`!IR3|ORH469V48BoHu^loOf$0IDJiQ7I=^6bXwAaIp+DaHXP#wJXFVZ5WOE0% z78g2>S78&rx{^hHUfUw6+}zAOCol5{94k)B8zhasRjpU!@--M|vdc`PV>F_9GQ`Sc zx9C10)12}i3fUTd|zl{Z(gd9dURDuOu0?KL-F(|8#md-W^L0G-uoo8}%l8?Jz z+_E}z$BN|p6K~xz`+2{HaPrBq72Byk$bDEw=W}G4V#mu%RGYaDQk^14Gcg0wg4sCN+g)z-2P^;B#$;^S+@FVgfsc{S-D(GX8U4=B+njv3nN|AwtR?s?ARYh#9Vefx9or+K$0j|XY?U!PI;b+^9Xw(R$H ze-`ta-~V`l+;$6fboy@(nOxGkFMgX!-4Epow$VvjSk5$vn=PoN^umFU=bdpt?$D*36p&pm0! z>(v+@D}pF6MrfeVOmA23?_cpKJxKQP?Fd~)aDQ-}<3Zg}cF#l0VK zl9IGXTl#mO$pO>)RjW~=?IQ~ZcA`NWA8=_)&uYm0CnMq$Xf?X+TaC`_5}lSJ{*t1x zB?6GNrF-?q0LVnmRNM0n=(h6mQ}I=xe?PsgJT7u>DN3}&qo??&6D@qt`>pAPt@xCE z^l|g0{}B!GB=o>ew4X@ISqt`|M6)5wW>TUFd;fxH0W}NbGuz*>sk+ms@nJGzh}QT4WpAvRVQDEMmj}`4;NaNZ}Sw{i27qO z*Wy<9IaPz{mLHe9&gf*2Sw{>+YuJ7I_c#D@d1%E^9PB5POG>nrI#h^unaj>bD^-$< zN!@HN zHkcw>#rFX!MB7(7j7>s<`Epdk0k{N|Xdq%xqS==8?hinS#**|iy)ZNV=aDO2t<7d< zyk@&zdcR5q-~Wca^x{&9ifsOIL#F;Qqpp>aubI7PSu&r_ENV`cj+uzm8mST|674%m zwEv#TEq0W|CZXu;a+GKX$s~{xO*a?GZV1sD{FAoJoY~L4L4+J2Z>Okfz&xp9|$SV&{;SC;o4;UWBR)RJ@ z3R6zY3wS==_@pcvl}kn%ADqMbgkCP>pYUVnwPl)y8`|(5rdfjTvi~hgfqRL{!zt*k z1_%uE(${FOP0Z$e&oqi&oA`i}^j709ubb>eO!ZIJq|HNAA12FYQliO-{eo!zH4DF$ zey+VR>NKuO_@NSO-+E0-M&~A8_=(lA)S|rWE91L;g^W)od2%>$hPNpXi(I-hq3FUg zI(uwcqsJ!=zWF_#LB&gh64uk`F6j9v{-TuE(zS#D4q8 zU7;hS&G(a=;hq+=NS-%-b#PC^-WPWw5u)7(@3v7T+MF$pzsCWPz<9zQm2ea;0VNuU z7?fysCB3%(#xZuoH#MJG=HeAScqg+Y}P!#4ho2%ra<#mJpdpP-(4L z-nf<4$88zs!ncYQa?Msg+f+H?j7W(_x{Kt%(ht}qtiU;-L_0<%fs|-QR!9sXM617$ z&K2q2`$jxPlrSB1R`0CbG?r;8x2 z!R-6HbOfd)HMOAqo*kK&{zp23ORw|`$4W;qJpJk|l#Ys1PNS*5Hcdynr$TQo13K=u zU06Y#j-M_X_nfV4!}HXrCe#d!`7j zpun_X{V~Xl8P!0>;OCeC+Ny`1L-qg4sWOzd5p=2_PYpbyDdgY?yi+NuxX4Bdo z9O+|L~kjYCeft?`!dgijwsnEqUgNL@xxfQtp?*-j|544g5WSfZ|(2 zlM)=^5>W1gT!eDpzNFXLZ`VS*o-+yAyGocHcrGn}yKk>5r^ltu&mVf)(D1Uk$-at= z)TGnya$!C#v3!|ab@y1iMemy^iR`8ub3Ix1;asXnxlj6G>UEFYut`|3P#AqCcbrTD zDfjn+3IpN(`wKGZGCl|QXO=yQk58i6*`;TEbF5RaH*s&RDOa3IesapVhGu@K30CfZ z-0XtQXxiSzXusz~W;Ax9LFf5ENNGNi5(|v;~oSQVlPkq_)FvTeT=HXl zgb1Tz0{<%H&2N&o3c$L_-xKZB5Qf*Lf!r{!$*mrp$>fsO{XJq->i$)v?kgq7aWUWT zIc;{Y{Des8H|d(TyeF;>wyX1L#0Kb3`m|O=4O#p2@QliD=*-W3UhlzqiRGEAB(LUg z>wZ?>rQg>dAi+C*Qo>2N1hnphh(YVVLrIn28Kmxa*!AS^eQTd#Ejp`4%JfRD!WJ3t zW2q`O#jG`+LOq6_>(#Whd$NUQEw3=4%h~pH*jH%Q;}pY@EPO!kHZIb-kM?_0>n0;( zjIl`wdpWsF=qWM@q;=mR3Q1W=-LF0`Q;pR9P^X1UWt4h`Bh38rcYE($z;gaMf3)G( zmvpLMe`?BcSInEN`@f&TT)$%G_T11r4B+>&8WyZQ8W^s^%8Jflx*;;aIlh{nqDN~B zce?=Yorcd_L=$_o;J>?W5|}2wZ*m6nG+8+QN1}mC-)fddiKaB=1vV#%>84B6+w$xh zK(vFy22ZIIt-IWrPGjT~KD2MA*e;!aZ+;`5gdW(51_D#tvJ52}F+EowpF`1fEwmV> zSpLKPcZmi}4G)xo^!XrHDTp?J3}2oyjy{ufAd|7@<-($q`=R^Txw^&s!cktq8o>$B0?Q2KzhO0wq zq#J0qsWG3%FR%B0wHkNe0B+(40Vzfwh&xOIqV4)^kM=24_V=|E$dy0Lk52!&k;$DR z8j1rT7m5SNlBysd1PAZUEQ;qho+b<=^uFw{?UDLc!5JlYmh0j<*5yXu4TJJtxQE1! z(q{Im8hn)2%Ameo3gGqBkaPZN=9v5ksu+ORfIui($aDH{l~#dt)&Cr#(N~;CPUQ z`feG~`EQ%<8ldV3lN!4~JW1{k$)o>|BC?4phX(em#m5pTZWeZ$HU&YJ(* zrXKMm^nl_3+BJZ{YnBNunWON5Bfhw50I-ArVb*?tS|GR z4UQ*SHk0Du$n9U?z@sKMVfKf&=j+XTl!cz>mwW56|KQ)NC6NBe*80`Y)UO7quN&RZ zX0y@7D|A1g%U|ZfY~aeVh(kB7l8zu(msK}B+GG2($1;s?e1vJ_W@yI?WYHlB4zNwx z2`osPQI@F6_188bMqng<;DzRJZFuCl5) zZ1qgN?$qFp!yWG}leg!*dc%0|oxlx|Vy2wBF`D~B3^iQD2B#lcq5<6qgl}sdHVGd6 zlM=k)5>TRnh(U>Vyd?FS7ecg3y@FS9-&X6eEPh)W@469pd*ZuW_tP&w)mYBoRe#t_ zx6!kekzRkvTMgMg3VE_y=zrM$NFn65YAhWeX5B#1;7lz#j82@3O+xK@3zTR+WD-b; z7Se>IEQDy4e#zSvy$&B5dn>te=c%1a01o&cZDH^6FS2OhJU!X5(S{jl+!puWu^fbRd(xb zJpf>?(Hvpw0IavPy72CDJ0Aa_qvez9zdd~sPeKnU02z^?&=k=yRYZwK%;q|GehQ+& zX84}&`>WYJgyc}A)G+$)?^&{JCN1$H$e%&qC%O^D9VnL&*ELt(Ea+3IUD)ofA6pu( z>FqW&IxP7tYt`oN;(JSnW!X4fRUeHp33hWdZm{vrBXHcWP^cD}D)EM3%>uNQUk#vKU_%c6PCq=I#bE}r5r?+RzI3#TJ$fGUoXIonT z;cRYtNmi?Mk5SNQ@=dIBSe}I#o*4nZYb&dvYZlIv8U24xS0Op%30ofL|mLS|LZ@n2j}jixVS*(CG5C>mKmVh`QuBp%n-ehvhp!S znSo8a{JH&xpkxE4>K!V=N;WY3^}1q|Y^qc7{kiSG#fQP+aaW!KvhB0p%1E7Ti+k$V z+==SIYv-=qT_)Z7Z)Xg|m9PU!Hjt|nWE(~TW9`|E;^HD%Hk0Ba>NXWz+~nd8l!;$t zAh7rL_m6UGw+9@1UVPzt3DcU_hqJPDF6Xgo9`2UXvT3D>r|F3?df_Km*6V)%uK0JsDc7yd0&aB-p}HQ8lXjNLQgfDf+Biq$`M z%zY;tM7M({D@nj!Bhu|Lv)YD_VO?i?v;FQ5)P>L85`4(VmHl4#-qMl`%jL|P;SLi; zeAGIE%wi04TtGq>>k1SXfn*X$aq&=>4~jSof4@H@0407h$SVcr@L3?GTwj+S_kS>XKG zZbga`FCfWs5_AM?FV*9SC7$2(ua2nk% z9(E^b{m14;pd}ud8loMDwZwzr7bfnaC0=bRzR$EHrkfWl_i@1zq{QQ*3T)!2e`GS% zMKfo9>R0>_L80v2o#uZf9&sh?z+U1Y;grTqc+-naDTGO;qU` zyWuScMj9KJHL4%YCUlP4s`1;XOd{;}k7+8j8D|&`MHomOFUiALyA<@7wDA;*FeDN< zaBkgZO@Xj?)&Zvh!oDAHzC%PJHnu|RN&IHof6POuRi64 z(1g3wg}%v0U~B=C77q8G{z41&LVw~GL2S-~ulORBWVI!yTK*M!#FelETIiv`27!@` z%7Flz5xFq5ZYDYBqPHT%Y)<@2`IshSe#JSE&7|)gDgE8A4~!zpMoO4y0m813Wiu(l z;>D;Vtk&d~ItRCZn#D@nh!5Gy+WMP=eZ}REtgapUq}@C|eEuUtKU>Ghwf)zWCwB4p z(p=!$cXE?-UZFPA>P77Ba?0a)_Y?-3jf`V*^BzniOm`AtWtSSMB1|sNZ$=#g)>DK6 z=!>V(LhJukGJT{7!?#dDSU6{Q#G?C)x}@?&9^h%0zqQ`qlh+nh-lddf_q0%Ri|+XH z4FeAe!qsXQ%E#IadcC`Np$wkOOd+hVf}GBIqs^BkU*<+@XmiY?zYs>=g{_%-E@Rs#hD|~!zb=Zf>tqs05msS^#1Mk8 zLf@1*iXO*q?p$$EedxTbz8e262_NOB>}|}Vp1EE+Ie|glp0b}`luUJDZUdgkb$)S_ zo6X!m3ZxA*b(|kvLYd@)3;?OL_Q)9uCh5jJJ_UZovWt78rkS+uPcaEfTVPsxeJF&s zz}Zk`tlf2J7j}az9M};CEq%Zek=DsBjOaApC~RGr>)-9dfT?sJ10Y2m3TH5UzVJO1 z&KgtcU?O>E8qRVzct33daOTZw%%KkFXUJZY&50fOT_-KAS^57>v>~2^9#A+#vv~{& z%=7x6Xcu;qESpIYmXS{#VdW+fYTudtQa{eN;bm_XxHzEDX)yG##pAiLvYgMJ*@~A> z+xN9LcRdOFWN|q6bnn4$O9zKU8zU6l@6ppP!*Q|V&a4>Ce>v(b+j(`E*o6@h=0F`` z-!5W!s15YJ;r2o6e;AoQQiPRzQ$g4*&WZ@hd(yI!J;zw$Z2c~Cbr!7&2{ocwp?O^6 zb#}_;fka~!_T1Bbd87J2eOpTyGGj2C_yJoaA~rmzz{cN!!jOt$nmHVNO%eNcqmB9lOhuqKer z5QG)@%57J4vt`s|)y(xTlMV7Rx9QeO3#2W#ZLTga6!(~aogwt8!ZKE@2peBMbIS@S z7l7aGDqSF4fD#W3$DMaaiRX*R0Ozy!I8h~@b&20J@y3k)6!CzwDG&xsb7ajx5f(uf z4(ten-t2=*Z?>mpq(T0_G&2$dN<1+9&`VlIu$q_XG_LdG6!VV6M@ytP`)YrTcu<4^ zQ+JolWju^X)0~Pg>r);^kUK|r+u>5?@H<-~2=lyr_F)Nigz;_GraM{Nfsb#w@gS`6 zUw?^s4|-rn*fiK(D?vvfi2;-g}wc7|sN|_h=ie8p`pX0tEfaU9T$#<4hCH_tS0F^Ffn0NV% zp8@m-2_$TeVMHkv4VQqH_>w(TO8n^($69-&#Gfl&K`&wGI{Reaya6S8$CRH|%SDf0 z5az9CxcNc!ZADO>d-Z}bZqIqnaxQ{-uVt7HexPj$=DF|faf@{uf?n`pL}Y0JS&3gK zABs)FJ0m8v#NQ^9Kw9FvqL6SyN_?J=+!aMPJBOt$I9*L1&Dz(uqw;g)jt}8Q#pSzL zR%#}m8;S3uizYrR`fZo;wK@-*(f-CP=+c5aWJdpDX#uqUgBX606h!O)8AKyEx6hkP zQU8-ZHKGra3Ob6X-<{$9yB!ZOE$f6RglxbE6u1DxwXQuuadDT-OW1J%Evn$sf;Jb> zzCdfrY0QpiruzbJx(IzW&=++0e)vUSu(b8!>onf4_<(uzDp42y6;;HO&;wdjpdUU}iv6LDN&YryA0VLbG`SQ5NALhECeWkYzJz$5V&= z8MWt5PfQ}SE_QpQny2o>&0nsvcm?a;G4a~+Rkwb$l;x{z>gH^9dCAb7Q5%yGaj9;o zL9nIDc(ZHd7EU+E!<{dJp4ol8ykN+Kt?evRaAp0>jz{BHlXfp-g??YtfW98z4Jb=u z$@G!p@JBv190pb-iN@S7m)`QT^yxOoh5H%%&A&C72+W)Bb+et;K9S{7YaUzU%5K+2 zrSaE;gEBbtjkF4N15B;8n`DU{4^a4c+1(s%5HQc@$`LMJ!g{NM9?gjXRt0G4Jao#>1tJS$lG>L z7?ki6R3yhWTB+gCLvWbwoszETVlTWvx%>t1_H&{} z9d#iEi6)$nUbfxx+;l8gV+U7J<{NQzr5$fg!EbRWl(~L}(NKf|zpFjAL=kotu?w7= zA2h{;u+FW2s)d0zXK0pxw#JGuFnnZAE{d@GWZ}S$FlhM!m$r$%4kIe*964#RMM^d1@Xk!%^GS!h$p-*WWD7-;vtTNAJ~Zp0#mW_4oW;? z^6%b%9ux8I{xRZ#T%{l$0WmfG*)Te3_kb*$NfFj@n>xbcOrq3|G`T!#aMV6wzH8oT zk3Ns;;gf4zFVrXrSqmQ-E9Iw`PUN^M&Zt9F^%*(~qX>&9(?^Q1i9J*h78#h5Bz*UQ>>}^V zrA;r-{kRnMislAegEPP31+Br{DnU9WJNn3q;85BvFMFNX%3c^Wb-IhH(n<|6m4VfN z=yNB4Fd5i$CmOa#*d(k-nv{?Lmw+M+gb0c-*AmAsM@<$Q+T9is;P=@4Oqio#PL|Qq z8tdntMSbWuxa2Dl<{Z}bSi|{@>BB9&&ttXJJ3k2pjdNwaopt-}aWDEt4PniP z@Yp1nQDL4fDFe(eq8176D|_GmBOLm&HAlBO%< zs#f_z`p@S?@1h+Y7K8~b^_^if6k*_}G3%KpiZDOKE^uBdbeaNTreA!Wz;h?kF6{W< zMHnzmXxUk;2m`~#>1fb5T$9McfgNFt)anU+RT^6Qh)(-Fl8CAF{pFi|;04{w=~!tC zhR>>){H%t~R60C1&6(~mQ*t6yQQBTRXdOg-(oS#JXaayy{8d$+;Gp%NR?T=lSiA5x1nLan)4e+iSP?OVr(yXH-{n+_6*8 zH=T(vjly3noyfa0F{2Jq)wgbiD2lKYGJPZnGvh@54EnxPRA9>Ed3PSz%BFDz4XbHn zOel#(tySH9pSv~GkA!gH-a0w{FK!~6Sb1N|#JcuCdz(p^K{3|C}x}Rz+a`!u5F1)c?Aji9k z(Uk6X->m*_V~g>(8AF^!>dE#@V>wd%LuJ>>ktoDvDE$Lde z9z1vAtttF%7q;%p%;!!}gn^%iKo~>(nn>c(39G-0 zFkl*cff-hWf#Eow$%({NvT$HW7_l8#Sil&DjM7V;{xy`?0|9sdMg3~<0}3cy%ixQzxaMSpYK8M9iMAE`*80s0 zQa`5*=y)xr-TsBeFRjzPywNS6Yhb^e;dNX?m`}_-_H=&5gICOtskx+*agVd-T0ND(&wHWh?L2L|Wx-(Fm<_^IKiS8Ce% zj<>^lz2`pJvK)(W6IQ?0II!V3+X#NWuA7qY#Pi-!hKlwF>I2>*CyY868z=Dpt^R{{ zVFe3qeqaBA1g)`236J3t0K(7^K@sL&Vno<)vQW;-IYQR#ftNMiJ`pvo_|J{C1yB5Z z`m;J$(GL5!W;TX7;}%EK*c=efi@K=i5w?8dR;TKwir$@WhuQ~d1t$@fOYvN7{tI5%z>k0x7~|M3EmJ1Yu7+mv2`*<=DcUw`22x1@DHd@=wyMNh)<;Ra;brn=tld; zuIxL)iNtgLV~@^&vnlRdfoZ`~Zs~H z^l&pKguVT{2%|+zwK(zyMc6a4Y$in*zc+P+`I+2S?aV#;@I%$2dXBd)_UrOy+l|EE zOWne)XlQn`+Iy`3`8QuqFWHP`!flQ88J)A%*N%x;JW(5Em#1acVtefVct6dMz`&cJ zI{JEmum3JuF^c{1ojQ2cJ=GC%{@lLluwgD-^ zHp3ZRy-HsO*i*N8m=;WyKT}(g5xW6B=0hJgTNq5LlA`V)^5ML z^S{-9KD#lzg9W1C_kMCBF^fzBDZf!A?Ent92Hd+z#9`X@KlK<6_<#b3z%(hoMihrRWM0CK zLndmM)_k9e@@>tO7b-QAruo*i=HS;#(4uun`>Rpsn?hINBHEa*_;J@(vC60aS~SFy z&;!ah24pBS=BIC_LGDu+sSP(emX6|(n9awyA}DYOd$s(}eX z#C|kA`G*2Nt~57fiMM0VpjBftKR#Fb+|HnHj)Vk%w4)m*#Ub@qk`Lb9_WPO!^u0N( zg5ofjOdlx@)mo_F@D8Wli&;@iY?aqJWQxV&zx!ERUJ@XS;4lNclKcsIt_H=ShX95dJs^SZ=A?u?xC9i3 zAVg3cdX~uZ?L}~?zo|k$s-chX!0YC{h4fLwo9tcpvZ{s;ey54s$gsa#OmeLDb)TSA zQ>W(T?))>(ta|Kxm!quhb6OXSwmhMR!}jnw*rL#VPz{}w%qNpTio;D-$j=Fa!-wt< z(-luSo#yCK+$Qm$^5X05VF?}*H^zcKEO}&j`?Js7uHnLWw?DD0?zbEV9J*g z(2njoSvHd*Y*jvWgxQ+h$-H=PU&;qZo)Vd*YNBgW*fvS-y^_2{quK32ft1=w5e^OE z`R6qY1@PP<-j9S?rJ{^(eO-@Q89UXOq zbw9f}qYhEkC$V=aim*a5eWVE6=uHJ-cRB4|b4M;oR&vP`HC^zS=}6hQjHd9)K>Y6EQd(5YW@Aryxa_+_2i^Rl)Qc*;keM2HCcjv#Ck>>EgJHN_=I`}0n>fv)Wtm&K`NI4<9AUI(JAPjy zK{jgq^v-;CO#%CYF-h(PMGS>e^A&xf95~+@{D-jW?N_&NAJm=t7RLIm7;Yp$mWj_@ zh9c}GnFLaVnS*SGAS~H^dB38w(`SKV?ONHc^EIiFh2E|)+CMKjf0qsVa!&U_0_S~W z>02%UVL}+5tGTcz&hR@FVZdmC*L{#3#lf!#`b!$R(2ssc>>?O|{PnKl9~66~zmQ5g z4iKgw+j5nNFvFdHwF?8L?bcvJmxdIPg#$anpoJb>T8btbE%bU*ULbtBF*eRq_`Y3y5$;xto1=99_>nOG_4P+%+gKiEM49 zxiepK5%bm6G7mJDO1f&CqI(}Uv@r~87v{gmZidlNgaN;EpI5-zg@NG@VwKP?EC7)K z&R>_vQgmTNgb@NaT_Ymw$lpa6FiqMn2kpYj$ijggVbDqmE`9UaN34|+9nRH=R!XAN z`Z`}zR7%+XLkIp?rG&y6m|FU^5rwnu0LSi=(x);2eM z8xz$|^Ho}$wIj_P*B`c`!XTmo9+cOriVE%81jysJ!+62FL3)>%Z&PNkWP{w`bmvt-ZL z3&+gT><`8e5`%`clE3@AG$wVcoTS(87F)0J^~PwubBh>0@WHsCU*GeB=`LlC(To!B zzQGHdg!07`G=4~kD#;{}mUve!WB{bZ-*hmI);EG5L9nF#am zUMOex-1SY+Fx$5m1pmnr|MIu1|7=&!e0>jE;z8=j&SJn?;=yqG76!D$2O=`Sc`uTY zEXcp_+3mQ`^2otshmn6@exMdzELhDIb%j{s<^OJp2c~&^O+ZWh8?tam(E|^Iy&0MkP7O$dF6B6Vi!=2uW%!(h`J1{9ywzFDi*)t>3L)Y> z=m9N+jL1-EA$)Exg2I%To)62~DAJQSrTM6C3{Ol?{Xe#uAM`c>C|X2*(T3rjKebwyM>XEd zf9N)MM2lB8vW9`MO`GY#Wnq>@JB=cT?|aVaX0H}i>}dR^O;f8kAz3@PJ&f3;Xix3^ z=U@r$2%txTjr;G5C(tL*x*naSdP}B{6k#rVs30tkQ|=>kSVHnz-p;Le?>xC=`<>=A zy&PRmWA1=<>F6M5*UKjHB$3n4vqyYzwX5k24C#Z%&)lY~X6E$jB1PCS*wTsGjwRS6 ze0JZAN~nfQKoJH)1Vxx{39sfZ1YsgWYG;Q{PF?P;`t+o%o6kzEfhj}O??BK^0!@9f zy2x@@_f_@7VS1l-5MskU_ZZM*m{x4o)Z6LxUYjX1avEX0Gbh^K476dBAaUCeooK5e zlR%0v|0X2T5QN3KJi4Ocbn?*lN>|I5+3sn3eZ}dsr8)YdvzCW9rkjr2WN>Ppo0BSt z6=6?0da)Tjo@t0Kov0-fY zC<+L%h2g6d44p0@f9ydLl-$6yxYL>taswYgmAx>0j^gkgnU}ERkdfM@X=+{3s!Md* zk^(1+7KZr9-Bsm4F41Xff4Axar(JvH0cp;lpa;VpuT1V2vVJNZ^zQmjcRW$XVx8Lo z=y!R(`vvI5A`h%!INpJ8`&hrG!}{Mof`})f2Nd+sY-UCRW9QzEK6iRgmd&I%JfTA! zhk7Ql8J%`U$)=YE6Czr(1IHs>R!$h(b~Z+-i0bRUD|fIjZ@oWSU z6P<(h+gVH=fwZe~*-uyviZie3`Pn2@qB1>cr->|v|IMVGnmOz5YZ}luTX6CN=3v_i z(t{L-{>Yy}-{*=8l>5kdGofppeoIEf5)KE!#CIDWCZ9K~$zs{rlW&p4Vq^KGk^5b; zkAu{}1>L%NUFVzgjz8}f-GEOb5OD}zNrpam(jP0pCP945q=b671QdrLL{J=_DelwT zX%fTD*Q$NBz(r?}G4@4yu&7h+uA|2CG zPkU^lsWSWnZG0hX!ku2xq>;thqwRDjceb6t+|g~Vn7Q5q$_0=*I?F8}T!0b}41e%q za_K}cA_JUvpWQ`4JmMS7)~W7+#NwUyclQwlrupiWp$Kar3&;P6FmP$3U6YHbiC(Y} zwZMe1wm&ut2W=dHsa5<3up$f$KljQ3owVC972iA3$EFdM@;D{zF+f=A&TGGTqPDYO zL?dQ?2mXls6Wuj${~-+VB=o?JFcu^*3wtM5|9vFOW>SQmX`zlVO_Mm4R|gD|O&xwn z8q8MjERE(}e_uR-KE<7r9bF@tK2n6;yiEmR z_X4>;)80rl(09`ok9nkP$mA$1W@*GF{7O^EeID-R5;|tXrQ#`yrCMLZ*fkC1x(v7+ z)vcYz?De{Tg7>+=>OVt(u%oc06Er_&?kWxpq_}4W5}LavB{ab$pa=sYf+Fl}ai4(^ zf-t7nGWQyy*-dYpmKxY<_m<`{?dvls0hWRRhY|`+jDr8@S|qy4z_u z&L}N?F8WMPcjsP?G(Bnv`;nKAEeh<`lN&=dlSv>&SiC6W00_dOPp{}l5axY`rfy%Z zp5=N@(X;6|Ssh$PyZi%BO&*Pq+@Z8rD|JV)c647$V`mr*MHuiqcN7m+gn{8Nx6VSF zwh%-HINw0eL4h#hH?>sdD=!iec5=&KT{;0wTU$qqE}dv03kPM}?jt1eU5oVbB;mLahVYpKsT)+6H z*28kUUZsq7{9K=hMb8ZWeGnM&B=o?JFjgcm_uA>vNy%2SY$iq66>sVYlQp@ga?yB0 zQvHf|k&&w-HY&$Du3Ma7E}yaa%ls%a^>gEbI}{(WadLebhzzG2OJVvc+-}CP;QV>U z?M~lW@lSF!xzGBJ1`6%hvfF7&iZJynT7^c`mrk51JT#*Y0qZI1KhTFeas{2yYa`P~ zim(SdR1kJQkoyPj`m!K{MB{=N?L5pyu3`p}2*NUdi7eZgG=irN0|21>c7gj3$aN^wYrK%=B=e5!e{9|niZEcRmX|NOoBO7z_`X#3nBL8u zn~PiX8|cE^B_91^8^+U&bE5fIx8r*+B!61};UB^fPeKpu2xCJ6le}hfBC(w;n@JHC zBSsxzQYQB@ju`4Cna--67wKe?$dt&n6<1v~8keJPX3*r&pA#%7s{1od^9L^>?X*wo z3eF`_%tth>m$>E9v^ZW1*rzrB`lwkhj|@)N0i8$$T^RCQty1AiZrLvo7G!QXqYeS< zDG&zqnO+q_5!OMbj}&3gTBsoGK|uHSiEGQcc6?DEuqiR%k7WDMnDJat_-UEGiKE+1 ziGgjR@=d|LCv+Ru4koVp7{@IrroQH4|ANe_G%%3}5LQ983ybU;!Y1KI*`$Q8a0w{F zK!~6SJ6HU6zX5`xqvnBB-Qx zeBydqJF))4u<16Ib=^z!)M`U}(;ELn*yQRzBEn2lwXsQ%Xc0mY_Ki#eDZ)yukQhP` z7IrdqyN097&Ds^#M{t$4O;Ncw5BVrbvwyT&Rb*E@mypvyr+BBL&jo82Hoj(jhS5-j zfz+W<7r}}!F#M&t2#T;!LRU*|2t^ne z?%UmpP9$!gif@-)!*mxGJaKf9E{hfp=kCV%gZiSEIH3{NmW*euIL?nI%YJM( zIJa1PxR3dcbIDkcx}rznF2Y*JL2rXLX_{+07*nUVuLTRdXD*#san*K4wFCxIfEGv? zOy@vLd^ec{(h^?>f(z-KjDX*Zz;!$?$3CRu4@B73_kzE1>8+I?6}}S0uwed3!SvKzZ4$paO3CH2I*OSxQK??sOLWHLD}Gs@fVt@JfIpZ_D*+EZD*$7tEEjrePR z-sY7xi*fV2-Fr0!JYBEs;Lz7ndKRHn#VjH|^5R-YHOZu%bCtrCtY39J&t717Wf$nX zbk_uBNgtU$QXIY)qk_W(j*9W2D>6Y_w(KdD`g;27Kn!cdTye>dcUL-puB1Jd%o1y? zUT&FUrBTsGC)mcmM^;LCTuPgx@wsB6CMgb~D+zPI?fHFK1`^ahOiJj7OF(f5LImx2 zE)=JEtV3|vczVyzSmmFLZN*D3npzw1CmypHt5iPN8!$@$R8BgoOh#?H$xj~crQ)7> zmD<|Z8CJP`Dk0ojpJQw={FLjzj%Owgmuu|ACV{rY1jXS1nFLZCc0?fw4Z-0x=atnO z_HK@Ht@I9>rdH0Wdo&j0=$H&M$*DAT8S&;dM`suf_)WoZ;P=y) z&CqIm1sMR&<<%&+ksvN)OE}fxMXbhAoBmWah9V4@R;soa(wqTjLzQvzeMAv9NEQz4 z2!nC~Sd#Gc4y27>q-Ke2+Fi6kBKp{(`V!R!iIjNf{uuF~2m{KLqteli2YlHaoEK)N zOm{pL{-gY12*NxU$$D5*N0_D%-G<$^pYb%j4|VJ-{~-+VB=o?JFis>8ug*xJ2pb~H zW>SQKh2+$Bb7wM%SMFS-oM_^ZbD@IWqyFKG)&ROkN=t2XwVRHc&&^^xcD-SK$8&ug z!4oGnRAAq7 zP=tXHks{1v;%V%Iga_gG{EO54^bmxVA9!7?#hqI?Fw(V0&{{Rb{B~{K*?#%!-&U#* zEHrpi)5P||GjxByf9(f0&qE%x^7G~lXzkh~9927#X2xGBx^MU>!m)9}xD`(RzrHSe z{xxh-*on(W5jILDffQlGT1YxW5EgnOwI4xPw+Bd|2)l~d1DKULMHm>aAt!($jOgPoOGXNW5pNUL z-mIEl@1AD;r*0FV2m_{Cc?qBh+cK37kNtS35th=!W!DT4R=6f0o;t$DeRpi+RR4_M zonDaVLHp%j7lybJc3?*s7ZR8=#uexS_HnXoCPi55ZK?<>8Z?Q2_DWPCp>D-PMsBTK zU5^?L4BJ5&uTF@HP7_OJwH6m1$ z)90LjJ4d9UAXh`^)i(YucBSqk1k1kHQe~%eu5KC0zQlZlTPgtW_+Y}gthrh0JjcXr z`IBy?6Cd|a$k|dNj9bG3IqD}q_O!ONh5L7iV?c2`3%?fgwcJG0nB1+jKvXL}@P7!5@j@cZ!~+n9(VjD{>6|0BY{rC02N zeHM}taUt~Q`P$+-iZG&&0}KNw5C)rf?Ebq51Io4(PVW7;bt)ZZDc+t&7%rfE7Y8eX zu;^tjzd)G0?^(66*+>`GTsQJn`X9m&PeKpu2;)Wqvzjo1B8-+Sn@JJYt3w@OT_y?2 z7rB=wn9PdvYH|`y;`kC)B3(-RNn4vmDRii{{44F z9~98m?fxF6mm#=dpV{cz1Oa1y>v@@Az6vaz00>jRqE`KjE^JTuz8Q50RCkKs2+%j7 zyajDx=*aYuAk3T@`P`3a^WVTdl#~m<~0&05(nlsTOWnJl6=Px!$cyGAn36bm4ME? zlZ1Rq0*WvYA}GQFiZ5T&HND$~*NSksyZ`)Ap@%A;qI6e(tY7v--M@?D@Ny%dG z^B>RRh|{gVh$2h~Ll<_z;_QsnfpP&Dy|8{IgbPsOf#J;)Yf$1{2QrZJBn26Yrj1BE z2Dg-LYM7uDfsrWipubW0#aLMgW$^c>M-p77{uv4jZ@zuPa@rOmK z(**w^j3o1B{wsEb@gRX=8k;~7#za=)Nf1^_+d>^-jV6i8N0^r-?0R#9Ao{>dz-4r> zpK&zYU&&_e($L}8I%WaO=9w&@jp8kCGB6x($R4OZ$9qHLtmJ9E)>Z+LU|fXLLYfDX zoEj%rrK4S#If5`Sk*MILQte260sBiK3~&DceeWgY(fZF!rjHb1e7C6}EQRCYoFBo- zVobG(@FttI$FvwCtjiy)y3KxemAY?cCR0_xJJn>95Y? zQSZm`zMNj`yq@QEU9WQ;-I099U%VSyl_d@N^19WDNrcg}B!q$PKWAubNb!E}DXA2i zaDWP?cB-ffY;Y9-!cZ4M5f)h%o34i-EOVi)iEfPl^NyJMdyW}p=}Cd+OXx!9EY`aoMilb;sagHFlrGdF^uocRGMCm&{lOiK{1-V`c^6lmBq3~aBGG_VWtSXn zD(q?2L=nc0QGpy`@=_o-(B(@Z-SJY0F}&8V-y5uHc;v)>~a2+nsc)1ly-*{93LKt)H+H^$X}wW0%}C9YToAve|NJZ9sGzR$&pu@db#gfa$5 z;<3X4(hLt}3>f~xc@NrQZZTnF;@+K;9p?CC&pru+F}Tt;AwTLfeWfm?=dFal5H25I zu`OrrzcLB2B=mqX2Krb79Mi|`j=rTp3jWARa2z2Aov(_>YSOT_^r{EY4;eyJ(ZiPIxFNJ z1tzN>99phPw@8ZpWPP+K{j*4>=LsUY_HHNW>XvrNMU`0Hg5XfqvWqqqLf)=Gamb5N zfgFcw`N*9I!QruecdK;WgLo5W?WfnvuI$%kCg2O&bXm?{XP_hA_{Q+fWYA*4PL!fVLtEcT%X+)?V@Y@V=@RLPtu8XN&Vc z+9H;O9#GmsqgeoPjM(buXhp}5iDq(y$;ePgSoxNl_c=OcuWjBNIe1GXXfSua$5v&5 zag;}`U7WAPx zT}oC(v+YI$iGlS@`*lKcH!oH}uL-UTj@E%h+Jx3U{{wwPVFpHMvT4ZYz$xU=(jzs4 zM3?Q)(L$hgdZ^O(@QPLWLIypVZ3!nD=J61Sr&eD{U>jhJ7CDi%r-1%&8T+%G9;J@Y zyoDN1*+le)%QO3I3|pHxek}!06X-`Jm64B1l6``taE2vqD$EvXN9`_%QGpy97>#F}_2k^1zCsbtI0?Er z!l;wL;bYvL+r$=vq{Lb!MV)^nKr9J8pd^4kd;rIM&UZi`K1k7AV4zL);e)*Hik-HH z4`?*cKt#P#JiZl_FeaMGp|zxtI{6V`h!^>QJ5|UKZO->0D4mC~s-c}b{&v4OsqR8n z(kfNFehaPYkoG?(PoSTZg**DhFM`pJ99qkR;m~@fQXHSC+&?;RQoE!G8Zdb<*GalMl+4g$H`-g;`p#gq);h;*qX$YnqQl?h%5Of_yMCU?Mgc9-YsXm&?$D+}#|2N+?qV1f$f0Eg zZZ!zBLiVbtAkb>lc$L9gtpDny`5h;Nypi2K9r?_r`RbiIl8YEvMueYV8*l&k^QRw! zn=Vcf8i+&zEf6e>i)jAzBN*PwZ;wLjEOI??-gJ_(XP;DQDU8%lc8mD^-AW6HCdKd- zg_bzR9kfG>nVQiQxR}vNCXy9qUZtZt$)tJk$K)iF{In%IC|`jU8Vdwyo!11zlMjfX z^O_(|!MS)e|Kz-;)I}Uq4|tEP@WQR%R9YIfylO9Bv=9Q%i3auU`o~wqlF$RnS17au z5l2*|)u2zrvoO(2p42U6;7R=zlGINr4tQiV-FP8#McUSHeuAUdh?CKi?4N@?57i}Q zZ9+E86f8*ztI?Gi_(eS0v_dZ2oA6&!-;7D>895SDk_%O#6V5WANnHXXG%)frj|6~8)&`tGBT%vHk7 zzq;yZ?dX&ES`!lJsyyN`+%1+)UWvq2Z5$va^#I%f+Eh3*CWj_mT#DSf2%b>gItli3vkgN(vx{ym86X6N=sZvw+*b1C%j)szvp*;Gr(@@) z2n|G{Na`S1Ec)isT8V(+&rQ~$N&OshJ#d~$N7=>hAQl>hZ2?rCwgn@q5p~Np7ftF? z765L$`Q@Ww3|E?OstRwA1wjF!JccCr%5+F7ya8_1Mj2Jye?GO$|k zCA)WC3t^7j?XW|*)_=X;h$&$QG*d$13>>qWQ5CI3NYUK;osXgt5s(%O2GLxuUNkuk znmTQ8hDNgxBC20D2l{4}G$xwKE0J}`Z&P`*sx)&;R&IEy@YPMP_jpPd9bO$AaD=Ef z&>{Zm^sp^EL9%d};OK&{t@5sF_ftP`(MiiNG>IIzMW`?&dhGsrD(6vC-7_7!WJO^H z`7NcsJ1Tm+EW{|sOtBKP6JJzG8p~Hp|!0L4z1TJ%W{I}4>)T)%ePc! zlb=sYzy5Q7yNp2x?*`X_WBQnEV^GuoV+_)1OI~-wi4kAZ=8}m zsS3@1NdtwJEJkQ@XaT1{E0LHoi|VBav<}=?EODqksv0#%c$s2Krrge5o}2m^za97K z9Zld;#v9lgq=erL*rl%f=+>|t@#&8a>B=BgRVIaS60};7=`#Q=(ieQA_D$VO zh^h)od&XNJW@A(!ht>go(9JE3!mSGcZc0U=diMBw@!3h0-ZGiwcR`n4)N;GgIf%7#neiUOXQ!>j4=6>!Y9QLIycP(nsk(Nb??JQ$VlKu?v;z>@(*PvdqE~_vZS#cD zYMS#WiN@UhNrV zjuZ+&FkHLl9ST5F@QYo|qyXTzv|V`gac=NTF=-v}?%xHVFe2*NZ#Wcya+qi)2cZ3V z3;?}$n7Ed;t!!;;Elt^SE7vVo;7ZLY`;MSdr~IsQ4tm_zx3fNv+#sDT7ix?@-p$IC zD;b#OCy*z~)+wORFKqu#jKM>!o8ztc$y+$P0(=S6`)h$n#`PuU2-19^iR_*ja9MC|8PiWxMFa*z$5@wtZcH$0BBJBg*FwEw=1A_pN~<29DpJ9 zh%+Go^!HGyTJ9FIXGcw03@2ZSZ**R2mw&Qq|AC_CcJ=Jsy4}tUi)J%vqc0QF@(Fw1 zLk&}e1{9zm0SJ~S(YsM-r62>q`468R6wrFHB{LG-w?kXk_>=D2-f1htfc2oF?zZxz z6P+-H#WuvqS2h=@q`r3Gyd+@ zj39;dh*T3;jb| zd_PIvzjZC<{iknuM&hsGUkhay*B>v}JI^a^LtWXhL*inBfZ ze}&do3mW738fsY1*fnnj<*$3zFLeNQYD{h6b8Uyb-*)rN&uc<0!Zjej?jYXf4JF zO%5&K6ezUf%9J}45NO#(?j4!C;+KY(bZt)deh=JFCB2I=95aP#dM>Fe9`ks0E5M%d zmsU)zT%A`-LGeNsgAv){)VL+V1q~dwPSl`fb$JzSD(qdh4Yj)hMg?+c#o>{tgh1<{ zyQ<)Fw@|TXOA>_QtT@J#ez$OXx5__r92p?^>*)g)z<&Se*n46?+)(4 zs;&`jd%ca)i1 zC!A2F@MV&jc&AiTFm;J%xD)o9ot89;d_RSEG!u*lefa~H{$nO$N$3G(Ce z+G{L0(@!$m;x3v!2<>Cw>9CmFgg=vNw7`(lT3PQ-=WXG_`WY-SZuKS}XA;K>M}IYW z|6ie%bg62}?MGEVFNFd$c`9M_BZpS(c{sE_Di;qpN(_v3<7E3%=c(HkbTf3T)3f~K zCXTAP^r}cci=^9lmZ{R!pxS|XV1#Ko%MTL?juaBK{Bt)8O|7)%+@4RH(0lBQP-rd1 z2u%(x;1npdPM0YUEI^=T>60p)@6YgvRY`NqyR)J)%RtW1L05Z}uX})#UfWzS0H1Hq zkk_lZ+5FeFA`^P9b*}shil!g+!g$9HbWB1^x!UF)=+xsK4`Y+WG}=^H_NxfByD~-v za%f$WLM{q{miL}}D$93-9@S#EOJqFfAtK<}oR81BN&oTTt5>OUPjCx`xyCYNGs4lS zMg&^@_;KN={rx0vYFj^)Lf~dNmn4E#T4~6Yz`3e8KgF{tX~C=bVa{Zwl{IZ$48VF6 zzyzXk$B9BJEhx0WaM7FJ(cv^e3k>JG^mB?KLn|tnIJ9-vk-HD$l=EpK@#^4y}6SC&F9A2KR=4wZpp~i;~QAwyp69 zZS%3JQf)J#&CX{fnXiFuzsx70$*80 zD_Hv!Nw4P8KO!E<8;JLNaiPbl6qj7jnxt(?=sH2S zuh&THV}Gi&_{~bD2n|G{AOQ##_8+?-B!Dsw3@^U44^8dok?Vo;nv*URshz|);RpPa zyX)#tTct%$O|!3yOVHG=iE#(*shyeHrL8i@_rE1sVL?VRMOC)Isns9Uo8*9-={jBS zLPOCF9F!1o3sP@Fp#_H1m*k>FoXv!P?>_!-yIk|?TeHD-xwy@JE*{i7;ro<)SbrXE zCUB|j4Ll$I4_b&Np$F~I5=R{4x}yVymKG+O$)S}ZLmgV)Tkfho52MSldae8Ibvx6! zGHw37-y{rYAFj?mWtKLR=S<4HfhN|)KB+nVhI&s}Z!T!Z<08!p&HG2CMCPcQD-9O2 zigrG)}2-w+MI!Yf`;Q?SJKAlM-Ht!jc{l+D;o>1pE0;y=~XI+@;A}? zAJu0b9~1etzgM=|Tll0>sc7&St; zVpJf9RtzHG0y8K`teOAFT?N zw$ci%8Rgux32o9p6K(3y!wCIQd%?N zLr=^R)&u_lh*%PO&LtGLD5gD45#&14W18P z<9sFPADkhIcV(bw{=vN-#}_`a)5D+F zj~3rPF-2%-nF8!iY}G`Q`b9)8aK3h~5=BxcEeNTI)E*%%6F2|eqz**;&Zk9dQU}8$ zgx%1jZj5oq|CrRlr3u+BXi}fL=mt&dVEALYFKALH**IaxXR1m4!{)1#UrEgPyGb30 zI!mGtP3m?N;h_D!b26#de|O)$9V|_~JCyjFr73Hj^gYKin+bL~Ctk1G`7f!1J05n> zp44X{j(cfEUELHXTj)ct_cew8|Ff;Q+u)S!1>-$I~-g6;wWKU?>OtCLD z^IP)MeEl;vaTV69>`QI9KD2qDPdW381ungUKAeF->>>`=jo}#Lt^B#x+L+bq^nri$ zkH<0H!YOG9hze5Ely~`oy3*_Bpt*DfMqP4-y^(=4?Ay{OR+^lHCyvarmbSb0)j6sl zF!F&``@C8_MrhRu~839of2tD51V*BNxzk~Qyau|^Vm-f7NG!>?sW z^^~sAulU6FzCzE$lY3`@T@F(g%L~C2Rj=U7P3+xK|1oSMx}0P@F*Db~&q13C-SOjg zUx`tHoMBC=h{qrd+wGPkxZLIVLH?h78RFSzHfA-A>=^5A87@nzY9QQPTzHw4p)|a{ zX|i$ePcw7#Tf3(S4Mc(li}2k6vDMej zJd67LRf3hNA|6?EaXGm`vuxGBOpjO+dO$NhGcpt&p6N@_rv*}Y#xhG%JT08Qz=uwm zlJDeTKW$G7(Bc-vP#CikZhqK*kErRj zPiFfa&16qs?MR=_I-9TiyLMEljfv(St7~(weC&6PWyO)1_1p8Ff9* z_VLot#}&*+E7s%^elZ5VshVV14q(_ev&}Rz%)47_N?M|-F5zqn%CI#Ub;%jlSO{m> z&!tbil~@OZR2d&#kKueb*Iz~KK=-TAXwDVH;LK6`fb_hHp2osw37zd{5RP6YZ<2U3(NW;07 zBf*{B?2N)!^?RL*jNOI|HYw^oq;;aMZo$-N2Otu~T@UQubZ!fzxPdYZ46jJFL(8_y z$o0T^XXZMJvTdK$wm7h^;@7w4IEi7Uf0toEwC3DbD8tM!?x3Av(EI}~-CI=z$v;r! zf#JUzic#b_Oc?F5IbjlcIDL1XURDHobN22pqK>@ns&6h!uV^A@- zVF&HV1CH6_Sc)Q#6wSu1Hz<%tS|ubekn^SudC;UTiHN!@vzM*_(byal&EyRGbpAIC z<9m1G)%U&GW50HLN=H`eo;_$UnX6YMKKPuWnk7+I$xhg^T-)@3{irQV$qR{wXl_RR zMSY<@Y0cGHvPG$h-xz1V5^i$2BS%=1GAgl_K!>XqUEd^G zG?%W!s7ubUZbvx7x|bSz%g_(@4*rl==iH;%%6}!qKHOCbrzhK^rT4-hwdxduuOhcg z*rsa@j;lf*MoTgJ1ninzYzm>M!!o|K(I#~B)A2fNJw|A9h5;Wz8FtpBPi>aX>`cDC z&n2UE+CP7tqmN|H6dhX@9LJ=+gwHXB`%8SgV|k${k?Y4u4PQ+rwU8(CQZl18m zRX=U@!Y)mLNrov`+1w*z*v3&BwqpWz@3ftZ+I<5?1u}*`r>{ri5W+B*ooZgow}qb) zmJ-c8V$jtWla^{!DDakb?cv&kyqfnJT70=5y&+x}M;TUa{KsDO^9SBdu{#inf?+`D zw6{BGWf&M<7PJ#(SUPe&aL)MLk%D1MPPfGahOO9`JIS!TX`6bade;ZD+(H>CHe#Zg zoMG*TzhM|p^R)OcFFH2lTKMKl(VcSdtN9Z3tClew4CogBGR%^s zM_?2P2zm4C6f4WR)R~RJ>|2NMQO!1_xkO|WS!^BF*!g%$E~P5;nZng*9cG0Qnw(+4 zM^J{HGpW)OM;Jy|clb-Ibm#%Df~JBpJ--#K2L2a#7*)c$xHTB{oV(a%3_RPJg9n;e z?9RE24jK&d&lW!K;haH7e}mECA&Ft10K<^hbW$A_b8$IsDzG%KMeV)`qXId@`0+^i zLm0Mghg!DjwurGyLrP1RhHW>lq$3eX5c_GA#6H zGi^dU?>9voCN^V){!POK7)ilHaG%Ag+R@5GFg%2Fd~d%D$1EM|G7@wEig0T|qJcQ;laA~6PYFanU1H*@twxAp4IZYUClj6q74fA-dx-@4PPVX45-$(LUWI_@#)R^JHNk}SaNv1-2_>D?ttK*+wKVTB9`y;}Az3J4pF z(Byy^X`}*(WRog0QJdK}hsx5J3mp|%?c1L4g_nG;ei`kGtM_(tH{puV=G&{kev|Jz z?@qx5#ij(ABgA0dF^x6MO1u(ZbzX`Ngp+NPRE4&IlK|1Aef!V37N~H1>s{3Dwip%2 z0U;xW#3BR`POdpsrrS=qX}-3(SMzG@bfjak%&1y-?`MTi^D0U*AIY5>D#(?H_5(%E z3>sdzm6-1|MQG^E8xVS4jw)KUTtVal=WczAD6;g^jrj?nYGI1iK0(UT``1lZM>DV< z1%H5OrgzmK{DD4lfZ+p2;?N?;4&#phF{y(~3)O6bJfK2z2N?eI#b&f7BH6g{$_AR| zgM6kfcR=S9fvBu^UC}v3=ZSF8_jjJGi8|N+in@pJ2iG0b{~P{zemL=SV_YNQdw%KW z8C>7~%_$f)3kAsJSy_6 z<gkg03L(!Hp zWgnbRom{ekeXex2@sHy*%ejwj((oZV1|3OW$lsIToa1on;IC`gB=#bM((5n@rkH5*DJznGt<8Ys5RBc2T zn%H7f%7a05ekNF6^uUv z80PVITLg(=x$FNb!+>altLD+lFfhDK^9jl@XN)`kM}~n*fBQa$ve5T{w~9SsB`ReP=;-r2nP*0^+|^1E7Z9XKn=yZi>Zft4Yl@Ddhppj zjRe1@*rG7`e+)w`2|Z|Mm@MKL`5Qeb!?t0fnVexWWq!jjmg>b(OSj)1{k73a(p|1x zJkkB8m9^9RWv+o5laVLxhh5?~9-(5u+=7JHLoMFI6P==+NWZMZK3~NYsTFKmUSe`|R@^^b=i-S+M0T=+vJA-b^}mvWmya|ZLPiiM$ur-RwUjhak4Vhjk5Yewt!(1|+0 zFl3?*ypBt1`a8qyPn+Gpc8}ZL6{7+@kWxe~_l#xZm zF7EM-fk0AM*=R#vm>k_k!|SR*1wH;PsNHvCR3K04>wteDNqy^fbuT2TcaD7eX*lz| z-Lact1>K`v=LU-FWpeA2Yy-K(e{E*u;i6W^3p$(IVY2D2Z z48L4EjCOKgN3I9XP562#k~*miuK0C+)MOpAVY<>8)rc})9gimUJs5ZVk4YV9-E;U6 zn$$^Fc(VIG)uc{7(=N4hx~u?A>Oj<_q!+X%bue7{;Y&2ByG;03_gCp;QqOXJ-y8&z zy5>3JZ@%&IhOO)h`=v&LAkP)HTDpHp9kC?zpgpP2K^*f?;tg7uxMQN3yfD#n{7qp( zSG~AntK%*IEo*Hg0#{aFy5@ag?hl;#dwrc1hXUt5plhAOqNMAX%F1}CP2;=Eiu;0( z-)rN)1%1nK>2RVJ`!>RI|FbWN+ulKJAg558a$mW97sWB2MrX z@+EcaHBI1CH<&}RFoZT0^q!8Z;0ae@nLUp81@IWOOgV3o?ahM_t!(e^Xz{)GHng99 zCJ^_0$GgTCoVjw0d|?mSUuD*IMD1TQ>Za50x}eOaT($7gO_}~SLcUiWOCNryno)NJ zsZ41BL?bIxE;H%VrozU?Avy^}|9u!0*pZ*T!7;9>$Kl)Cr?#p=%K_lg`m;J{IRF9{ zoU;xrrI-OD^`1H335X<-X7+p$F}tnTt54;2RMIjTa`G$w9LkxfiItGPHzPt-#}E zo8>QNym{7F+YbYG^|zScEj%8gxJD&m6aNeosqWK9xi~(D3;XU*YLCrbuoQp(Zpem( zLJk$%pEz9Z3mOaKpEHQRH5Q+IXq7QJXci(fV6=ip(u#&HsDZl72N$As#{rDGVO&zCCB1xMtNe~?^7AGI>k5P{q8Z0&F1-G z7WgIK)0gd($D9L!6QHRWLqN0J2?83U4>WxG0jN;=a9jm%xC$s}fXASqxnL3($YFCa zvcUTGotB5hpSY3*7MoVT7`XDf@6xEZafdX!RNlhH`j1u+E4y*@BlEB2KOEN6(i)YR ztsv-8xNr1n+<>R|f1p9OrvuH`6Kgsg_p<&B8lZxw;`p|JJ{T3qOD7L~B)TD`(?;jq zK2wL2>}I^jSEeYKFQPNQR6{2o&A1Cc_pqIOWB!IfCZDW@+7YzQhixD9o+305i2@Tq zXxX#&5KKS;2!_u=hJ%g3Zz6xdIorjp6aWl~U6ur9f`;~PoGhK}rwxEmq5;uPraeT7 zb`awZ+KC38H3FB`(pZXikL{SSLPp)<$?h@5y}M5wNAiAt?wk!wy!yxp=+S!QYs0}W z4TOglMf7f=|MEW6NXQSNc^_IR0>^yaxfId`$c$VE8vKG6m!i#Pq-d5qAx+V2Mp|qm zFM;)^?a2fh&2orlvRmlU6z_|PX7Uv8?nouY7e#oUxa*^uc}0UicJm3f=N=nduD#G> zU#>MPajST+dLHXby=Q|_{+Gs>nuk96e%)5i!IoiGglm^nCh{vp5gT> zykIz+XZo0zJ@a;Y4LY`z85~3Q221iw>liHV&dTBS`(+q&hy7Cee<^;W4fsMIDaE^A zD5Fh<$9b0M?3h1B1@aUhSdWAoB*kxV)(|vxICX&7_WkAl1Gs?4_XsZoZbkemzR>Dk z@BR!w5HZXZIQ)6nc#1FjV{bCiSagce(3}lIN7zRY&DoiVT;NE!E z>>rH~OF|FYX#^aTWc3(QO+uqt9vQC2JBAi5ftY9}FIsjQ{-$Uds#b81*>ZCvQD?(U z?~LY`PkzW$ERP)cz`u%n26t=i+nR{`OB@{nPd@jHn$tDb%5w&s`cR{17!p;NZDg?n*K#2xC1}$1HnZ(7jAVj;OxNuC< zY~LIg^>*T?)Yu(r=4A#MIuCujSqm3b6cNnMjij#W(Y9+hj(X~7XvNHSLYZ?@y;`uI z1JCG(4OpTXiHyf>-|4NidBMA2K1#G;j0)sLi^3xpg%E9>lg36(k* zW)2l;uNdr-u$XVHcP}R>>2v2rm*N-4Sm=pS>bx69mT^g zY5yYf4{SS68_}ST@j%oV@15vl{M653K+7>OTxgvyT8_C*_;**J*JL?X!LY2x2Ta3N z@v-cr4nSSe2hm)d%>)AlVz^4zzdeZ%OF|DQ0HM)54{=Pa>-g?-M=;S$PPBuKzad&* zwL<0v%bR{%RMv^7JZ&?6)x%=R)3x+6bM9!@_qPkh%{JGE7I4W3Ci_cl=+(*zYO)#i zq!0fodM*7WJwdPSkUl+gx656x`uH;X)l6hW^8$@%v`)j>YSOTC4^a2YeRGs(AsBVZ zi57XD3elcKtc$zpm1CB3FDmlG++v}y&KWo7&zAbB9l_y)3tZ|wk}FVs2WNP~K=c?> zlDv?;hxA?TJ=&dG-}7fqA=;uQb4t%$mc#!U2S5e=uj49&!c{QV{yJ2ym%41={+Rpixn(+l^R2(Ov4e`T!YWuEQjQNtol5fVW^+Ltjj8{kVh-gTs)?J2MdZQVE3cP z$2ZBog~$cYqsDeqAnK+-H${yWHf@_40P9h}7l_tia|i-osyRU7*$7IsFpN9?N1_2q z26o*>iAFNov+LI>h{iQ@K?EplnQE$Hh5pH)gCT!?%`;!>L|b)2`RE+EX2Lqhyim&z|A>ZI5_-^1wE2i*>dd+63y|v^ z!$dPV(T)ZGhG;$23gve!ZutIMv{sy@ul;2W!}oz&Cx_HuD}L}i(F=2EolE?fc|Y~c zozcwI{egkK`oyO#%spdFhkTa#G|VbFdR<-QOscpbvHfM>1+M>yRy#qoUOZGu%n!Ra z*8Dk_0(F-jw?m0`9HTBd(UJ|R5bbHil5^Mh=S1D>i{vuu`(@x|@xtWn$&PQf-hA@&+${5D3SqS@jtqVN znT?xrv65>S|uY;3P}j{>Ho@{k;+v!@NF(0UJu`j-7UO2^$3;o!*lbdrwtl`EJ% zK)ts`=93q7Iu5+C_+j&)iLj7Yb2fw0zs(yEOF|FY>9_!KOlt5DI)fR7iDq&-o{{+t z9lNR(I-ak;KKx+OO0iC*ZkGndih>LNXJUqo+z zR1jS~Y$MpU{%ani?QnO9*sO1><%3G~maQ8ZU4*4$wJ~jUjHIXGi|at$ceiJubd1KR zOHRidja2AZ60zjc)qOb~_mp3pm|5cC>2SHIGR#Qb()fIE@z1;^uAP~g&3SdK@_q_k zT@Htz9E{R1%X61w7tfVTNI9mlfeNzC<0_nltANrGcnnI% z3=^+=M1+oE?F;vBR_f%j@8kP6C!Qd8;?dDzIqx0rTbS=0E$QZ2Xz5#P)+uGdr_pJt zOkk=SF>$Kd+~E@9|6=z*SPU5*$J;I@=$Ify!we=+VeS3#8O&1{70BuM*dB>)2pv~C zX!M!d#qIc3UipdVWLat6hLTHitrSw}k}+*7&%k;VU;v_>k6w!sjmC(| zuFgV<7K3pI?L>nj3P|E{UlU!EBN^@3EoF*ELK3196BZYfj5d8=1%x6Bh)VZy6}l$3 zXTrZ`pN%Iw1MMXbZznSA<8Tbu6Mi!#*?~I{ukxyqpz19kIJ@~Dq7X|$4=AFbPes5n z+t!<+YjQM1^Fm~J(t8fH-iyUVGda;R3V%bi_G(3i52n|K56xL2CgL%`@oisr=I)f* z}rklwzLHn56)T~wlo~~o-6q1l@VM=>weuN z+%Ky(a-uB++dlc z-UB1rNXjDGRJgfnDXPM0xC$uIfXARjyJE8O@hCzxpX#(EhLHPrdRO#lwQ9@aEv@$@ zjIpN+zGP;o7u)G^K<%+%)XEn7P>Ld*3|U_SXH(YkM}nIbe0i2@TKbc*5~Aeevx5DdT4;*0|DE^<9^erdBc zO#oD%HULt+DL$X7K#7)saR=>0gJvV3^_z|3UqZl5u`x3>8|OaPM*&DO8uu}I3ILM6 z`z|b`Rz?D#;|CWX7Oad@fNu??d0iZr-^~qV(Yqb2%n>HNU z4FuL}i&*UX|23r`mV_Qq079dA5#pHIS@iUv#y^9JX0q?xzQ}b%h!#rrMJTTwCaz^| zD@)s2OERqsl%M6?Nqf5_n!V$pq&C~N7=P~JOFgSyx%qB4`sc%qNjRP?%m3W$6kK#er@j__!4U_w4rk}?F-$K zU5uGgU88od76qEnk1i1IO;ucnAMw;<n0X0S_!p>q; zAV=7zRK&Xwgc;dA*l1}J&v1x=uDMrA$X#LX));+*%XCZvy^7Id7JfC2BgGruFZKfn zLtZ2PBf=h(zn&sAlu5wu#tH5yld=)Hz`0|I8wHa{i*V#cS@g6q30RMUwm>vX)^G@I zsooSj6-!Zsox`|;c7#C_J&|-GnggxWv%0BO*US84{ z%~i7Y4#)Oa&rZg@`6%VIMmyJ;aQn>IU3K+b!V)*_DqIi0_Y0l6Pp$d;1iIMY6m60u zA>70gkYH!Eh-H#SrkKORp5Ne3}k&6 zA6GWi1|FAOi#%kE5=ZekRj?lh`b7V(DZ5-kMp>AMzuj-N%hm29YMEC9)^N4H45mxj^go(-1@bm+24fg?O zG|`{O2u*vU2fs*x9B}FHx8fnlfx;9FuaZ57!t@?;J#fxob$}+A`b-;4p_vkhc6^T{ zt(g)Gf4f@}&6Fg~22%wos<78~$w#o6Qe@gRgTfhz>Qya|!r61e3CAzao`f?#KXDw+ zLPH!YsKYr#XZI;ft42a**7ZYm>i^)3SQ2_b;S5dj3W#IQrd&(gP#H7>w*j|n`R0iCx|wl2^L{>gHCx*BNI=*qTVgC@Z9U&XgJXJ3Zz>G( z&6Ichwc-0NOEyM&XfOmBEMolVO+wgs6*hX;?hgpFk3IG0!!>z(N`UQ`$I6L=d0NEdim~qu9WSyD=gBrH z?2PvF${gb#xihll#Rg8Z-1BZ5){;^k;eeFg@iE&ouDf<6Od zCE7vHOwDMCCZAEhk*v@mQcJ-%QYDajckW~*aBJJf*40V|bs4d>efuLAN2K%75QK93qy49<<|75pj(7 zX909l@(L!J$#MAAkU9>dtc&ihib_v?b22c|K2@5z_s)(up=8cHQBxV5t;<6_-LZht z=UHqwrDi{SBw5B)HOxy3n2!;=#;tP6PV2%+Wy0 zp{p4E$Z#B2`aj^uyr-jMRRCV!LMLkg==sXP#gjmL2-D) zMCD5lf|N;5Iw}{D3!LW(4O47ZNMcgr&Y7vCd6|XNmW_e+C};~ri*2BX&=v`6`0#r? zw^4-Mz_{aoL>Q1{>*K{}Gb71pT9@Q0ni)w5tFn0boMbfDX#+VFVL;TCh^TakiRh+; z0~BFkc+O6JwD%FjEI1!c(wOXhlsY=9fFf+6{<_~FY+cdeZ9+d92wt9?_fv!aAq=r3 z^q?JKOAtrc&-jK`bT={4OpdVrM(PMVYF(^6|7cq32G0YM=0Afj6!C(>ylZA<;DxMU=3U&3 z4|@-57I413%VkfrmevV;)BD=@jjq&@?IV66gY#JGD$2t^$fM;36o(Zkh=G>_QNx?}d9aw>;8d2J^g^cg@_)RXf8!&-B|a_VVXZZOW31fyIo1VG;Ll|O7 z=s`Qeln}>Ar+1(TyMu{lGK9Tk4yKMUZ|f(@?B3^6H!N|z=&4Pw@9EaW^z_&;aUi$$ zY}K-pck_4nGcV~BaPwH5ue~<3v-x{jWCgvhzEm0WilZ+Rc+))n*A4Ya=8z$5**lBT zKO$_t{iZ2-2zs{y{TvT!qX@f;(T^Nq;)YZZ_7XwZX}1SFrNi_hiAAdi6Hd;P-+<$h z@~m7*?_2PwpSdCC?WSnLv%}nn*z>N>|BB;t#Rn3YG_+^%O-@Q8RhYFY>}|H1%im~I zVfeZ(szNqg1%NQrMNougnh1`yBM4L96n(itp1#e+vEs;q=1vDXLn-mWLGF=&p?>DK z72M7GW@B_0gTC;|CC@Z?d-dq2xxB0N4R1>f+xkiD9$^&jezt^!FlA7Mfw#1cMJ!+P z(58akOI;LU_b@7uBWyuF63!5Wscn6@(Q$6kzAVMc#kSG#z8@f4wfNE8SIp|fc(AEdN_A`A?#o6nCT>;ZB;aK5XN zjV6Tcnzo{i>P=x1SBoMn2jdRf5e6-7fYyuCs~}upre{NdqF`w(z+Ts z@uH`Vc+e^gXvTWK9z~efgnu`k`Z$R&shA!m6k#t!*?zOGW>S-0DW~2@csI1*NQ~(} zgdvuM9<(EDDdL#)XdZMTF&7ifnC@fxtvLP!)G_|R(Nn-;Pb#DiRgW~ zUwKs(_{GM|f>Q*3?FbXG!`np@T6Pnc?UFv7+Lgxec@y1y*)B)DJ!8cThk05`NC-n( zLr{d({h_ODxAtRMf3kdj0)uWU&|he1xWs1x>YL<$^X$Qv)o1x zzZ7xR%al7*&^nhTWd?3XUCAo_=k{YQu^Rpnw9dlq=yIH5cOVi){s(s7Nk>O({s+T} z&sfp?{}8zzI6r=qa?%b~!fg7x`5%a8`Z58{|9Kd9{Ezt`TzWs(ds_297=F0tJ(~YX zMoauqNmKs6`*-s{5H%^GAI<*>!s!_Mf3kk``_jN-R_4}7a{q7*`4<# zQUCHkVo3Nwd;V8O9Fx;Fik2{sFwsn&|K}7^&;NGTPnTY`i%VHwxi0JE$HVx@g=fS2 zFMUsXml7jwR^WEre8wg2OZv06+3!nd2w2XQUN@9UY>?MG>p8;XC#YAUeLeUd4|^5^ zdH(Nz`{xoyz2VlBJOoXiK)*y!c{Kk&#^^_0!e|ClDPhXPZ&jVz@gTE0utUZ=V$jd? zWKI07p84wgiSO!SAD7~W+2R}H0*`(VSWN6c%kkbhYN0jm*mmOX_>o2;G7I+%dF2=^ zB*AClW`1s>O@-pLd8i5mxC$u3fQz6AyS1X9yUlL4sg%5j>81BC%p(;g=8Y^E4v6<( zU2@ILkL}J3Q{=-U43!0btC=_EbH1<8FqHgu1E;d2?W$ykVnM6;(Mm@)C40GGzW+*? zjmWkfND1>q#6qluHWi|!$6q`4H>s0;uXV2f*e!7@yZqdiUx3x!44=i zpmi#6%#pk+v_7YR;ms!3(dQIWG%L>Wq3JoreA=ECvZNTJ zA2|+J$xy*zMfl08NLMYMcSVabjJ+bCN{B12afr?Nd7wyg`&@rh(>|H}d9OWw7#_~w z%o)Y1r0DTriFTAaXSCvy-FD;g`6}$b{+Ax|cI#$6xMvoj}VSg84K(tlIt5AfMVBA4F!l0QFTv|216p|^S z!~?@ai{7Ke^O-POoBP{I;^jM*6YUY=<>UOsf3wmPROV(-KeLgLAt$BlrZA(z>Pxbq|bFvb69TmvN^-|8+;;AkNO_t=aE4+^Bu! z^XJS1G#U(%fMg!32DtTlA!8h(Y#GJ?tD zMPsTe;OG{iDm;U$fHDsF5X!haEBfcQAdKVb2prIi%`WWY{azXLfw@A#<51Jq(8kkW z7Qd!5^&WgWc4f$y)2ruh%G)avxG|r|y$sT<+?@4Q+ZjTS3wh$L{lQz!=-Xh*pC(E^ zW04cY8?>pg?93v@qlg!tV^kn#+)jOD0EBT1HfyU`S|tk~TVQ-#;Nm)$%+U~k`r-l( zKKG}E#N;N!7svzH+j+5^VAgVukb!?c9=XnMj_rD}Q-lV$IK^!aBJI@%BNSSXkpbYm zEP4h7v`Am;BJZS#_`A>oqUoC-L!tEoV_({##Z1i#XSLJOWJj_>GX4}rvV%1PUZ1w- zbto`_6&Cqtpujvh;V`b+^OL~DHKiz{-F@%GT;@@4jflTC{FMo5jhOwxH|EX1)(FIq z@B<1==z|Y%jBv(%$b%0w;i)0RuinX{1JHVjiDq(WS)8X1t#G9aJ_lQ51x|-8*!o;J z-frl2yz#u3X1LvR(yNBA=C=vSHw zt*vW&@o*d9aKeqMQ&I_4jke}+qLtPwj7H=X+TlorLa#YITaItn%B(f%&Q3QTw)R%5 ztIL1$R>-&V_61Y=ts!hyVd^_mvRQPSoCo*0H58h?6VpzTVE!B}cDVydieTcAI)$zk zE~8C_&#qjk3gvJWPznL3Kq+*0#k<7~2!&Yc70MXfpB-X7z<+bu#R&aje*OYA!+3xH z5hh|Yu}7*wzmCb3kwr>T#>#->1>^m@JfAA`iS?_?g6xGyd93>Tdmt35%u1k5g_CtW zXr)zwQGuL72kQ}MLnt(FllDeSs}#l{CQWYh?`?UbyHf&i&N#YAAJ*E=FE@$~jP*4M z51mCRgiJjCkwTtyQ;T#U5(S08&9H1(2lBiFg%%jj5xN|O76G{)IG=xAodQ~01>?`4 zoBhu3nQVBsn6`9H^`;=sbwQz3iE#(*(1Io$pmj*2DXnecV0bc@1KJjTXu@ck0$V5B z!g=?xqYVlAb8c<_4LB@Qcsnl~ZX`JAZpphU`mfSLED1fJ;DA1S0LRRVaYEljAVo8a z)k=yt5lEGmeq8f2(whjafAd=Z&JU^)MNm@WuS}{y_ zR{(R~V#}GAS(oyA)(T!fys6abfkQa^;pe>E$D-~w5OoVj=ilVn$hyi@;>(?3HqY-> zr1+Q_+-PtTZ?Pf2hce!oU1hm$1tJYa_ z!Ct$1*?5dL6tAIiaI0Xu=>=o}+>+EK)CB7siNv;Z1NNUoEWl`iSc-nJ~ zz&*CAd1jf`Sb$6W5kie>;1g%DlO7)8^+GT3EjMSLH`&h@Sm+k0d8X-3=1Fvy-tp3@ zYO-`%u_l~06?9w28$PNqDv(3#7#_LTAkdPt%8RqznCf|`PUF0u%b+0~~Yf4FlxC2O7;9$Z-2r zjVQFLG0{v8t$mHup><(l+Q87KS^PW|mQXvA5ymu+a4Nh$c=h0Zg$BIK66qG7?p%tSTBW7Hi=upwI$NfkNxv zig(LD*p~TQ$x3SyT2g(RxoK0OaTga_X}!m&Kn|@lQb@c* zpe1XWSA{?;`KX44&{yAguAS>H95K3N%pFs~B^7lbd!&35-QH!Q721fwk#~iS|JbFz zBEyh2p;yh{g|>KnzzF@D77r)@K^Qh}9$)Y(Ky(D>FRxKw@M5}VJh?-C-n8X%pcDlF zfoNH{<&XlCYIM@Ct3v_!5o4wQ5r9CFiVHtb0FsQh-2E#B05j@4(M|N$+SxVLxO3_ZXv6ME^N}H`kr7SH-7(}#4rH~fdRf-nv zHMS@cl@f}v6hhf|=6BC!yzg9R-tOn|e(3kdxBGa^`~AM=%zeMM`+A<&Ip>}m{H+c$ zq6Ot8XrAuai;hfs4}`jHGH@R@W->s{!5)iWWjo8(yZgL7;P4}GSLR`5RnL^h=Vx2o;z zCduKm7|_UKsR#&uZ7et?49t4@t9ItrQDqNqJac`t zPb+8pizOn3w2B?Hb3I*Fx;4w}{Hdh7q9Xg^bR-i70GJB^xCB2_8t?K4e3?b~4vGajDN&AE#kUY7WHZ)rCat!eK8 z07PCoHY@=1a#%;Hq0j=MlXag99Xm`#kWYx5Usn2 zizu`}^U*W=H_Ir)xq~>gpd2Z_U30`D*0v*v}+PHAa)*&NW$tpBoP#Ien z+{<7xZqJC>7z!<**&fqaw8--r2nTD0bAzLzl-W)fvk+)ek~uERu!q*>?Z-&#X>?lN zCiQcErnQF5(oJ{BHQOLm$K`>e0tb&FD)wyw)k=4*yw zY`kyPl2KfCYbsK-qj+{SaLH1Ht4k(QJ>UJ1p(Wct4ri&_lF(<1WuFijEd_ShrYWO# z|Bh1u3$4d?h-o0uQe34!%VxzT&$#+%#VbOFqnfqNj`!7U@4m89d|xuJl!5vWVUduz zV$pGa%9s5&)s+xTr%&0GdLZZ?c_)GA<2i0Di7{OL55j7>scvx3MlK zJ5iszyhJOlA2@dqhZZ!o1FdiOgroI{?|>CLWWxTbNA{n6S5HRj5y5$5szoVC3+SY$@%f+{s_B+Ww|?sF9FAGSS77MWEdS;26e>2($!dhUXJo>W~uH`GZ-74KWxhLMMWXUc}wsTWzlnUAzAg!f6eLd-oL{Il5@kYM+3J9_Qw05T< z(2{0_7Fpp1F`>Nf7&InWa8`I z)KLcmgs1j>>`UvYax75u)mgo7VO8~k%jx9uan zkIFY`>VCX#VPRoVaE+6$Bw(e{53S*!CNqd&okap74WSUY8Dw>9i9P`anisj(q0?#U z$o0Uv`kwEE3bUvYD;n6|2D?qqpwOztxq~>gpa}6{&@ToL@#O5HOI0m8yois;rOdas=p=k>TaV&QAx6m zoV2el_sm8uVY-(1F%?&nCq+52`xiaS^SI1)Mds}xMXg!Snm7OUwv=*@KnSFa6)%h?Ims2s& z!c|(`tA7v^`pV7aD71dzgvLS(I0Xu=r)Ck(9}#F}TK{^^H8Yd9*Q$s5X~u2;x4RQI z*@o0?4tP7K-Cy+mVakU!T(^6B_#9i?4K37sGN;OablvmWXweg>2oi;UW!%Rs+r*#GwVH5HM1bd;hjfOrx!n3m-zE-Rdjc z0EIdqxej*d12>P8fip>x0&1q~GjTnSho2}{R)vd!04=%Et!IFodcPr2@(qb(NttOm)SZIAt zg+t3hW5KL4PX=cO?-g6geSKZ$+`e=O(#T!HLL6L{T&q)d-tJD%{2gj|a=PyVXP$;d zyfWIOAC_W<2XU2_WfOtjbAix{Pp?Ix)r=Dw3oYOjD72oLMYzAW8#m>V@ttEn=eqO7 zs#VWM$;@z$eqnB`mwsJHT`sb13?sYE(5bLbvP(2v+f=1wZDMDuZOwDuI8RFG=723B zfbH9fP6aHqswhYRL!c$ILjRM^vMYYVJ%+bsD(UiEQLo~t z`Miv!1z$#m(It~-DT^Glob*M$ztTz>b|s?tvQ}b3uRgX7UDVNv6Z&5kb%2&br~-Vr zmjR;+l$W5n#oh5JFEfzqfpdv#V+iq5`_EQwK(zU1{CK7yVz%L|L>z!ns(?#h7<(8p zKL(`=Xr5_x2Ay;Q_XRi~<$r2$(#dc3oU%$Xh2$3_6u6l^RZ`qT#;x4;l{RZ^;O4{e zf2o345_&+XLPnZGgWqLbG&*s~jOK-!K|>}kOSXBtf{sj@%-6w=%=3*S)scY$!Vrm> z35nlP0Jh_z84JMom)HZa%I4j)nu5(|EfX8V&8bDHq2y?()GUb*)#}qtQ##b|p4zA` zWZ_s4MqO`_re!VUD)=fgajK`^`%LahLaOpEQen@7#T#3ssYtQ~^JA?D0NYm94-3Hd zJf7im8qhDVSd1$e@lyv*KP&)$IKTn81p(muJ&g7#4Nkx4r59h7HJY@iKP@|#x~z3; zWPU9*{@uYQJ$VyZ<0)l)Hk>C^fFxB?6{;;Ob2b>oN$Jw;4{3p|M3i4z(N zK;RT80H2#h9Lzxgn4sb7Aj{owev?zyopoYQCT_h`Yd1F8c2{}Tn3g1~jP*7=(H;Bw zE0iKnS}SjvK~~+^7T5lDvLAOR$@EiGyiAVMUI+l4CQK%#g6v}n)b3q46|ewoqa!*( z061#7!6=($SL@X9N3!EQSXd5zi2z!co<%m%TSZ_#*0iqRE5MI;)np4iip;cQq&K<;| zMP?_-7SMR^r?)CDF|OlC5_7z*z@&p*W#P3mpPuVITGwoiV&>WwR-_Hf}$-{R1| zL={2AWgh=ABLb~|hxhM!!?c2QcJ`2QthXBHw}Xs`U7PJ``$sb(5Vbe|8_|pinkTh? zL&vcWAl5?tODP&0$MVbC>TrQfA(7+*4w2bs#2%}Z*&ox3X)b}H@{jZXW<+M(;r~rM zBN`!&DWAtcR}64~+Y)nNyYUU>?7Q&=Ho12X(KbZqX_ef1IYV!f0JoGtixQ_dH}7%- z+IS(ejmMt1a(ygM%{??G)ny64Q*K+0tH@oyNrT|eyP zi_ZRSEO`iew*vi^($}NOlM|;OCga=jT!Lr(9g|=0P1(VS=`dCjX}>jRLwsXlSKFhA z?kOS)#-5(blQp7xEiGD~nO!n72ua<)Lr+H5b?q}-OvWd(t{6BLA4p8-X`vg?aV##J z(AaJ~a0;{=|I*Ao;tkS`k7)VD|Mk;TBf-P6t@QorDdcr@ZT#W!Oi@v% zuXjnbz&!1#HKjh*eSQLBC$&uSY+4+>IRi=B9bA`^u-*6#5i3@;o5(sj6Bs`Pc6UPx z8$eoaoC=t3e2%md(f|T2QOlQmZI)f<4HYz)d+SAwss_m<%`g2pmsp)bKJ`gTs%dkB zdi9uh1CuYqk7GTvBCz2e5W4g&;SKjd^Vu5x&mX-()Bxw_1&2O=Wb*O#d62Gk7Z+V% zt|J<`=a1Ol2Dz8sq0r*Nxq~>gpnYSYwe{2-2noo*_sGY^Ri9HB`S+e}J2&ft<~u@i z&|W{&XcpHphcHg$m+V$xH0*8zbsI*CanNo&5VcdP1cg@MfPWiK77juyFW^Mm0WyW; zmwLeOU!cV!D-g9}Z87bg^Q~h_zy3lCu_W{$4lQHEF-`~eaot0%L&ilj23qe(0qmib zX_I$%Tf&-XOR@LbcS3tg9tSsyyv_3pY>~ce#JAr&U~ym+zy1E|9bqx4eWG6Nb(eE5 zHuM#mO3rKLrii8F`o16~{oqXSWr5b`@54eXSW<2H{15a?cvi&)k|!@tKPzenZX$;)la7LMnwKhpR~ z^wsUserWAZ34Lru9>wAzy8t(0LURaKqhn8eIH57nLY)GI7R_w+g-irm$I3Q*yRK^? z8KX?myu5$Wlh3%8ye?9v)M6o zU)h@~?%)f#qyB-`zEkNmdHvQR33y3C|b{&P50L~r%5nABVAr{FHXhBH; znkR2hK}iraV6=&so(+;9c~x|q6&WFcZr0pv5em`*dX!F#rkjTp(*n6?>dvY7O9H5o zkUv660PRfx$CT=4puGuZG_U@3V@PkpBH`3#(3{vUb@DB<3ub9EQoRXi7t90^)tIyY z)rEq%XvRWIOoctPXg2Sssb61pCO0qlm2Y+LDKlXa!SUfumn&zBnylcc?bYpa?oDq0 zrR4JPWJ=;_vU7{trr*EGw0zR)^ap1b! zC2Ca7Nu~2&33Qbeik8(%Rs089tNU1?CH_rm_>2#Pei%6!t+a%3LSvx?oC1Ybvf1i8 zX?9nHeKUIRrd;K!-IY{jw?!}S!nOKsW|6sTj`5Fm{4Hsg@G;qCPk@SB4G z4}J~$85(x$rGKSpaEhAnhc9z^|5-7>d|9E9tbd#ZF%_zwOh)Z4f>Qwttyy-+odtoG zpoPIF1X^W%$x0^47UKCLjhRJy64ni7m8v#Mi{%eo`_SDYuNT(SKRieqcEy0c#zA62 zKj8C1R}6^ag#KSw3;-W4Qgb2-K+xRP+X)3=HewQRe&Y8gLIC{3LybVRn_H7m0E*$P zL>z!nqJc|Sz1fcv?cjhFoHy(qBpTiQ+^d;n3W+33y?vcM(Q15eT)wfjnD+2YRrj-> ze~E@z5_&+11}&$7V{V5ALwY7;BflVjBSfPlX%4 zV*>Epp9P>PB5J&_Cc0uk92d=404k=k2jC-{57YP~Ezaa7W~}&a`qF&wsanZCyY2M+ zL(cjaI@UkdaJ6h2x32m7@0{zs%g(p7^l)e=m#$7bZ7k5>Sfr3JhSPV7XGiGE0RVFF zFac-@_1fSc!|EG`&uKuv@O978L@a^R4-3GV0dN3%PF|37(U#Qf+|x|UEHO--vR9Yn zpW;Z#N$I|0SkLHdnAP`PxkrS0@8QcVf4`Q}DksHrJTwdd+gJe@piEF`%7;`GfRZ?& zu>b^4fdVkaZ1uww1c2`GJA>9sX=jkviJVh6U2Wmc5V-1jM1D_Mt$6)Smz!ENf$W~J zJRT`KWINVag9CzR+fPT9me{Y?;<E~SmyVsRtPX!PaRwviV!>UN}5r95>YS1q!BO|)wJzLIr_ z7h1J}U@3efK@?h``K1sU6k5nW>?Ck5xmb7zv|#gN5B_Y`21L6Sd;^8nD4aWpLkpT4 z!KEM9i4tAr2Accb5JRWo!B7e~Kc6QwxXdltTW*>v8L8S_wrr!ZXB>Ui;V1KrztYUF zzmJ;w@Gs-QZ3H`@jDtc8I6_!e3SH*LjOH~)y+feItlDgvZw^9>d&K5BIT58myt43h z9wMq;bTK+VCXI_`EVMKo*hA~O&4$h~2>I+UkcvagI*GhOeeX0u;39$MdqftKe9q2ZG! z&@U*X8(k40gVPTSEmIXZw7e(1{&Zp;sj>chi{%*8jIc}X^SPcTbB^J1YnX19EMhd> z+e;4FI6=vw$^OVLAFq5)Q$Jp(81~RwWlW&d1VZQ5baR|Wyd{eh8VfDp6ezS(&1S!( zA<)`#>b&)}K;dg=7_+|B8GaQrv%7g<&B5ZcFSTFPQ6616%yDh~!9D@mr0T8l)LWg@ zxrezgx=IPt^H&}SFc{;@=lWm}TC+b53^kg_s&5x0W_Q)4f+)1)a4KM-wK5jb5dtk9 zbK0ye<~Mu%TGf{H&}Rp^RXyvVIh6Mvww%~)`8D^2KsRspGg!N7pcKkMz65?4dtf443SoDg9l3oSpnL_Qsh`(?@RjXtzVBrJ-bJHv?jQ~= zXtD#6{Oqtnd+1D~opLZ3lI$+s{*6v}VSDJwBbMx-MIjJ%roTN}6ow4=x0Gf*SQI8+ zm_7pq=I!i)U+jUo;_#{m0zbaeYRfwLmWBQWCSpnG0R<+s;{zNMvS|yXD1=7ye5Cmd zuN!D`k;g?dHmR>jg(r1CB&i>=BK>fG$Dn_?<+ed<-$mZ$>~kGDQ!1a%IjL#5cY2Wd z!%>M}BxfWqaqr@289PCHR}*IbXf>`9Is5qmOLAc=wD&|AbpB{8PH1dWH$;97P3mc8 zv){cylDf@);mLMQ#s}(eejA@K*+E@y`I0gn&hwmC$UA?2io8%nG9J&db6VXRmq&9I z9$nkN%~brZ(t;&^OT0N8N;p@?3dsx(#bJ{AB7;}NR4DY7L+6haa4KMv`X&kz50IqJ zWp224i}|f>>ABC|d#m-8L$8qraUxYG_^%D|H+ijJNqV+k<#L0TCNfM^=t))>?fW|`bNuD zzEM4rY->a` zshc6q6DRkfm5359nz5D0(o5_skuaNltwSCrQMqCl`|LK@`TNh{G~?I*Xm&O7NPN)Q z2Z#72ghkg?^SEnC)=;!dNGCc3o5B+J7!x+`dRyPwMS?F1+M} z$IstVa*N_Nxu!h6LJHL5JY1D_R8ja7`G|gp)WnjThL}p^7whoZSj)Y{gr2J3|FU}( zoX}Wk0jEHrm2Nis^AiMGtL^j{W9%O}am?f1;nq_vS8MXyQ0}Guw!nPlnjKbWX1y$N z-d|!v;&SCFH%dFk8}&=!4axf!Exn#vP+0-x$MF1t9V_PLfe-!bfS zLFbQzrS*N3q3J?H_&F*KSNAdab6_z8v9cw97Np=D#t9$LXR`H9;c z4WpLkMTL@HC>w<(D{d>FzxZgx(x1Qb%hdwOd^}uPdV;QglRu<={UWN|T-jxLQNZ|| zRNEJ7eGqA-6vsF(?rookKx+Y1x4}QQY=|B{c>?|9t_z^aa{^93EVOnb)`2}xba2wb zQEuiOqUV%Xb2jhu?MS0mNd{G(EEz8*T%J7pyFz^Cj;qt&H`{z0UQNuhQ*F_H? z@HnCfFI5N4C5nwu0Olbk0q1Kv=MVy5@}C7D5H0jgJUV|g31=nZ03;(yf*-SV1Hh%d zUeZvaF|9zVel~(L_v z{$l z?n*MrW%xDp-dkcoPB;<%(q?YsCm{(cO=TOkB7w2xArpYY=%R<=tF|8>x`)qcK)*OY z9W)VZ;Pk@+FyIm#fQJzPZd=F^s3UOVloBSP9-qrj%zD9G&Ef3_E zJRY~v?!+AGrHj9W7!N}gnE*UJ1b}yQ+ldM7|40V~;1rzDSO5a2KmnLxCjBc30ieP3 z_1FB&jAm3{`Z%ZkRts+^WA^CMq`G$F-?=}csG&7+S&D*p9Qebkj)2v`tA_^@{ zoI8j^3rYeY$?*?2AS58Olcc=jCdxRbjir393}GCzYQwIGZW*y%6`;@pnr$wnq0l-y z5Dp3gPX?i7XC7i~L8g#Mf)S##*+a{9k)3#B_gC7s!c$pk!GEEJSQ2^=ht?v*F+5@- z=t?~;Tr^{$UWSZjZ-_xQ}~ng`aZn;S{cYjfBC3a!}p1h&Zm`pN8Hj6!P~ zPCqQPP8!3Zbxi%uEc##79aPm3J%QGG1NM;BLbA5&WVAo<|*fT>#i)34_j z&{pvEXo>Jk%c|U^v4_@E3x1a52~9CTX!luLP-soZ35|soa0(P!nP$>$_Yr7K6@R;L z^y7yL-fOyIo<8y-*^3p)rZnEYFh~FP_bJyy^lg$9_v@&4ON%yXysSFpt(HaBvL3yl zZ|b*>x=mwiwY;_u4mI{yZ2($I{P*jLsql5x7S!%Da4KM-btxGMU`@P7-FZ5+=^x}qz=<`*hq=$#r__ecGgocs;*!|p= zJjkRI&;Wc`7zdj3`{$w4X&;a;!FhB-_K@i`*ifV2h%t_BL><%khC*v5&K>^|T0raL zsbvsoK}i6b%XR%iNf0_}WI0UUFALL=InU`F%lxZ)wb3Fd}$>MNvknPb3#4kJ}DL!)^yB5IL) z|4tUOaM6r~R;UVlXgS&xXx5lbi(IPZr(0H1aj<(&TXB8l1Kq9D_;NBg`z3L2CvzxS zjty7Zv)(uBWmn;W1{2X;Z(dZ0@V*cKdVxgK=-t@b>Nq&mNVL+@mg8l~Lu~aUtDZuW z=WLvQSZG}=hePY6`kPg2j5v2vzezp}PT<}6Rls`kUA2w1ndY_0ig)IfWIycPTH7`@ zGRW$}D6;3v)=D+nu^uXWX#JW@fV99VhqF(i(3*o28VfDp6ezS_&u{BXuve|0z^So> zM{}{jaUm75&wJ9cnVh~8uI6|IO%kM5q_hVdF(v9v<#V5Nl)HKNe+on}MK?#bq-QM+s7RKP;(o*i&Tkg|8n!i1x{ zM=jkZ7P0l>uEGu*|BW#`p?b4blf#d27}EvMNG4|tEb15@TAd$RcVq=3fe#CXfZfF} zcAymch z;-VP~t#hgDp=E1Rknm>Cl!$j?zHgf-+=~zMpW@8O(>T6{Z>y`^+13U!U7$76PxX=A z(O<@{RDRd9Z+6>l^fJ0y%=tc#Bh@AJg75a!O&lJ$7^7s1Fw>|Q!wHMFoxbnV2SbJ6l zsbFT1g+F7SO!rP(7HIilU8McXd-#kGg!VkV426~+PG~H&fK%Ym%4iq4ZGX95u5zXV zEsok=k@sd7rShNx`RKU7L;QMm@`XE8Rwa5D#WdKGCTNxiUk__kVq6G3G=&~odf=sz z(sAzlQ)=JD+^zhl(i$8dR0>NX6H~!Zq5s_%`ZyJ^&_XnTLaTW}a?+NCiTl1ZDAv-yz>WKQ=tH)VD~n2o0^YfZgM_TR}(wGyoqK z#)0OGldRCu#C+sSaPB_Yg6(J`mT^b_Y^4Q6Gj%OTp=E$`$A5$txO5Az0s1Tr(+ZTY zIzzUMUU>7W3xF0+gWV@4v^I^{8d}TJa9- zp=Dw7CE<{!dW3o2w)N$g#!F~lY@DgYd!>D0*c+7{4qLMYBcudGqtwe{uSWDq@9N-N zqH-f)rLxb~eZBAFC;U3Tt}pe6ji8$|E3{gNh1R7f_AGgb6EVe*!~*oAG$^3ZqTuwy zLMufD4y`BzT1Im?rSmnD3O#+rIdUa!^te_>Pn_{~)F{st(G2Uk2g-y*`xJldq@Ho` zFXJBVnCIBZoYOKw=Cpo{^255wQZ<&C(DsKEQD_HZu zj6*=g9*JiQ%x?`sOZ%TnOY}XJmwPR@9Z03sXqMc%W#L^%N>;h&dB;fp3qAObfVplcB|5*S6(X{e$zTZwBQsgm5w5)$U`@-a9!-yFTIw}f8eJ)vu zR&B=z{Hs0T<6zb1I)2VvO8`Kz^XLBM7!vZib6%N32YHv>K%-}q7TyBDs)%W)IOU}rddGFo3=I3<&Y^4b<{xNP0 z#r-|)RguyP>p6V)Fj}J`^Pl}Q)c6<=z#nbbi3x2rb`lCeGn~*^00O5#0hpChpl|~L zAjiGkX`7Lj{7TF`6k4AVHNd&hH$Aq{!d7kc5zD_wObkT@5KWZxH43eTICuObm-RMj2iOG)3Hl0KxIPMd6VM06B&C5JMnS+j8d>|KckMLwH% zx0-5xS|&QH<@xW-=Az$?AugSBlXq0@N471-*!@rEp8Mk2Hm?FM; zd@5(DN2Pu3b?HfrphyoKw7{YciL^#yD#ZHt+uaWw(5jtpw947+Ucfj~ zy0^)*m%@2v^ap-pHj#FO7#52@+3{RHn(MwPSLnc^4*g-PHiHOB7NLPi;KNb{fMBt9 z>_O+p3Xm_s`Pd2VgyzRaZtM`sS0I|`1;X=Vpn2_`Dim5vaqb`vEiyZ!Z8~-gr4Z9- z^3y$rP>8vFUSm`XH80JJ}$dp=b3MT8Jf~2XSawB8~~R;6R5OEpX9{g;udK zduVCe6eaNUjyY|vwM^{edq)@U{d{d2N+R0wI<<{XJg>c{cG&VByP}stX;kMBUcpuI zdTK)Agin6=%|%LgNS8NczM*M;?}@zdze0;=dksq-fkbR8od+segocs;1WTyx1frD|Xdd1+ z8LhOwAZmc~v=rqbl@@Fz;+{WSX#vrqP3lo-t-!hCKSB#!`nk#mbbgF!1pzbbA@gI* z@xHR7w+5m0b>|<277*3(_y(f$W1#t|l{?V+vG4)^wy)eeI6o$)IOd)NsI<;lR{Tq) zwa@vW{l?#Pn%5D#Eg2L3LJP4Z^dJtcWr$;DY?nu&wGtQ2SZMt$XAiB3cExw988WAG z#4Ik!&;Fu&Utx<5Px!${!s6zVsor{z^ndN|k=U3vK~vy{UQ$L=&{4k8E;6sYJw*7w zTk0*gh}fv#BG?|M!i3hSHd{iK7Ig=KZH|C`w%g6o(C5vIEVDy?4m~Qb z%w&-g$N)Ypr4=x=AEgj2tqPGZ!TATLPlQUVDYrTuI}=;7#elv36} zZ_GiQQqXz@NTTd6MKtw;=I>M|psD{9A_X{)3{o0Q{jL?Z3LKzbxm+l*mVLeQ=C;7^ z(o8z7evMJNiQ(VWkMSqVe?n6~8EFCy{!Mf>w70wnep78>E59<=g$P5d`&9>RC8poy+2^m};G6Cx zpZ|JSgh9$#Z`E&&{FZCSrfePW_i_rY(f+ePeQ3X#%h@MeRTtJ3!8c{OTt@8Lv$i*KsDjmU zGx)ZjFI}kGo3Nrm%`sKYzGF3?gz>24t!t9oAz8el=>{h6p1`ui0S+Ef-aAELa0SIV*af<+2w-g~?f^$i??jZny)h$sY1`xL4xJWe| zC4vpkO8=1vKx;=j8A1dyJEJKT^P-7|X*9>L977Th^VPwOpKhhh#FOtbQi%sT*akGS zye5sp3+&(w&R-=+48kk%vq(r9Hv+FI3p*v)!z-%7o&F%4PE$JFv_i@DFT4;-LJugs zpwYYnaYRP>FLbjeTU<0_>Xuvq74~(DsNJ`zA*G_HmLzIr=<~=GxH{S8X`l2+{w*u2 zwd=`*P{+~PJjy0{f{F%u-h8gV^VD=D?KG-@HdqLY^$E^x7o&v3YlY@MmSM^Epw%tJHhtL@!P`C84)-8oEf`|#7D>ND< zo;^-z4DnE>KRW~-jn6hS_2zwd%xeAmo8yHeU)c|$#iC0^@20;|tK zbdV&KiH@#;=FdJTqm(K}dyDFsBkc`_BH)JB~BiBpP^++2OTVi^-V)ovq&9cYOKM7=Bek?52VXr72Pcj6$CA_n|x z!uf7+N+{3fylpHuLa9|*R^9CPLT_0&Z?wiCIxTg=)3?5x|56IEB=jIosg;OhW{+z` zDYXd~%~(pwrLw0KuidvhTMGok-@0mA*YAwJPLuSVC^5Fdg0ARtFPnOnk9xeTGhX-e zF-7YI)O+rIo~BZ)#%GJN*M z_qVv0K!luE-X>GSeoBQSltNzq>xXsGIGk#iNPIdD1HIzqo@$a2CKFmB>Df7l%`iHfJG^M7ATgaPK)WNTBZ~vJrp(605 zL8FdT8+D*`Ma7rQlgcvKx@<%Llu$jDiS{y2{8zhQii}dLn-AWh)~LT z?Q`lyvzLV%-^yiI?zT9Q7Hi)@-Dz7nAx=|kSW1N)lV_0<$N)Yp2^U01bi5&oov+B3 z;QX9{?hx!SAMC}h`ToPBYe2NfyGtNI>Xk28$y*%Du=)ZUj4K-Fm$2i=)CC z6Ng$aI<4&YG_sWQU+f^3gdR}rK)VmXF@m+5A@w*knpYvs*RSeEDYXq3%~(pQJFunH zt9HARsS7fB!{2Ht*M@%X-j_o=>r;DgYxR4B_rl}1h;c>pP7%?$^-VEsSLlHaQ-xJE zf9Yy{fyqI+q3owreJbZ?Vphq335EdP67$Z@}0>ViKA zQU@pnUQsW?N~xZ01h#SjPPr?mj8e)8Cp4B)z$s8lz0Zi;5{XdiuFUj+62YGA(@j$< z<=5*M27DZI@nV|8kx4QUqdbl;TpM%vc;B%WsU$V)Yq8GD0!$@j=N@-(qi^2Jk?~ZZ zR(t9WD^^Oa0Q>$up)Mk3_t_JaQM>QJsW4#o`4nUhXlX1G?~sKH#m2^`olPEv&rnr8 z6TAy4%}Kr1oRkiJ&PVUkb9Y&iTrcuz7F7G}V1$8J)DORKLEsVVD~h4>EWqwA+VT(* z01d#0g>j&{@NRjOao>fDo!k382`re`REPd|m`R&!ZvLmUpr^onxJ!tUQ$AvR{Vg*e!RFJ`U zygToDW(6Z>KGj^lQEFX>T0%?<^@A=$ne!iL*%KN7eE4+A@W~VCmlUOeCQlcfepqOk zr^2E2bmF3wWun|}eLpz*_Ny3-Vb^4lAn%U3==em8ep z&8HisyVHKH5qeEM#F(>7_xl4WUXsaqo5@EyC2Fcq$;2tgcvbtlJAJ>T7$v4yx{q<^ zQxm6ER@NgHXo1aY=gF#R6cSTm3upgoJ2#vPSZLW$kQ)dBtwJN?BxjSyq&I!572=-L zPbY*-+(T{AX5{gSTjV{rHkv%K&mvaGDV`{_#%BLu5gLdDJ}eXh!IG7sfKsRg`4XHv zos%U*q3RK%5R|V#w6q%7!a%mSxm){v6k5A*?)Z<;0+N_LTMOwWliArg@!MLoFk{+S zLU{F%!c63sEc$LlakXXNnYr<+`$*+RXki94YrO7+7G`G#!ohZ(<6vQC=^eUf0XKz2 za=sb(FXMqxUpOMiJ)qMr?9b=5U;LM^h$W#1l&{cewn7{eDqD$GTDx)4jD^;MOYEUl zWmkH;`cCi3>^vC>=Xib9BT5lEib8Ak?w>d%{xzWI$>-g-+c$Lo+!}WMZoq?00@OU^ z2&I=HCK5_Q%CTcB6P+`BqbGDx9wPaXv7eF5F{+Iiv}WnEzA2ooek*juP-yMJ>4$~Z z1_wB_ULw#c6y*L{CThCa1VKUaO1hzvhmqAkbm|6IZ^=3O3RZZq|# zPHkQL9brXgr8N{qEeTQ&Oa5o8!tEvEsNLOhDqx|tla2&11X=}@)K+JcC(cfVr!T+ee%x#> zZgjHudQaPh_K#n+$j(MTcw5})0yam9LMx_%gP72Z9*d*PHau`b|I4xs&=P!D00?d@ zwVE1qAzvx-B{=7h`$1?O#_K-|Kp>ju?RugM`9SmYg^?%#J#khd4nQc;z@_bCi_qRa z(`cm|-Vf>R@0)7u2)1Lw5>3i;q=*JBFoCG)TZ@Snn4md*-cPi^j2;LFlG*pc0@E%@ zX&(TfpM6b11^WrHYfGh^XCeUnacjzItG@t53<*Do1JD|AOi4ojvWlOF}(ew@%)00O5#0hpiRaWD)4;PFq( zI$oU9trWjCe(L7rysdn(s>SOb=KJm9loy|1tu1Njyv%oOpV*oPGi{?&GUQ?v64jC` z@a4z*-PC)exg@SQSk@6NCESZRU+r0SiDMU~vclKN_aZayEJ@&p-34TE)^Q z%T&%hyJxcALD)4)+KFL#iu0nKK%`8uHQ}nw-JNw@8d?g0VDYtXhmZhh06r{?1I@pL zw4;opBVU4Z5%DHMj2pRqa-sbZAex_NHVQ2toI8j^3rYfTX|1)-AtWHPGg{%f{zV^5 zqmhpdUGy=v}yyIh1TSt&^kNd-;aW?2cbntFQy;kMxe#X z;rM_(wCXRNPya54RBfBy9=R9)w`xNy2|b8IYYpO0dhLD9~TbhxVho5i;vV__fOO5IQLt`{d6 zpS#Ox5K|LK67uq%e7oUb7Av%Rhh1xE5HxN0+R?@MrKrBBE@ zmBPg%mZ9qLs9@8bcV*iyRo{~>FMKsuq{ee@W#9Yg?onoS?_O*5(9>pg(Tn2mQ{6a{~n&ht@dSgGF|z@ z6O(&atB&S5h~BrJzMw=*w&SX5C8a&$V{+_mE@406mN=cY2IfCHxB67A)r_4qhox$B z2M_kj9;#bTOobc@H)?l(oC;WIg(@LBLZJ2D&}0D}Q*+s)8-6}ZpNnRV-KHD@RtaW^I-|gQWnflM9+M*v!ZB;yZ;iUt$*CJkY zhh?!#YU@S8J11G7wR(7HU0nWvB@eOHj}jq=R$75L{jktFY7B?g>j{faJ?`b-wQJ{1!@^ZE9KU4nzVUmirb6t!-w3&X1KNUxM@4tocLc$6$|XJ^r)9 zgFrMVw`Gv^Cv0y6)rId-Xa(ckK^$7pWCtW69jHTx2bo5Dexho~@L=4%5*v{0uqzSw zj99XR7KK36@vas``3jnUrdXnUJwFf*vlQ9~D=o2I9`WeH#hdYzo$UGQ=0!VskW@-L z5w>c=!@j?KMJx$DpnQc!^E$*aM*d&W`LPgOG-II^tim2zkL=2CFYv8Du~@Xm0eNS| z;a3fUrR3H6V*3o`(^mEr&vy~}$)A3dCp1~g%zv9Z&q`UouYQ431Jqu0IONaTB6`!; zKY2U3@F@$ltayg4wBDAavg9GQ`W-jpL7{aBrymwtXOa62Haz%t!lDO>ojhLY?N%lE z+Om7s`xJ@=z8yVrMs7z@Zuw2ao#*}U+~b`Y9KOTA+%~|sX|%1S<7Wn@(!#Ao?BZ}F zCiGl!G77E3IH9r70#1QKt02Sb+M$iA@vc#2F6U?KDuwQrf0A%u}4;XZr{IWcXivutF}fad_4O6t?{LWyW5$S7TB$Q1E{p- z$yP`mAf`g93K_Ng5u6HGXvNzh0Stjwj(*xFcf;r3B{SL|TWGmEHS4aGe(v+2u}!9A zwy0*H_veSgqXpl2pq17r0xJ=@gnzOK4MYMT7HEN+p>~!83atv{OK?8trN9tq!IsaD zT!R=|=YyLtDu_W{$4z2ZwBdk>VH>(ZBMKcy!5vlB4)w#2t( zn_I4Kv!v@MJ==bPI`%=@GXH2U{XYAdLR%9x@Ak@=w+Nmsr&0GF)c>W_ysNf&z1BWU z(%~1JTx2G+gu$W?0?=AH`W8zbfM4V_SKCr6uUf#d&Ul>kkEVO`ApwRk~G3Q|r0xi2^DF(sGmNV?_(hu+2QQbl}-x}4s&Xi1Ao_6Et zG+)~fPHX?ZGz(+{=m8}fv}XbwBN9FaQcgnwu>om*_P#5+6h9mn%~$|lb6^j^>vokI z)mti#XC;&zF4GM#PP09jK38q(p7nBNN>Xxfo@aEPZ07KmTtQmAp7g4U^HsIpUB4~u zM)#{k0y{W1x-6JOUG=S3b8x7U3BV1OP*V*4aoS^<;d2_$Z&Ahz6o992`e6b1Oa%_W z{P9^WQH?y6DcP1}O+n`|j1z`4+jpM`SBWYA7S`}&KkEDTz_*Q##& ztS_$G6s?o=pGLKT>iiG*zl)6#p+sA8zEf^y6nx56`sjAx%PB4fN&JA_oJanmsoW%4 ziBu~l0KqFpHuM8<-#r3vNdb0$rScNBdjw7eEC91(5r0Aen5kzv%iZuLX@758>aFxD zwRbBO)=_`9F>Xm#*yW~6`C1<8^jgsn8Q(wDI6MF)L|Gq+hn7M>=-5&n2nm1&;KNen zf#wZ&3{b{>N4^B-5|d~Ae`Xxph}v5H424!C&K<;|1x+|W>lP{nl5oiEj8<=Jgffn4 zw2A=rA=5A?T*c5u9jcqPYMG3S{j<+V1I_jwGC~;_JK$eNx$Yq2Vks}WKXD<9OT9Ah zUlw&lSUr1m?lzsK>h39TYw@>gLo5kBpp1h?vkl@Ha)>wzttebHW1;mB`8B)e$0F^% zYrM88J-*20y`p`*s&bR^rg>62*B6_=H@)XhGMUr;yPY@Ab@ZAg7%x%^p<(KGjMZO|X{bWy|mdaI(DGIVPjn!?(8uINCKiiGPrzULx4yf4?EhKNow2918kPgq<0)B>|Fw0p1my1Q$&sR@@K zu|NwQYE+6{<4a5h-ub?$-Ou7wz(T8-f&?%GTCa3X_qyxToDB*WEf5rS36U)8@b|fV zt1a}w1Yu2)WUA;q(fcb>(lz>_HT*-*v~{ezjRBFshlN7G?#WGLltMp{FTuI$MG{*I zVR!2I!$XZgw56JahZ;fiwy^&Bv2!?g{6}bkOHUKkAPOzeJfc+-g%;DsT303xftE$W zsm-7;t4}{W2(9=(3oRh3l9C<@t&0QUAbeeW5LyinEgjMUv~*Rz6tEv^ELeK+?(14Q zEpEZ}lRWBwp@moydJu<}E#jE?d|nh<=W)@Dh1RP}?4cEA_dPytY4Pzznz{EsjIHFR zeD{C#)3(_5EQ?a!p>-gcGT{|z;l_q9ev1M*jODl=T+VqG`y4$|@kpmoBMdKGs`Bjlc$`ub!H7oUa z#d(d#K1+jNO}s>?$!-Rje#^D5uAoPw!}_MO!`h%pP5he^Re#3}LTkuw?LTG_*d!MS z{Y<|BU5OZj6B-LG;1npdiZf*1Q4wfSmc=HSZ3{be-a^mq`8pr_7OwW^$(=VPZcaGG z%cCTE(x{v_xbEi|hSJ{blKR))MqP?i>Aoh4Q|f!FFG%c(TXJ%6ehjU&`k^JeA)T1r zwYD~(c8|rWfQ8miI^q}zv{L4#pGKf{jx@6($91hm)d{zN>n7_vg?D|dejdBVi}uck z=Ql;7eLYcVb?tb7p z%MyLLz&LDEwZ(14HLv%bk(ovj64m=Q^Vk;ooQCc%YJtaNl}!p_M1>Nv+FkTa_4(x( zZ^N3~ERwQJUc`yK8+_;)3=g8vTJZ1Tm%i*Si!0dbcVznpw8wuLrymwtKapRPAk3Y2EVO`ApwRl7Aycw{$y7EHYC)Z_DA0%CfV|3-0 zbtab91P#uQ{R1u8gNiQ1>^|+8Eo%3xI2Ewa>heRJ41pF+C;gMVe)6FlG2i8-SJli6 zIeOl5Nblvgem!}c?G-Ib=<6PhBbAev5QWz28~0d*1|oqE%Y6&%eyhR;rBF5UB{+|! zt{qZo!B!%U+$)8ldAKK?F6v+!t$bqB zkU1^phVj^BM;fux2|AJqMC~(U5M9&(nn%jA(5ftF4=oS7 zDvbrRKZY&LP5ic-V_vp_n|GtpL23kV9?TP8<^?MqcJmYZsVWHJ|2@b9D zaoICnOUa#L&Ggmj57s!}e$jnQubx!Iv7ueQpg6(aJT&O!0_oJuXTc@)ANmeG=TM5{ zET&?hg?pLf3$IPYgg&P@9fj5noX}Wk0jEHr^({l@$36sFTFOe+^2eTJ@|X#E8DBX( z@w$txXY}1Q_WWswxktxxRnaD`^%0rG&1pHR@FH(K_l?M|ZNFz^D7492c)u`1T zWZwV)c+X7!UjUe7daI;qT^ViGMKczFEdlHS z=weqDuQoL|Y+*veuJAn*5_4vccI?SHTq^bB!+M^g3F)8s6S`I=&d&BL)H^U&SSaEM zV^NIaeeZ~p)}D3A2Q^ppa;%~A7)D?KX!>w<@2~)@&aNLmrvd%$P1QmZ@g1Cg7yvr( z8^ZzkV_f!1yJGUMi6wHi3ltukgfE(ZrETH4XF7&;+M0>-r7Qj!n>bmmd-rL z(<2t^e^)XiFm!5&g#?N7vPB=a|GQ$|Sc)uf)aH6Wsgr)+)cAxSE5G!=(04HYE8mnj z)qQhV0Ej+6CL4I0z}EF>Tv2AAc2C5qfCZpjG7`WL06x>6H_Kf&b@Rr|lj zbI#20+;HKFm!>A;d+2!zHIX6iz$Ea8fkMFObha%hg}xy_1n2Q8uDB@FGHMip@)ejC z{mmP~S9VsO9h~_Qh1P9MI`BgaN+Dp$k}GK_g=j&$d-@&|g&efw^Z6mlCi z3PJe_Onsw}0pTkYTA;a1^&1pg7k_n!!<&D9ZzUz4OT7m`D@C(g_D@xtzC!%wYI2m+ zx_IaJ|Dc6L5_;f=mNSx=vjd+{Xx+i|W;$pU6BSuQ%hjRb29Ioh^b4;`JRA+mafa+& z17%Vi4P2!reg_8&DGW3jDPgdlB$72>)WISzAH<(ofqeA4r+`F8s|_J zOfI^->v5}U_4=}~2ac^B{)V-!Tcl)9CHg5uG;8*7#MEr0K?~W5xZ(@ro9y46^T22H zGbwHqTK6zU(?JVO3KUvZIbDO>5ok%V74<3HCXU^*vEtN=1t(^%ZF^DYJCENhj+ODWVX^G5 z8VB3+%j$0eXyK{a&Ipt<#2rck&{^bKaPLF}&G$*qLK)YHxB0BKEw4$%Gcgw^@y~S)Se$OXD#jH`8>vO z#8i`=E;gH-%lIelNF<>LRNA?arXZ^fLokq-qO-fv(FCn~p1JDCG@97&uCN7+CbH-J z7#>Zu{N2$6Fx6{90Aw@)6&Dwz`5NwO6j~24y_p_bVp*)AW#{yDYMxL|^gQ*rGZpKk zNb`%u&uDJ+Z|MFI*YY-3#$d&c%N%J7a@MGe`DGkkWQH zy?1P1#(EmGko7TKl~z+%4ucM{^lx625~}}?F#gf&KkyH#|CF3=u5B(84+z%=<@e9o zZ4|tT_~JmaOrgF2DQg3<{`hD0+0=CIHlM0-NB#4q2`VeOyaUze^jNjmZk3C%&N!|b z5*00xle+H1Fa1v(nzb9!|K9Q{d@dwqDWUrR7~=xH{?7!R9HRgCb#sm(`X4MzHFfhY zew}AzIoZRM-JQUbr#sWMC~kI)XNU15Rl|UxqGjZlUL;S#@hBGPS1*Q2v_+udLK!FRsz5@eJL5p{s^cl zp`MAzqRa3!&(dQgnU>+VsV?1$-PGstcb~Qbrap?FiY_%>{*?sLDN4gjO@!iZ=VyRy z=iUu&Wlgpsi@Q9!(`v|jW9^SlTJeu;NF<>Llx$Fa+<+uT%6Wh-0=Z5a=02q#)axt4 z2lXwJ792Hw#eFbQM(}E^{mLBY*7^>1fA6$vHU7J#>AZn)PV9|-0?I3c-zIT$1g~D+ z%Ez5WB2&rWQ@`LNiYqD^2lWzJIKH_Kj6T&n0~PgjjM4O>KGBJVs8{E_7xF?xox8y6 z3|G03^RaDebv6|jY{UX}?cI+M$^6DM58=8rt6ZJXr6{H&@i#l*&{y7~~)X5kZ=tX^QB9a)0sNdC{QxZ7y z*^l;ej;8Jjg(^Pp$w$-D?K->NyBkU4w>`CW;g4|djTjgb^(P}fUt90CoWW?Q@&cpF z>O1kieh)OC*VBi-ey<5}1Du~((2DEzdrwDg;)>;MP@Z6nZhFhWqys;$po4ng_1E5d zbWnfgR}@~qs{K8vpX@gzeQb*+Ze%TewnbwDY$a>RVZG~Gv^V^dJ|vRR11fz`e1gQB zYi&RW^|bEkKBI(bP+z&p-y4h1uu;P&bYK8XoqhT%-a-9Fq`6#r3p%LJ#PnwRLA^F| zFR*&N$i(T}^@z^2gLy?!`2$vumql(CdH03gv$!JBVcK~Mk*yz`Qw3GGy`_3B5(weS z;(jr^;RvA~8P<7@DyL0g(Vo&KS93qfo^&O%O@i|6Ki!8$9K|!e;HHd zQ1L__^JF;ap=Ff?ht|)D`S6BXnv{S1IkNUZ;nzWGx2iXwToVWmmy(3-)S2=h@-|! zXm55!OxuU?^pOFM- z?LR0tem2{e%jG_D?qx<#@;cAhW%DL{xEmB$F%U~(V=Hoq=h(7L@L(V#07qVmc`R!- zvZew52DbOI?LvA$9>zZx}_mwxg?3%ZHltR*Mb@rwkaUrT6kA6!h5WrI<|7>c6z@&DLhiT|v|!YCoGx@di)v z>XqP91cLiY9K-`wF4OH}18+%xfw7XFcpwW<;??H7S8_*)*L+Idbgj^?7X!%yGfSOs zd*_e2SfRPgU!Y{dxW45lD8)his{V%p`Zo%9x!SMD4frA?kQh6Im(26Nf7x7thZ#Ca zT@d0`6uRLHbj;>Hv^;)^aep|zKhiPz$g!eBJCOapNbHdu|9QlPn&;2M-t&4?S|T*7M^ z6QyXo!s*|1boB3*X}~nKW!KPI#e7UU@KXvZhTzglU0Dz@gjRK+dG(4Rs9pONv<>#Z zp{8N(H}qp52bT}rtjrk3kgpep_f)0V8eNVJ2#ZeGR{x&Prcnn@!%u3D5sE_Ra|tk6N8!{{|@u~Ik0lxGcmMo zDa2Swui{J0S*UnjPU`ech>F(-6^UHBesFM1$>gAu`TA^^we80%_^$I+{+iQM)%3r8ld%J5yl02 z72ioh0sv9*>)Ow^2kPbSy89zO(0yR1b?Ave$-!>+zA81dndH8%!8iw@fY2Wg7vfd% zL{2*fqoFDe;_mQuGhP)3&6kw8qblBlxBx`ILr+M&4ZOe-j~DIoe?V!K97ETF&vrmj4D09`@3{wu!*CgH;? zC>ag=w7|A6Qq}%HO(TB$(lySbrJCGyjW~Ea=pS&9NJ0-NaG?6Q2}#Vmh(1)s-(Y&P z3vz9mitk9ms(2JK7B?jvR4?|9R<-t|{xB^K5mbC&xq!EHj5?=4zaW+BFp2#sNhpoo zY98x**c5#FQE&^>5CvQ=n2=~+3iLync%Q8oh>)N?&8ik7q+eE!xslN%pfPNK1+Df zJ%ew8x2gAD$XHxTJER?-3%j4UqbmLm;{v^kN5~^lhp6~fZOi+CdU+eKzl`K)oLhgh zb=rF?!-pJgcQN_Wf>ppG4I~E7cFWmD-Czx*h z3Ys0)&fygnhuu5k#E6JA2HlE2#(FHi`ApB2KD`?9VI6VvJB$8FBXf@*`WvV;av?th zoq%EM1xQTHwJ>zXh}N4c*1Ix|#Wmf^wt*R=qKzfPuY(=E6&9#IZbnSai>gFtj7l-R znSRD-YY6KZBT1*GYxU2sADAobpjxCNE6(vOBC?oksrAJdEkQPC2FW~u{DdjaZ5+wZ zrAeP;ITRD#T+T>Y%l?5R)ZNvgoLodC*Yi<*H_~Q|z$2HXE07tZ5td9Y1!Rvbp1?o1 z)7LI<;eagrS8kKeR^cL(v(!OKrLGOlr4684SCXnOtQ}Me?Ubd2|b`dh4yB5Bry{n z4Sk}h9MhZWDHW{9no`0}KdxO)zq~)UXnkf(QV_3J!e_H72?btpn#*244fA`v>gU~` zt?E4&OzO_|A604{yfSS;P*SRKQahil+l2n&v&W5}T9$FwNP-xZX#4U0}pp|R*BP2ZRRZ8?U*$s8v1#U*g z2Pd|14}7yMRp{8G_rkgQa#94xdV%)TnP=<|WCW(Qaa8e)Av*Nv5yjKK&9uLMMUn=s z8LyC+T@P(Bp1;`uU)!nVqBny)+GldG!ST=*US|U)O!(fgFRMQjAH6zy{7*% zL2omm;tKDjgvxP*x|G&v*IFfXVSjAr$fecNvxo5+&8FsvF6>ufjQ+0+`yhuWv-KcU z0T%{;7aUr!fYoMAOtE9Q=;*Ng+L?Q`2@B`2T zNzB!m&1jicjp@zw0F2FI3BYGmr{*aUx6bd+O5koS|CLDXu;hgf3(Bgy`IzjdRB?LCr?`Jtj=+Y{}q7k=fpm{VWTk-(YcsIh}#LmjI#|xyAc6(4xE@zg3*@sodbho=%t zhZR3#x!hs!RaU~qsAcC^E57ETqVq}h)nwbkZTFM*{!@G;lF$Rc;%`9`vtN87`U1}{ znBGjU_=!%e6~D`=Ws2*ig#A`tHZ8>`Oxs)5IHXZH()0aOWp2f>`Lq(9I0i%Fc)Al6 zS`69WG(TEUbBP*(NYVzvCb6#>4n}cLH>9L;Ur5ndni+e>co6hYtLH07s{VI9Bh%;npZg~=ec(OFwX{#k#@-$$KrZ0IwQs_$_`@z&p z*KS-pedTLT+{QIX$&<{#u*P4q&ur&@ofRG}{cY^_jX{)t((aEv=G>Kya*SmGpQj@c0R%JeIF3_;}Jt~ z|B7+pSKKWDv~m)Wi$b7vM#E}*piaK*-C38$Tkl+@mvDfMljD(rP~BYNd5ONgA)b+g zwgOz5X+zLT8*w#Bqxls>+<{5p4+FG-(GpE6D6~3|AA<8UHzzSctL5F6cx>I__IIHL zOnV+N15#13>`^5T{n0Yz8zvq2p#`N7uq3E>CrTk&&^8f#naUIzg;Y1oRny9p?^{Rf zZN*T&0#lPG?}6|Y3N6swHti@1t(0Hg;lh%bVQAUj*|RngffmWW>tqybXjNY+jZPA+ zA@@G0bZ~R~2Q4I$&;viTJdwoccXOc7YQ*$rdT6B}_X3LtaGo_gwOms^erjK?x^a}| zShgk;)4-5f#riy6_Mu~VxLXA5;~upyaS_QXHj|=0DHuFsVYDq{cT+I6pzjDt6IN6Yuuu+k9UAFe^QQFC|R!L^Ch-F)4u#T9u5@O4m=oXLMDU z3JR?zjM4Pa0!e{F>s!t)&s7^Hx?lPxre*nP&iegNB*$vFdMHtT)CRmwF{rlbjxe3U zVOnuHQ@xXOcbG(qt%qV-%Bs#byQTtanysO5am$I9bkJJ9z2rBv+Pix3xgaEbds=bqWOXCEh0TXK#uhP?|7{&7a8GJoU{qa>mn89pcZ_%VkBs z=IKU;mei}s3`PT!z#j%^fzEQ3Z!8L}PUMH+T;{qE6SQcR7XAL>*uM)cV4A?fv3Q{c zn)jR>i&k3Am~`NW7F627rMd6cqSAi*SI`FTej1i``R;`sw~2_f*FLlULp?Gpx_Sl= zrJ8(f+L_7O`~FEg5=rO*m3A(qDYQ4AOZkG1CTP8xV`>4*(ZqVbdlcGe!sG9bCO{vW zpwa{xO+ZE63u&&xSBB25wP1QPJ+#P4tf5uq)H;Pb`oumfVNHeV*rfVJlclZ1&)l|h z{81metI{JkJ;s-3Vw}`bg2TGd?c9PPYEqWX9)W2uMLW|@>%EryG}c@!nMbLY2CWWs z3&iIU7ndUWNenu~(!U9syr_7#V*I0r);lLSv?LH{y+6lWbM|>p$l7DKwU$P4D`tM? zR~B!gT&fR#^Qq@a*y*vNUsHT5&%Mv9TxWM?dw%&&vS6h_ZgyK_YF9piS6)MN zLiU4N;bSJ?SkH#cH-o>!kPm$8K1qWCS3h}yTnD*1p+zxuJJ4Ot9l6z?TAPl z@l^-sGZ{MzbejRV8Q$e~;eFKsXx<(%^s0j{#0_vhR+#mx4(J(27&XSRybVrWu|}cw z6O#`7(1L0s@Vct`462Q^D5M)7V5yCX`RbK4Z8ZJ6+6YX&u5lKkjZhqe=5^aIqd30v zE5GUU&JW{QWSnOD7b1e=DXMe+fa7m^7hZo#sV0}$YH7^B_7BHMB%ucs$I#yFjU;AW z%g~O94oq*RhgOj}YiN}@wW-zz9@%GEr2fQ5u(l>Z&`+YpXG-KUPivX4VU4pVaB1~- z@W{I+2Hl`e*X`d}-Y!j9-<5f!xAjYHWsSy-6w_}XI6~tw(0Vu0zP;OBIJT1k|N5lZ zQSt1=_(u<|IwU%<9T8Fr1xJd{@JR`1K9IV=F3+zpE^GFaP}d`c&n5U%?Ka9U<)BjE z5O$?1bvm4i8MqKsU;Xe*6`3`(8n)snQ-IOo>)26fbzzLAhZaZ*6k6YNc7-oTpcVZz zt;B%ZbIe!mfJ{gmC=hg zOk5CnahuAei`lm$T@0uD-FUnzaN_HTYx_5!xYW6bN88o=qd`WF6i1(Pkv5k$vXebFl z+)ILCpSlJffIke31I>@eN}-JFL4F9%+t;w3QPV~!UxBF>2AL>d@BK=ch~?8^zP7lrAG88| z?ak{-WxaN^XOGqq_8rya0j+M|p5lLeMIs43pnQcw3nZpfC3i0fRphEWTd z(>UpM?@Uvbj$)+msPgfccudnzv7VZK>UOVX8x2}bohYyoA<8^oTe6`calO84t@^`AOu=RAkf-<_!~Z>sii|NC}qbOO%E-Q6ezTQI$I?jRk)s%0v?)PWoS) z(Dq@ini}arA-TYrx^E2@TC;!ck4SHl!si0p=Ajpqa$sDbhn83dk~Ijl4yomB57#W> zG~`ZuDA4Yuy6sRHzv;e0_9-VmJtj22OJ8g!qpaRZ#c4(!48g3`bIv@I}ADf1IrX%R8$zz;3(i_kkPqx>BzIE-)h z=3>=+d`1b%I9kwd?kHqp+!abo2)gV0$>Vey<0iR|*7zUFI1sdQvl^6f_kZQrvaMp6 zai)HG&$n`$B8*F$r6Yp;Km3TAE?|3UDTVB>{knYb-o}3)5Qm3}0A(Ch)VCptk(*J0 zj)=K1y_sIrMQh-qK1sp)ZAuJZ_u95i6~1$?#Az1JzGd}cK)I)=(^>s}mYmb=kQu9a z=SLb{G3{JIBnl6BcKsBqq7!w@{sC?&4F+*xX|%_2K6F-#8)G!Rs1GW#5cTGqm6sMG zqVDPYskpDbtD~indYtW4yp{3RobyN8TE1NW`S!`D&-P(jMkV##Vw)hdg)F(P1DTau8%+$R5;hM$U zJnF_95lxa0wyF=B8{01JF^!eKDsNC4G_IWZnY%^q6JAjdwVlEccc`cXqd%n9;1zYy zd>?l$`qt?_#0_xHzvB~=sMDThrLROR{%$1#Ow-F5dW8ZnCLQ=i9jemc(k^e-qbf~{ z!qL=uOe!s=FdkKDnF&f{no2ip94(awJ~HtKn5rRY58)3~)IoEzr}n6*gC`2W`H`Ge z!=j#eEVaXp+mv8Rs?z&|sK2(#Hon9~Ay4d?C3ybMKT$^_2|e(Ox-XK5Z5JfbN`w#7 zo9QZ%*MeEBE0JWE4ppl~!I95}g%tV)$|&9Gt$r20B0O?dl$cc6+LRlPD_T}R$yhK* zH8LJQrdvX?inCa?h(umLcy@N*E3d{c{Wg1AC3G1pkxwI5A{p!x8FUD$|G+<+fP<)b z@?-p?hn8Xp99k;!1?^Y%^EG|^loHZ!wOyw*jLQ)uU0P1>i1F(j{DhWH}AF?q;nLAcSD zrUa2H{>^TKll>uVPB&eWywEAhZtm#J@ zfb&=34ovG!dyiV2!tN6H7&QWdkU&jMuv&q>uMJ1-0X&J6bV1+OM)Ry>!D6QOwb5!1 zdaCu08r7hf0;WpWyP)rDd-y8}FS_lA-`AFs71{+bm7#umfjaBjBX5gz=+!t1nZ2#k zMQ-gsOd*kk9{4e}9ZAfI!l6e-;Cn=$-F9m}9 zeq#!}>1Sk2v5|1>%?ADjo*tUG5yALJk0~uhIHuI(Ux_6~@H?;1bK&!vxWfAM&zap% z#CELsKCRl(F6^=V_$@u$%j=FuBrP_M><>ts*LgAbeRI(7+G-^RQp zT3e057)@7Oy#`5vPMEdk=s%c;)K)9zCHgDO6R;ZRmpvwu_^k)v2uKMbiFe-_LN@uIU?3Zi^U9VC#K%6TubwKvBuBwc_MOA2(+T6%o9hT74t@#GPmNr%8DBvPRW@S_qly$DD9Jf`#Ix! zNo!fLSkkWvGoulqwJ)y>pV2dJ>!RBn#4twx*KH0UhkM%uQS1yLK7#Yjr?{E0L)+&~ zzs;dy)UX2uDlqM3h7bg(?1ax2jJnqYbvs(;BAG6;h{IxRZzIGfUK5WZ50ZCBc5)E5#PRUl5bP z8h~QSrTC04@YhD^Aqh7ctE!U8j|8Kx-0~F5vnhihpWFCVj@YnD9 zB2+cO$%(yXZ

r;`>`M4$p&am5DzhS~i_yUt^&V+oD<2#+A5cAA!=vvDTbcr{Tqv zl=k+KbqM(9{#g-K{0SKU=rOg}34^KLp?BSB$uHE74B(IUmffdM9^A$5FrG3eU+4^f z)rPe9QjNt<5}8-BkuOtIzr_mKW#||2o2Sl_@FIx^v`bM3sGSKdY%YbLJtcI1&?Xh5 z7aYQ8wD&~hg{eq#rQt@Sm;y;5LNK-XK%K7BjJqebbwr6bL^5W`?}@MJ7tKIk`=-xNks;XcvF5(ODQr2J7iDh6qQH#*zD1^`G ztxJZCmVq0MV#<+%cm=`KUX>Te!qwk;Sd|nkEBF4YU~=Q~g^~^MZwtvLd)ChweznEcWk6KrSrS0j5{PZ^+ZtG^T8r_FpB?OO^rOA zsGoCdWbp+4^(-5DHK;tsKYC0#BKJDO14zFxr7OR1DQP!<7(4IXVt$WBp9Yc*wpi>C zc_~$?Q9&H*E%eCPCAu?SuY8UDwhaTXxQLBolL_s)J}HY(cbTZ-yO@CFg0E$AD_{7`V&y272!ssnA%E0x-0}!!7B6a zhpWFEzh7(3$Ck81;RB1#IdaU{BJg^4bAV~;IS#(h9c$%z7132MX&f(Mt$N_VU^Em{ zpnJUI~(LXi;#fdW4HRb)&`|;3L%3&vP;$MCE4HT+*okt)`wJ4S=WptMi7dsaTf> zD5eOTZD#&qbB4z2oK{t&n!427!lwAInnD5zKk#E}HyY}SnV&|_~UP)!7f&_Mon^Kc} zgG=b6G_F%eG8JR`4jIQk>z%!ahN&j>6W-a5Ba}dQ5pI z!7(*U?$uK7?E+0p>UN8%XJ*g3vd};A;nPq#{`r0rUw>m)K5`*_>ngUkJ3MM`+tSR+r-i+d z5bG6Yz?36WOj<5gy5Q-@kOgivm3j89I_5-hp*L@(`&&8@akM}l| zYP6tg5LT}Ig!{&?wuF8A2 zl_8UUr;gQL)cIFUA(4b0_%Y>&B%(0r3yLXKOmC*glzRwkOu4%BT(RPE4S!bnI#*7v zl1nRO0~a;Bf&5zEtS~&MwqyS0GlFT(tr5x3ru6TB)LKDdH%l%LF$tf>_GVkdbCLY> z1G}zuJno~%)B_xwGdvDej4YnOzuIL2=oo(r#y@&Yk<8(kGM0O_&26iI*9^)8s{UN5 zW0VgH>H5=m_6mqfYJc=gcC#T@z9-tLh%TZSoz3X@-fjNy$Ji9DbW&P6f~js4Q}vz_ zzcIzRqza$W(K7^5qo=}+Mll7FLWE$-Cc5^4lh*B%VLHdU-}Y$Qv%j9~zu#C~Zi;(j z-=|pDXnbcBkg;kt^kFNH`1uWO%45Z6KFcqNmQYU&^N!OIdz4dY{(w8J2nOv5htKBEPv?G|hc!S}&Ja&)oYK>{M zwJ8Z`&;R|Gk{?dtGy3jUL3BIDG>p;zbvp*=YD=nZP=>K13numNQlEoj;UHMkf?9JPm=fR9j9 zyD0q-Izy2MJad0PfFdsqxgI$0n$|J_QlscWmEpFwx*j!2&V-^(3Sw6{W z?lb!h|;XvaT&zUk?eTPQ!q@n|g-=o_!I z@5%TpqB-&|i-}89L^I>Y7iL7W@8(gj3vtuBpI9ccD=Dbfpo$K88vVk9dLe8|RrftJ4;)=--^@`*$$~Ov^Xf zh!<0!xpLjm0|DBYbl}Gn_(f>YJ}v2jXg?RL=5wZXqg12CSTwH<7u5>>Ze<377HP+e zZkta>d;#Z=)QH2|<{jpUuM6ciC6IC-O#Z_<*QBatU+3+mkX_wY1eUt~tIUu{LJ$0y z@<$R;rabhby&0I^OpmF3S*$T-=hCPA<%j*=XP#tsjcJK;DM8g-y2r|EA6^!j9Z=I? zJx40KD`71As#tDcL;l?6PsEvHnloEXEyZ_|SjS#rA=f7e~ZSCxl#S{2< zxE`^q5Krfn&--ZsFaPt^)BpbZ<;=oD^yd)&Lmfno{(0Gz+P z(a-c$F>T?B9#isv7gNBri!B^@F$J2tNnb{*DSb>j@MDS#$rTf(z@^#bub|wa1+9Hc z5)*e^<+kqy+W^!0YSU;BAwL^6?f@U5rplRLfe;m1O##p3$38@>DKc_Ba4y7lcetAJ zE@M|dz>Vx0G?_lejP-Vm4WLjt>a4v)I?f5p@{gNF<{#Eyi5sh7H%Kf%Qg$+ zA3dg$72%kgi(qQ5vp}qZ0`+;p;SW>K#b=w&`6*gIbM@9@X=ewomO$B^yd1WZs193h zWiE-pX}6K z)#P3i9o3}lAWI1If7o+BPw!~nCR4|Blfk{``l8g&HOXG{BPoOt>Z@Asn7n28-@d6W zA_9Y{C4`6E)9@L6B7DebL%7kFrpV@Yaw3u^NHw*6()^O>sqfj1*y4A)8z$_{401U} zQkg_Z=r+C(q$gs+UC1SUMX^|BsG1rXQ`XU|8H@&91KK?#tQ}BH0WAaFA;pz>>`31E}_AAlC!u%X1=! zwLdZN399{s81qSgC{gAd9(&CVSv>l1?%t^z{}zvsK*A5G_H!X^p?S2Kb{rjU&^rHY zsohM&jW_mZ!?44R(JOPIG9HNZnYSx@P)r$PdNVzyt|7n8>KQB(mwx3-rOWqP2wgEY zGpsQd+AAZ_w6{p|OwT9E&V+_+bFTet;gMqV2@7UqAE*|rUeiqVx?&ypXqC`I3+}fh z0a;Q<8b`AIf5lXoLK}k)vGh;l=wejyO)&n^V=6rhjwx%|S8q-21v&Eto$Lv0D|yJ8 zvL56wTyxt^KF4uq+RC_^Q|3{>eJ7k%f2(M+pr0!4!s{elln|t8iD0S+UFTxJl>Y@| zd`8bOT7nvF3O5?X6i5mYhN(l&5}UPdh2;tGn5gW16vr`p;w`HSKjXjpy9&Hf8GC}{ zemXSaf^e(IhCPm{k2sRQ3oCfN)~Q-NhReQkyr#`>OqGC1za@mdUO3jdKu=n7Z^&pf zxY3pXQ$?o8Ed#;Swuvu0qNjfFK2f=bx+_ZgyZR*K(zKrvT}3TycQis%OiC2`Z(IE= z8o-Mwp$f)GLl=C2(V>=eAleUZYNi_-G~cvg4oWrfst9mypl-rMHQ08H?!UVo1DLiT z^*D+tb4)t^M@)fBr;3_FFa=dZ(ENLsDXNCFpyey5vQ$I*XRz*#TJZ&~rhuuE3T7x# zGm#De&d2Yc^_!>_r}u-ksf-*Aoj*sv_0Y&t*Kk4*#ktB#`g}N>r$f2O)`A zeWwD&)ND*|rpMHyB-WVHa~V($f4(>@yYSMd>YPxUwEcS|Ux?04IW*J4spGkO;PZgP zok6h^KE!U>X0j-@zd0Z?cYg4k?Ge=f@ z;9o(pGOGAr zkn}?o-)rImo#?3_W%~^#?!8;8v84ayNvY6`FTxy>&vNC8JxtGQw$H17UXHskadKds zKV#XG5)Y`LI~%MpM*m@F1IugeD_w$W>|D&X@M|m-OrS&DBdx~^CeZxi!=ZhG+z6P! z`BRS7OkkqT3exWroI7ge76=IwLxE|ZPjACZ572!24qucWG|#B3Jz3I&e(Emq@6rR9 zy6M0{lpa}#>)^a~d-yOt9Nze?21`Z+qpuVHP>G8sxH9eGF1}Z~1<_ z-saVl%1b7@`kuO87R;{GXKOjTlYO^c+aYq21 zkq43jL*9bJHKjgUH{)aV4%QUC;=WfE;P-a>H_m4l_4`9= z$yL8M+>KZ3V_nxTEa{!8@bx%-kpq^DmJlp@jPM!lpfhB&HQZQZeucNlx!b&u!DuK;K=+6f zvB%32(7YkZ7cFvl5SD=R)qSg&SVG$f@po8)bX{ha0Mnee3!`7qScpjnegH#D0dVOe zy?v;dXCwbMIG6ksHZ10eenVnTl3DYog%yk0DGAQ86!No%g-(i&|HK@LB=mraImjr} z_yHt>ZT3FA;|I{(KzlzrexUW{^-FyJkH-(t-W-gWdS}s06jO^Zy_p_U?`v3NO3r2Q zveK=2VY5ABWqdw<-t{D5=WN0X!%4eVjurkHx>cvl{Q<$xdzvEQ&e6cPRlNfi?5`a{ zB105y2;Mf-`?mgVY4MSr=KJX|wG3>)8wpb~hj6^N7Wj8e_AsjWHW>ftG1Zg=$J9#M zMPl-c1;6wZrK_x6By!HHGofSQOL-}UFmvvgs$R;(;AsTn-3+lz4WcDMVAgY|#Xv0TMJ)Euwf*f~6r zHK5nJuGUxotpOo{gdb2$aUpG)Fa;7(mgS6cht@rtsEcrMCu7v=R?zvNrgqACLWl}g z#1OGmoLkMe3NxD##xh~BK&0Z9pK6}HZ==s-=IhGIHFLE#2^Je>_ z&aKWh`5k_9b>3u@tjIdv$H#lOGRSLtcUp^3U-Bg7Ea8QEh5;h21qn1v4Rks#d+@$# zL`;nfGGNdlmj0Pq4t*wk8OA?)ObwXBF|}HDk+#%AA?arZHkop1azSSfo|a{VhgCF8-9na(42SoLXWJ*?GkRa- zkkQNGMx&SlNg+a(j20Yq+JRuo+qqUbeCODCuirj2>wTtubf!vjt@aDKEDCv2R<~$q z_0wJ3x~ki51b27y%!#wPo;m+(pmz0EhBLq|Iq#gK-eqJ(Ex2}L24KVdu zFaznf5KL{7&o7Bqt=PQ$b+MFdnr76ijELH_sX9MzcnC>XJrv^*S>H7|&q#S1-fF7) zEaR)cp_l^Qzt7kiFQ!0q;>ryuruYy{f%7lB@l{i9qc$_gvg31(8>*)4FzNUoF$KI% zynT|}9f?2=l1y;^TK6b7i0~FiZ-V#u3v0pDn5l7<=pVMJZ9W$I-36(p=7qP%Dg3LZ zkl?`&D5gM0nY14yV$Gv?FtbWv?EGA;nkOzh#T|hBkk*@v>%+Ktk#i@G^T9&}^aI_S&tk=X;*C2kJdlkG0=e zr`Bb)jZd=PrkG?OEMx# zf@>;|!W=@VPu63zwi9deZAe9f945ZhY8Qc{gw^YV*Ba9>brX4l_cx|$EzI&p5t)5TMc2 zsjH%QER$^#XEc|@$rSa)lo!F>#7s)g@H*EprW%*wsHQ-q4}TvrdJWuY6jL$|NY960 zYJ@Re%3m$elWfzGg^nd~wI_L^R60!KICC44)I_ogK znGRELxU*PdDx2y`xO6GnDAc^rn@u3FJ2Ue2m_)IdtM*H$FRb$88jK7b!%Gdb$#tPl zNHCW>!=olI!C$pbJK(@zoA$%*F%CZj*)5*)GtM?ZW;3tCx(A=8JiV1&FdYSUU!iTJKxi&kdNnBGi}JV__k z$m?=tyA&R%7m{_$GwCLO&quwQ=A7oW6V)tFJ~%fJ5KNScobq%3+hPM@_xj(I^eO1hlqJ-QXef9M%i!c8|9`b!g)MDhIvDAtZwY%)ldlVXzKe6l;neB6ghYE}O(;U*s9mszY8s3+AQ{= zk5##1l7k;wP-zF3K2&9mRx^1>9>BRyg4OVMhO4h@xdkWr@51V}_&DhX1RaC$G;`Z^Nb&uZ-Gg0ucyi z&%zPU7IO|k%MH_;>7k{J{5FdRrLyZ?*_Af#&<@G+be#R_WN)Ws_ra!olhci}X3AEC zxNuN!1}*13dj5lCjMJvftIk|Y7rc7tW#KTO`j96zKQEFLor8QSjv%s)p-y|>G$OPf z9BN|FA(sBh+6}!7VH3tbdT1GB!J)NTX3^mWBVk|Buf>y<_Xzc7w^-GC&#PIH;~4d@ zZjOD7Po~6;F4FnRpN{NK7Q8%7i`=s;lO+%M+qNOl>IDxm zq4uXILXKBXQzNV(u{}9a7;JB8!ui2TO49W=w0^C6nJ&1E&*%g-eH2>m7^CT-Wo?S| zLm0Hma<@%Jpmk=@vSMB6_##=KThF7;RLFT93{2N)=CC8mc6vy{;RZ*NJ0-Nj-kCd z0!c*F`k}YjZo%|sdT8k;v4&QaD~FQOM)eT$MfRW5RsAJ&$9O0xYe*DNpBpL6Gd|M5 za!2ssd8-=1g#0m@N!U_TWW3WR|fvn>APz3U!kQrQJ>7kYWMTaz$s5yT6_>5kAOp1LM@;lxbqg{}{drMf4 z7AsXWiGqGjH^ysvzM9@|$rt>@KHRDTlu^la?qkCVo<Ap|2op!}Ml)30#8+DD2rEZ|TB2@3n>HO3gf0imXw2Y%+2E!e=s3 z2WQ>zH8dzKZc1fOzRejVE%C#v$jHQSJ7JJ#3}@{3q}dl2(IoH{GH(AZf%UJQ8N>yu z+ra27pQ)&B`@)Sz$L*HpEXM68s*n3=T=7|3PxN`;#4}aH)#`A}#@dS9_Or8%i(V$n z&F<&=U@Ki_$|sz-K#)5+MfS8+^wdg=hTU22^-qd^kK2DuHbsWT<1@NdNe$KQ?HHr! zb=#eUB#>FRUA7{+Eix{a-MOCpZE&obKOsDlUDAwKGpPCG;F{EB-A_i+?Y`Mh8LWir zHVExcp9y$%8#E6fO+*z-1kr79zE7Wd7xfzP6DPr|O6Yal^zZ66Fl~y0J32Jqfr^plP(IWa;L^sE^Y-W1gu-=( z13vBNcUE7qm7@?&JnfCXo|#%r@G;E%+(zL5Lvu1UgD?O-evV^4Y0Lbx3_1jrc;Mfb zKt6N{eHX?*dM<5Ff^*4Ny6{c0y6`dbG`-gPeD(O#PkTa)8V1I6xPQH;eA)YSPf);| z{-ETxmIR$!PfJaE-%srJq?UyS+2#W-k$=r6{pOOKIvJnQfp>ci}TyPKTce>IpxL(ezx}Babv-=F(a(giFdC z3rh?aam;A+8!Ty-DSNstNbxP((XWlqBYKXk8Hr2hZXCd8rTJBUbfJF_#>zh|^n-CL z)7TaCttM-I6k1~tXn}L{d~K#-HVsC@hAzFl+Fvti&Se)2ag>P7O>7 zYCB7K_Lx26fYCi0vAc$@Tpgu*zG+nzcg()JP$=M`38JiYj2m%p+qpy>??46qInL2U z%a}lnfAoy=Glw&7mvmuU##G_`H^vF>FEX(7YqOj5K(DC9O~c%Ntyr*#wY}kjAML?? zg*_Rk)y=(4e%=?Zs`1Xc$UXoT`iB_jN?&{Jxs2n>Pr&H$HA6-R!Hq^42a*D1+)2k3 zA=58KhKX57dh069tw^dYG%6L#P`+JJ{#0Y)6w1XP!3z4EUEGJf0^MQ;Uo`hcwk&+* zINLkmMtj#rumu}1Zu#&+|M#&|G~aFH zfnrP)k!5iHA;J+C#=J)jV=UwU{K-9(OTn1<4zR4f z=wI;jmmib_rk>4C!drWR=9F9K(c0@3;yO65%{wz(d!>*p6wqgLuTJ?BmmDdT`*$I= z*Nf{rIH&)sy^uh{5Byx(k0fGuF$Y?Eg1vqg(<>fpe)^B4ql-_FG?lH$5%qU73~X zw52KX1Z%tl(q|!it{0DAc>nms+JoCAU%h_2{m?AJt1w@){m;0jlfS(S^6kpndV^zS zibBu;HHgY4-8u5RxoxI1zQhQMDbPKpTDE}?@L(zdD5gO3Is4lO)mQye@5}mzoG?+B=ms70o<_AKD3@H z2dR^xBO{PEo<$V&6;$CE1;9Ttf?oOna_Og$LtAnqF_)&7@slBN87E2Gh^8or?2Kz^ zBEOS1HQaKrTzcS6wsXSU>gb)TF|POH?agt`>9>S9}Xh@-B~bT zssjfHT5S{|u7h(M!NFmmwA^rQJj!iKASh3e{lf}9UXTc6&dNX~sany-*wUOY;ebG`@oMa}X{Fv>q)Ay7Y)kllSZ3&;N z3DXT6udwk14-TkwOQpm*6#C1JZk{x`>}8x7iBhEhRc4o%|}d-F)lg_ zR_eH{^nJ&O)kcJN9D@$A^p7)C4psbv82{)obyX3Lsjvw}T8T0uO{XfpujW1|DHE!C z$!?A}zd*G@=7KV(m^F3g6v}Xp>*H$3sGOhYlMDI=H4;A|Kz>9=lhbo5}vs4 zc2GG*d2*L<><&j>S;hUJV^?1{OrWf-wNOCtgI6`E95bKroe#i#Gmi?{6F^I0<4Oy3-~|4 zA(WQx5J6E1ML_9L@hSoaV33j`VF5~)t%QM!5{hCWU(OcA0BXm^;s8|O~M(xDc>ovd&bu_?H- z{d@ZzJ%l^@+~zfJ{&EK~B>aMM2RI*e;%WS>fIR9zz4-((Zo(y_D5fGY-b{(9MC8}h zUT}$aWSYak#u|@rmWSb7S2EYSjLZ_!LW1MiW=$=EsoyV(dGQHPh(lEM3v3w+o>3V6 zC^7XY2ac&j2&T>oaMZ`Y?C91X$sTRbr#Y^*W^|9RrpoSI`*7yg%qVsCt2?xg8mnv! z{8BZ%#%xrZu**Iu-INkjkH#?-5y+2E=vPq^Xv9AW7aGMB7!)EDQ#B8c$X)K4)uL+S z@hU%bM95;p{vTa;8;@;adgNZJh@ARI|7;zNTNcYf(wme>XkO)wh&CM;(NC-R^ppLh40 zcjy8eFG5Jr9zObN(gGWqkpBm3@PKHKR6n7}JA<(je&m54P6}7x)=4oLkj)haYTa#X zGSF=!ve7OkKA5y^1Y7ExzLGuYQXkOF*7zNonH3|tfcEu(SL6FYgb3gDXnlg&zNrYQ zQ*6n#79nI6_qr9&{}K{0B>aLB5{iUyWFSmvRndK*XpA>gBJb`c>c~6j$dvevfi*z; zHYe?!r^~epMPipSt^C&bj{U>>^`iG4S+>V9Iw#IIXum0!dv^uf{)m|wZ58YR-up*Q z`=nd21fvK;GeV-+v#|fPRde`_Vvr7NZ{}@?P4)k#|I>a9sos zr$F)&#uFdhifTq}FFYTR>!>PsOm30)k4Rsq?(mJ_A&bykQtTf8eHOiH+ZsmPtrq&Z zd%}@7YGsd4=ztm@ROoYXp;6?4K_SABccI1;L7uf%PUJl?w~kYbmuim&t2L*TUmVQr z>kH}}m0;c9=-SF2VW&@X_gi4U2T`{z`EW*#Rlt$j(Ms?s@qdw5>$@&6sOKp@q1kl& zP@&Jmg+`HAQG@s_1bHj@*6xj!Xk=1n)oNk7#amw;QF%S9ekt9tn|-ZCH`AXzzc^}N z*sO*kPpa(q`#^fj?@bUIMB7Ob7r4iAJ!5o5kPqPwXg^qIfNMpN|IcP-Ks5cCCU`S5 z&^;sf4T`B4%y9gVm;$%{%o_}Os)ojX(EY}iU^F`_K?VV|jjBz@vm*~qYjm3@P`75P zRk?!oe53Gdlt}b;<1vt(~eBL`FK--NyX}t-(WAkBfwNoSuKL8 z-!C9)EM=b%hp6gT(m%F|ei5S|C8jEnU&CHuIw54V>kua=dxpf^d5+bj+ya6ZX}3_r zy;)DBB7`m}?5GSVIrnxoW^m;5AYK@k4ElsQXI0+g#%MN zBVSIiG&Gw6LOcJ+z?)5h?xS>>Xg0-JFaZ%?6gHgC{cl^Z>yQmqr~4MZVK96`n2Om zi7RA193$#K-cuD$c*-4UkV6V5vKOAOpZB+L0x=}~f)W+#&5_7J%&j_zJ_BFEcr#@f zZ@dH#<55CJQQpj)VeeSY@|aDOuhd?gt!l`0j_Dm^j6UOwED_HPF4pc!vkwmKJF0Z< zkY-9OSaM2!X>fYQE6OnbZ@I!MM;zZx07CEb%14Dp%H1Yhzl27^cu5_#FkYh^wIHGQ zW}C+*yT_S#NS>-|6)Iw$sN7km>Lvd<(duAXn1(U0BV+S-Qi)H$0djz5yz5n)oZ^sB zW464>Vf^v4iKnnrwe;nIm#EN*aG}vK-Y<^m0}11LJnwp9#hMzPzFaR3ZDwenruf;)zg1$0C7~BI*fAhoq26pIIfSMU zWN)5xviqH{NX7~cE>Vb!&EX$A2rLQX&6HuhPZu7>&j=Pqc`$Mg z)eiVuHs#nHid&koJHMCZ!Hlh(LtS0$?<8`5C^C11Dm={mz^P^QlE~{J;@mDhly%}R zWf=drc$k6NX+m(JD)jX?V=G}*tMZ&nv$LFoL^sJP#7oJw@^Pb1` zQ0D2U*$-t79UfKl*zXk3%$1aqVp$vI)BdVm%GxklLiguLkXi2|_eoFV;vzV9q=2P! zImcGIuET|1p$$%%&O(M262`T-3wdJ2zMt}+lM&Ep>GJgKvrUa<4tlt(0v8_*gGzQNYn>y$33=_qCRO=bXi_kEB12Y`>!#Ffr0If|oa-)yeWGfJyIVOEJWw>hf0dUF!T-rHBK7_JR)*<4@y7O3LWdxV9iK%M>5>8O%0!Sc*K2 z_b7f~o&EP|95E#Pf&vQa%_ot8VENpMZs*^`cr)dqQKuyJMI$R`=EUWtL;hN=ZD$Pm zmJTw1j=ncgS{M*F&p-KCe-Y!n6Nq_U;mM|;5AGVjjE~lA3${ZW=QK(I( zF1uNqF$_Jg5?M5&q3^9lzS8#lMWc%W?nx32lLadF6-tYUUdS&}QbGr(+6aE6%DcYC$}!xbP;@T;i*u`nzJ6 zG6R_J@jcnE(EDh~VM3b&>C{_NUd?VFo%VcEjN<6qnuVF#xypy zE-@{!8^MdIh}Ze}gjO_DN4N7+Fhc*=?R+qmM0T3vMIPw>Gh`E*=m;Um1MOBFV_eA7 z|Fa8FK(zkvxhV2dF;>ElJSZ5zt;Lz?(afa`aSYHtsX!uvhYIk;x1#2Hv@A^fves0a z-p{|t<*i0Gy;tSAXXO9g^hS&azo1|MBRc7c3=D*^I6WjCV4&8$c?L6@=#ahn=GE3o z-b}8upsav@Ic4!RGa10(qHaS+43nO+{sY$cJQF%(Tc|Qi($bhtQb1=X~o0)GYJ)) zbn1(>kWfpmV8eqv>(Z#-$26edi9KTrY3P|h#>z*49Vd8*zrf&UNOr1tVwNve*Nkpvl0OQRO$ltvu9MKa;A|j_Pfk1O$1(*>8aY7)$uQcJZ z%oUm0NtZn%N;J1rtGw^6>@=!tU#FkAD*A+&u%7Pa(*g4A-Ca$jm(E%uPl*GnTv{A1 z%wkfje6v%Dr#(!AZBAweL>P{A${<0pkJL}Fax*n_J@Ij45BP6DFP$HB6Cw|Eegcgu zO2)R8A7V731e3HR98B?oMxACuvqaZx8Ym?3i7qPQB1rG)Xj-N==w+b#qNsM4j@RlA z?Um2FJZAb5=I!fbH7i{e(Xl<`ksldMZ!OmHYBqESj_JU|+KW%F=1#aEv}z_ji0E3YeD#$Ijm9r?p-Dn2EBpP$HkCWV*BnqS`6;rw02acD^(d-8!E z4&fzQKiS0c_6`NQ{=6^$3rxsaZwQ!dd`$@aJ$vv8omjURJqPD8Mrb=k`Cn&8mng?0 zjY;R=uzX#;skSPg-Q|0N_M6owY`YJNYk{mdR5)l#^^~^eTRQ7Sf*26Zut#@J^ z*!nJP?tAw0$7_GA*R&_)`QN%UbALZX)=iqAJCpGX-NlHbF2t7chh zhEK>r^Yz0H6`ONnz=>>2rud3MV&=pTL_iY_;1Bt)`61yQ%-czG7j&b}koMNHxREv^V{qIV3 zBfC{CjR_&wi~m9sxIgTILKEuE(a1p9{^&wUl!ftTN)jntqE4b8%Qjm+eAE7An6B4! zdG`@BTai4WV*{rCH~7w_(OGAle5d#UDFQE7eCLl16&7@69|=xQ0bB z;-4m(*sKHAUdW20*C=6=rf0n!TfOp~uXEygKLYy#=3Xlj_va52)p0gbqT?`0J~mB^zsy23Vcr|MdYK@>f3# z@;0zKA2jfMzMer1)Q2M&s;(oGCN2*2~i!iWhAA>lR0szdbElnODlG&DvFnNS$UcWm+n?Ohl~BKqM$le@pI`c zG7#Ig_|YaJ_j-=;W=bw;>r&^^pz0<|uiT$s2$clp%%}btE7MNbE8e?)Qs>S4a5~{E z+B4P+ZbL=a+Zt9zv&UB_g`{XSU9B*@Q?;*oKwj^w8zU0d(X~1U?zi@CpAE^2T$8m^ zUM}$a;0ZLEwec~^rF@J=cE~-E=XO27rHu$LAY8gnH`~8hfvreYZHH$sV*vY1esPI_ z{U_SA+R{jA_N~jU9`uWctO{n)CTLa&31s>)_ayvZY#V$jbN-=9b@Y3G?^6Ro$&KCyE8f~@ za?;WhnD&~TxMI2D?ZCm4--^lgd>c_N*&w?{DT6EU32hMi80FGSjL?)^vJXV;4CRu( z5|8tNmzqC6YWRK%Tk-VAfc!EA$Mx)KF&X(g2J`=6bxOKfZsRXW`(Ew))(lc6@-^y{ z=6JcJenJbM3d0EZfNNH;s0C%*YmAlfGme4Ut$zqEMsqK+(L9tz|8LB_fYBZ+EykOB zf$l{-N@(u&8Bray9a81SbFVN3Jz_g2lU#!+VRtJq#)#>T2RpIzXc&s z8-c&X&$x5QK)A~`pt;u@j5kv<&Zv$m<05I5%&Sy#e|)CTnNha%*4|ZetWKZGQyCna z2;8nx75%&m#W%dB^O(uDjmh|w|0{Z*4QbU|N%1D}!gCopqlEgx|7F}8*C`qI&q?3k zM=_w$y_yXLkw8xaV||qme4R*+>UQ*#x?MzSA{1<{jR2=@w~yfS-1- zP(-SSwAgw{ztxw!O*5PQb+)byGffz0T&*2jgZZhmBKV{X${C?Kh)DSsBPAu{fZd4@ z#;IMl>^(1gfn!8UuVZ#o>H@-!D>=-Mh1paMmJNH0FE_H0JK}#gW>}kJ=G{QUWcr7x zeA-TK-x6Mwg&b{RtN(Wciky4hPq~gyXeBuwlyUDcLQ^tsuQ;L=jB#)Dy5~3_crn`X za`vW6U20c5N@u=XsT9lMs#SY$m`+KcPbK=rT4xAXd>c<_vJ8peL9i!igbGh_C|ui$ZSk9s-KnCjvm5>X z?oJ_wgkR8L&w%uWj-!1+Gny2Woj)?>J=LU`a&6c9&(=wtM||eU#}<@JMHp|UD_;hme6CyTm&^W`-AQJdsH^|Op8X{oW;E2y+b=qMpZ9U*ru}{C zteNV&hs)K-Tr#b-lWQ=)a+?dElvX!qp}AKHMoLO90lO2ST&gO&BpWllcSulwPq>U) z#xa3icW0jxp6mD7>QQ-TUqFLwNMkxD$H8~0Ru&fqTxWmdX)@B+S!L)Kb+mURoV*9_qB zbl(-`G1nw6^0}N;%W72@tEH+ypX@R$b%6BZKjmJ#MsU=202L1H@YBWnx4$n{*sfnYfQ{j(Pas%0#L@3q>e2ipL-WVf4TmjpAjPc}f|@L+YSWyn#`| zpHox)LS%-l`i>JmBJ&QXFfA3*l@}F0-*4B^uU=hOM(8RHdAIM&rRZZfYDHK=Hzkpy zxaT$y#f1VUKIKxCa?|<VsFq@;}E?vhlZcw*IqOS0!1%ZvM>W0vHvzLk7YdfCxq zs{H4Dz6_+qUyF@t9T1jisAG>xs&d^oGJ9e7&$;rkHLo8so;xBYaBw_|Bjs&q6kpiJ zIAQWr6@2=EaRm(;zkqivT-c9yX9slWZ=*pY^K2wC zgZ7u8u1PyPuw>*9e}5Z@Rwpuc-1SGy;Na&I_~E1|4sOjPdl-^kK%+S5F7JF8eTXF+ zt-~V(*F)^|75hS?I1n{<#(8vSrwUOWw7CmUjVD4y{Y>aG5S$b>SQpKQmr$@N78iS>o*T% zcAXpA-#2%2;_Zy#i|s){tHb(IzMU+;pqQD*8^G?%@h0BFWOSsq(Y+97Z`EGiP7|-os5HLM1B$RwN_~uU#b?^6Btw)^ zl^CHZDU}Gk6h^66xqA$q53IBjjb`Z<*4ppE;33j@$#0k3aLN9z0cFVY5N2fK(L(@aDm;cl{cf1{# znPxew6r)Xovcn$+Jli{j7d_8ePpkG1E7^P}3`q?42BUd9_mV{DjWQMrkP96GnQemdAUpNI8 zYtBEr{%lqDUF~nWbVq}^e@1M)P<6_4(1b`FJS#U-ym|-SHnCrnQa0TNDE0N+SA0?m z6|X@lRg005l2Tw4h)_yZnO>4TH~N^As<|MzVtdvPyU4>%zSjL+>A61c!=iyRUWJ4a z*u#tk_h=tWB}SCZN-S*GvU;oPlk}`@o!75i3i)LAFG_8^eFmSyO1;cg+K5m}Y~F!g@1>FpEm$`VTu#{iI&14?nq4P}?Y^Q%o0jm9igyn5eeSnj z`-@UNICCj)=^lJ4R5YzYs}brkDojz00L{yQr&%fPgfG%ztniabHW?v{)4x~j#x{nj~XcV4>$yj28`!r zXDS-*TNzW)?$=*8W|;jxrUCtSq!-YE+JtWy{V0idCkIA6JyvEhKShJ>5pC8j+5M-F z&Nm#s+j}B=p;y-G1RC)lbZgt$MJ4R{!%iL?P_!ETI+}fKfAje2p=muJp6R2L_@pc+ zzNI^eNZE*yl9G5}6o@e5U9D2SA$#`6+_H<2C+CIlD`cuz)BdU3_4`~4*Lsi*J z`=2y_-Abym4Y=UazpA?SP+QSOtF@|4;!jb#SUd&_J=oWfM}aEbs<27 zQVMkUZ+nYU>N{p|@KXx>5EyR4>&D>L*{rsBj|u?Y)8D(GM+J~={6%2vq@w~NVkHlP z!}ut-z>iH^8V98m5Y>*x6+NAz7Ev3tn**H3Pp633lO%-ZWy-Vqrl^_wygjh%+kx85 zB){3>ozMT~Wz^;>eoDn51JR;9jMi>7W4!rSeAEL~)H$~B_e}Hojf>j~xkqLU-TG%+ z#KSr9>uYW;eY|@ex3^4guR?V>NAT98{hw35TD1#t5?ns_KJsepGG3>it#*avhrVaJ z^;UzMvUacOFI9}b$WI6^&?yLnHZ)&`rZFvWq4lkigC~+N!GpR=k|i$^)Oju~jBB7P ziap{tH=XIx#vP|VI*xRWw3`j->{A=*j2TdiyRlj0dM zTS8HRV*1(mgl<2&3{7KNF+x)Y^^zLI6tJNFBIm^l@3*>mLFfbG)7+0@Ey^n z?ZcaZ^^3!b**kR(?Xa^s(KuX@&%C_qKL+(>suP!Dp{WP3J5xd^BvL{d0J?WP2tyel zgD?QJ4QvA@F@U@!M9BbyXQTyD8PS7zeqsg(e^6&YMs8A22U>6F9eaC+Y=tCA z{z<6^`P{K}PoHFvU&Y-zZ3!6A5h`j$j5yxZ187#TWdWLc)FJl+Z57M8;7ju z8MO(7$2m$?wd_j`G12tVoB(QKd8qe1F?%(i*5htJ4V0pe20o$v>c@ocfD4U6 z3k(Vol8LA!RhHe5jSiYiysaAO_4wTTRhjt#MPF~dN{TJlI>f+FP`nW`{{fp=zk?f1 z`niCdC8X^#kx?1zLI++fZ7kx$LW>|iV-7x{ySW#l(CWkpO$n_!U-sZJZ?pqU8J?65#Fx7d~ZMKbKp46rS*|P!+b5?)W zc6VXX{^u=?ZzzX6iA5=WmE6n+XN6Nd&c^7;GP49`On{bo!+o3Itd~8|c9;-{ptBX| z*ZN%$g;oznKT2rTUV=mG9RjWL=RD30r7MH^y`KijDKjydKP>vn;d!XkN%qv`WM zGp<=6DpAzpm7BJgnfA+4(FkR6!kZw*?N`nR~h!Kg{~=x70Jl%X(!MWQGL( z!)6<}i#O=QX}b46IOzU;wP$R@D2m|uKo+0SP2WUOX!T))ri50fIO4J}XyxnfIgUU} zY?DE!xr2MiobPId?K^eTK#|wTTyM%|CnhhURggu1t^7p zvq;u%MN_AF2!%jL(5h|*4S3cfM3uyN0NClc{eMRmE z+9HDQ$1^RD$7?b`rlrl^v{Ih>E2l?fMy1vv(0cC{5dQctv=BqWFZ|HDgbc(X))o|6 z0~l|ngw~f}>d?w`;ge_(dAd)%)zQ_>`oh7~GMfuOGKVhr6}{bRv;Ug`BYWAg&9P`^8+)t3+713GFfO+LaWTY@?Pz4p!H6+bV3}0&Q_q` z+A>}={tsgGql8w6E*x4Ve8rrF**rCdIkIAL%>kb!)a(3TE{ouQ%^3A2{gzw%kPdg( z=s}es)|!oaK^v~nFIXUv8#mX__tb4Nw36*9Gp(VH^Z113KFo&-Jp>mTg%%hTA_Q7X zu3MZzpv5n{=v8%0zSiT+!$~@9uTFiJm9M#*6K@-kaC!AeTxMUp{LdhG&4 z%@C3NqxkH;epC>xau~s=Fh!LE6o9}S7ysFf0#FtKAZQ<1wS5u*$!Qqn+96@u)()Wn z1fq%D^~Vc9(4F<$12k(J#aIbH0KpF@Q3c#OKI9ADtPONe4*r5>ZDboWe1AD9Ya{cL zGHX+xHeN!rHX!QyS6|SqtpQOTw9TJZjAv~eB?<@kGa>-Yx<1wC181;qU%rvKHZ$+b zyfykf&06F@Kj!54|M~yQ?|wh$FCzm%v-u^ue?(xIXh8}?UR>T&{j4pc&z!Vz%}ghPU@O6L_b))$ z6*M>~|GN}N(#7%FQ6O~e;Tb3(Xy8JlfM}7V28c?}J9AHmJe%=lvAC<}t7q3YlOE`^ zvI$xQZ6O+P+_&B;9{h0yOPSc6ov|%9_Kya&Y`M00mUcy`*H7k}9qg1#aZ74_qXJj@ zP5gQ^En>$>lN$)VxtkdU1T98rNr+%KEKXp3!_b}@6aM=H3%T69-&P++KxU4@i~j-LTfbi(P31;ANrs{ z7+7aXnhzuhL-7K--;gKHSYS&$+7bRbt znaT2ve#7YBwKd9lM)_wlg(gfq=z{?|Fg}HlbQ2nC5|ME$NEku08X{(%Qf4(nIn=Y7 zBo}^(F#5aR>aNB)Vo4-^BR|oGITG!0wbfT@FUSmJW|Q<8#NQ8)w7${jzFNZWNtAgn z)*PXge{Q&uLr{*B<(W3S5rLuKM4Y-DI52jD6HyA-#VfaAP7Fer-!WKf|Q;6kI&0)s+?Wi>Y{mG8_w)n;AO zcA${^)yI%m85tvaVUjb?OY$$z%;?kNS>Vkzul8Alr=`U3X8#Pcq0NW5!n8LV2MsAP z#iw8@*`fmT&$ZyQ^m_vt6k3cJp(&vy0sI;Ut-S2+`v|nG4v2gqke=)G`VEg5YqnR0 z2ZvP6?X@MwOu%=*=Kwuadk%q2B(RlXs-lOP)=Kw}BaHJopx0vhmvM;k;1`r}P;b71 z421DX0$Oi91LMsU(E7k^OC4HqE&>vEJt^MmhDNS6hBXfU7_eUz}PNx9EUcb%!SJ9R~WRE|vOhTi9{D+m&!1$5S_ z%&!7IJR)jd<7^HLe-^BHwCk?>=b8vYnN}|&=_0y+WcL=S)cZHniYP3@Cp2kvOlW4f z&;VNKpb#O@T71)j7lD=#qpJL;lyvhSK|gfE=nef=4YuVjK3L(pE2vOl#CU!09r0zm zgIMka2bIipa_^R^{sIm~Z?7k3TIfgfkk?|P0we8O@d>Tut%5>}1tWA2;->jepu!+o(>1(wq4x zv{*5NgCAPphm$A-wANFkLF=i>R(QX?V^XH25&t3t>>p8PS`pJmAt+ygsGGLZKq|$d z&;s3cDrnI1>n22X&`#VvGM<=OJG-}o#4Jl>>s0$kNlvL2CBrqDFJ*XNTZ{f(K0_=C zz3@ZpDl!m;T>T0K|Wzcr?eYMRT%|u@k z8iQ8lz3Sg=3|RKkPl!X%*^277KLLdnJ4QcBXvycmp;g0YvNI`-cfVp!U(~jzDL#d7 z1LAkm+oWsodOvL6P0Hf(&FLCCpw~82Y(PBVt6*zOV_tquceLJf-f?Il`$zv~T0ARp z6g>c;JMWAM%>fr0g%%hTA{1I*dhg6V$#9+BSFXa&XegEW>OX5f?+t8TXSemWv9L(s z;pf5OEzGIWqgTX5_AwYJ2n4f#_?l@|a6q(1Kwh4lX<C-;#|Aw2utJXKB-%1Qc4F z7@;YlrLB#)EEHNgr{21R=2geji50{rziDA%UXtgw%`;`F$ExiMp>vq;aFGHn%^h7v z2KUw)UQ+q5ObZ{h?A3%Or~qA*19sQ$V?ik-hfoN#Ge0p-q7ba;Va>Er2+CI=+Urp5t<{ zeC_59X%CC#Pg}OE9J*}O5R-D&I3urNXjUGF;n^*gnZ?9_dEoU`@I@eHa_UwG59^pO6Ji8`?-{yT5o^8 zL4S@dw_^!ZFdrN#m zN2`RRTj+ckq5ta^Ixs++qX8OD<&kg-+Q$_2CWTYj7WyB41rvxiv&0Z@I0fA$JjMjB(9dPRqztiXz#t+1oK|AhU#Q2jkhoeLrcv24h9=2kNpkrcm>~=Zwq}=Kj zyVv6HlQM`e&92xT{E)tpK%DrmCH@viXt>n zT|-1oRnkWnt^_dNOql~L)1{sR1h@!^e=SY$QcITKNvluXaIE`Wnc&UOJ`amGwMfv) zv0KLa4P8v1UBYqPBt-fEOY<5LU-oQgk|=TJ*@HQ5#_MhL`UKeT4U%(!5p?0I^tT5E zY`MlJjaVNn({6KjbJL)zAMw&16c>UR{U~u^g!~$Uiz@>BgFD@JU!-Z6J=5vzQ^gYt zI5tW=%C*bb;69+&!Y!$~ZhQ8b+5?;K5uYAf$W{C3BEyUC#m=1KdO~gM*(^;D?i<8_?S3p*&i-MYaNm$^1!`TdqFTwsygbF@^TRt&wGB+^XYMEk>D2k#B^it@P0v4mSpqi zqq(Y2RvVl#X&+r6XB6>pWyZ+IVT0(wHiZW_bGldSE?*8!B?t&@S3C@yv!%^Ttn7n0sl*7!qaSTQU-~= zTjJXzm5bJ2o`yKwr7^w0u-aNFG^_Z=aI_C;XG)P>fYJ)`JEcfX0L4nxm2@1hX9A%! z-Nu9#hYOA7IABnSP-uPed?Fn=BZJ3U)@jQ**(ev91iJkftJRfOdPnrjbjCjx3tshe z=GpjvZqXRV(44bm$j|b;mGJE+as8QDw9&tyHMYG{jeOxChOG;qr44_+L!l*s5tf{f10D{}hJ$Rhg>{Z9n+q z#MLoqeZ+e<_G&xg1Qnpr0(PgP5y1;B&|UPh2nsDl1X`eNV$O#PT7P(Z1r;?g9bKuC z#0<{=2rY2yGy`q)k(_LWlq@-_kK~Q}BgYHbzWiBe0Z}bYwIRs{6k4GBsl_^I(%6DH zEojewpgx{7X6@P536e(bo??S3l133Jvk^=2+Dsj5v%N!%f1!m~5_;i>)(vDJIxCq_ zXwAlWGbOY(1yhHXql<8S;`DKp9kq#sSMwJsw7#!}Y z{%o(!qb+A(PS&ajvAShE`YM5D*EQa^?!8e#t&WT*){>zGwpYr`OF#T>d!^u%&4f5a zi{)>S4s=2euPZ=zW5_mM8=W_4H-?;;Q8I3L+876g z7SOEn{wH*Mr4`WywB4-#-Cjv880bQawL5-JwY|b`ak|P1ffjR8!neV{&_WCezwkrr zCNdE3IKHAa0dq0lObIP#N$SwDautcUOFr$X#>%(PSI65}O~O-QvFYl(b7BeUtjjBM z}Gz7h}0KnwC7o_d$7)@gF|o-J2J|5<@#dUjG0?!JmT83(Mj&- z{m-{%aTl#P?YBm-t&cIQpqjDHS8QH~+F`;zy!v{qN82q@pVabl(L&=DhE$FLeph4;RN zD&5IR@7dzqB?lC28YClB4Fg+u_6PdZ-ESmkTI0J(EDJL534QsM5t?bu#|TXctrOOW z&qAT4eV+$`R_3?oB1TVs-Yat7PG?dd(_46JuVq@L&$7zzT3XLs;>DKxTMvwsjAvSX z55fI@d$Gh$OctTg0?uMQYblyKEk~E@7T&<7_`LyEVO`V{2fanNg>r~kk^ol zW?Bm{gM%Mh;D?ha1SGj2?Sp1oWTPe3@4=O6P2by1P`(0Dm+1N8&9p%GMD;Uhru7q1 z9kdC`k>i<`LXiiWX$8(JohsA1XZvND1p=+YgzTu{ztEyORq;dX7BUdcs>3L>7Gk`a z5?bCl)S+eODk`pjC35eRRyLUnE$6NtIewa+lRf|DiAMIHch#LT=Zwzi&{uossHKt^ ze9U5Ius7G&%(J|j-Z}kS9h*;(&PS1&YDvD97--$AHlV5>`Y(3;V=s;TVd~{UpxG79*FR;ycvmOv8qo#Q9@t?+MBK_ zPl64s0Ihr4p5CGK0HXCB@~awr zSlIRV{Zwk<6+bEnBsB@@aFqxTQ*8{A#` z^OlZxsGDEfCf%N=!acnVX?h<;+H3!iM!NeboVGmMKhM zIW4~17jv~-Y)gHjrL=4GeB01g_s=V}c08rOmpJ=d+ZD;tgX(a4+-}72!VwVqZ1kAW zOW;DI^Zm+PoJ$LGh7#Sh`-hOtZ_q(J150_L#lXOW<%vN8S+!2L`y*p|fC1}jfa|*cq zyH00LZ5B}e{j6<<(nx#!Q3DDspu+lY1H8}z-D7B1q0m}_Knt|(jrAu%3zi}M;TKMU zXc{G-QD~`P1_wX1prIYL!U0V*F=$6j4B9NtmFdM^!58~S{9S` zh4k^bejk=_tp8el{5-ABycruG7OoBmvG<=tV%N6Q-rA-)W8|w+pR?z`&_YhxHZT2c z%Ck~#XhIyKsvoVR915+a82u=rbt?xBtx=v5MYH4lZXMM@q1Co*uUSU+4Ts3l4@iGm zUH-Af$i7p3UTe?K!irn$%jx~NQjo_nR=)W2UxO|6e?e<&@`QCdwyz|7LJJ7Wqe3r( z3ynex3bkUH-T!^ALZr}$F!}5LU9a4J+e>$ zE#vDzbOG&%RkOy+_&5SuhsItwO|zLI(@HAy)g3|J-MRL%W5?>h@9t3bb106X-kgFA zgvF^lD6}*%-b@Ltq)XJHrQ#|cm**F<=gFGh#g-j@X{zr!B4rn;=z9okwoD4-+C9s@ zvg369&y2Rmw4=h^{dr#(3MlN;%O2S?^FhFm+6eaU;rgvz4${A%MMIBNMD&`!Ew%d1 zI$f|u=I>`K(C;XB6B_?DG5S$LE8P|fElygVb&4ww^Ebp6)XY{dx4AEQIc&9Gz~H&B zuKD#ZR>ui>(1=&CuEI@W`1N%C+EyQ)F{#YjPvy=2Fp#YVtj&)npL3qJ{H3_m^+ee36d02o#y__$IAG0FoT01 zTHuG1C_l_{ZQH!Z<4K{oOurOZ z?<0hW(QGmuGm!N@O{sQ)zw3R79jRPU3PB%yz(6comkUV>q28Q|bl)&*6y03X z#dtF%v>xhGhn9@1gjnLSSA(knf>AJ`p-&z7~~Rp_F31G^y>GsJ6ys*>$l5;{OO-3#38Esm0FIK@#$go zql8ug@@vTVj5vur>rAx5_%Cc+wmSQZA%DEb>i7#@AKJ7f*>t&;o@5oz*Ay@yUb5!e zZ!N%{wXL%IDyce+tf6eR{E6=~+oA6}Nwu{!R8 zTZUin{=u}CICAaz*eTnfjBlm%Vtkf%tzL>kYdJ<}N@#rvM0^|ut@O+Y9`tW?FYX4UdDk+w_UJ$JK){)Gx#IaJmURxVl1#ZxZU3Mdkvjr z3!1AH+5&NRhiU68nVmEzLhJX-gK9H#Cd47C`bk=heb3GSqaP)-K3{@Ei;25rr%I3j z7rRWJbwEzzF_EsPt-BU%`(B;<%t=zt`lLYqA2M4|N$ zMrcZCwTL603Wb)|X?xcYU2%mlkHU*j7UiC|lq=hll3yfzh_Fl6pOLXa-r9L#f{zvB z+WUPaqGQmiKu-R~wWoE=H6EV|D|lw1MIEa#D*V@?4luDCO*TMMAt*0FcT>?VC@+^H zyaesIpw+l|Id|Gt&8SYP8jqVO0M}rw^gjX+XdULnhPRF#be|!~j@E#XjkY0+X;KZy zwVL)|a1aJ;CaJu62VP4b3I-DXoS>QEb%j|6WF zuOLOf{rko+CGsZz89xB;AOo==rV0h1A;z020azAH9e{kUl5utxUb`RPD$|V^O?Z7! zhD|TG@|u`Q-<8-amvdflAOF@#tIi%-q`o%j?n*|{`S~xzwup{|HjRk1{m94)tJL|r zj&VD7sqwe_V-LeF{ywGw{TPeJzKypQqaP&zn|0v;WZ^D}l<^Umk;|h)+O6v8bu#9l z`mrGXIFVpgWFse4{T+chyK76pJInZ1CB1vDGfX)axyA2Q3VIPX4nXAPD~<82Ema`^ zpU_nY#L)FoBe>8g0KuRTApo3z_cJd7K&}h(9!Mw|RAfs;=QbG3j4n~2>7i>mWbB!} zMULi99+O&xf`oSc9=e+I?DY|rN;F~2B*o<;`^qs(jZuL)rGof`o)aj60?-&EG$jCs zvJhW|0q|+&MpyR`UBXJApHK4@4rbe&V7_?t4Y&Izt^g;C1O8vw6;i@FwQYF3->TJy zTK=20;Q}Dz-c1u!fKmuJi*e6ubi->ILLt!pr!aTYh8MX?ZFoTwOd#s(i0YudA*OsBTExM{n14{-eP98fejz%JlG~=Z53~S3$5+8;uGQ! zbhZNh(zVCV?pu%1&knI2`Ru;II%GahI=fGQ`8>|PooAT>7hKtWRAlBHZg1sB+kbv~ zn`rfYEh%~~k5`ddw)XAn;3w&f4jd}kRTYCit2QEsG3^^An8}&eZY#1kmevWzC#A28 z4oayF7%6|D6{Qpy1tN@6_cr&wmO4V(N3&~hUPoVX_~Vu7><=Smitjy>qF_ZLWeJ!t z3@s9|(CS_n*S(bSeuSL%EYS#K^f4sZ# z<+sN^JKKwN0PPK%O8(tHo2Rw~U0Xc1#9|6caU@CX5pAoFa*B&&fW zHOeqTG??>*aH>tmgPVafDBRN$uVv}a``JM!CUo-ck}ovN_$LapiHgS#5*_02LrSIJoq6}EPh$`DHR z6Hnq^OuS;~JV6C0rGR&YaPCA`^wbebfp+J+9g|k{V7Zj~pQRKK?aA8RkhMyx9@Shm zwg9*VGdTDu1r7Z`lHI(@cz3`+cUeq#ZCa)F>AKiPL6dD5Zd?!|uxH z4p<-Jw4nXoL2i5ptS0H64Ek2r$EK-rsYQ-WGq{jk>h`)WZrQ)N6qHclFY#0AJ~9v| zWl~T|ZN+#q1*J;qZK+eL&vlOI*Co!o9cK6-k>YZ8wfd^^AK5ctsV6`V9dTvmpAmhE4z74dzAtZ-4Bf|zXdg7c0Bgd_J ze;~;gay1c5e!a#O6WNh#TQ#P%>sJYQiGJ(Oi{>8qkCeJ;<2gYEXo3Q~WBY^EkkAi> z8t6XX+zy4B1_CwE9yZpQ1T}Ik;`!~u@#N3E?wPiLOV#hMzH&oX{H!s9gP&61hrkl< z9vZZ)$p7zgcD2_4T4&I>pt%&;3Zp^#xN@nAX)C;d0u_jArRjnoAYMDMmQkCK75Bz{ifyW!4_mI~BimF~va{9-{5{wfF(mxLPpJpUK$N_w zKq+N|@n%X&@gQ@7%4=NNjjnTI6BpX=Qf*0N2+2QaPS0~hOkL!ZkL@VQC2nz9dgDuG&Re`pZSR~l+GP4pTav~;!St&l9yxC5H@TE7 zY2Ji51f8uwzxrE5G?%i)=toJZc{xx@?UiTmO}lDz>uJRa#r08KQqH%#H_lCP=a==} zbf*0$W8Ss)GebFBqI%~j@$j{quuxE{;a4uzId2s{DFdD{pt;mGjFgm=0;52LQR=~F z&l0IabXqewcAskfNWV_B-XQ;Ve_&~yJ`L@8y55t@=x%d`y2l%jorq*k;ao9j-2IavO{MiMt}oMdU62+@iLbY{e&^uo4H)ta3##sS1|+i=7+%Lt_q>=fH`8k znUZ)?m#7o3+Fd#}RK#YN>aCPVYxa9*DMfu36*OnRCZ779Wkz1~T_cB0Bhg7O^hrVj zBW$7FN7_YHYGnBcS@nJ?`2Va@bomBo>atX7F<1FGRcaKf9qRX;7@G{me@Y!2 zSh2LTDQAFh&WobankR}kNeShO6jQ3tI@hWaWXJQHVRL=h z<%GmG`3cT>5*QtS=2WQ}QK*%8snn&cDcAjCY@^nVs+iU+y!!Pf^`SxvsvqN>*Um9BYMz}e5*cse zVt1EGzHU;KU&dCcAAH7T2@PboBJflxx_9gu9?!2z(aDE@*uhdMTclF-asOrOdsHdT zcf0?+C9xjIbkRaY$(EljJ?t8dobSI(>Y5ck!>VFFoJBf%SzEdE0*q`{dO#HR&&m9O%8HN`uN0=RlJr%E|g^sG`h{Q4^oXU+&PAG=jy(>sM7fvNKcKm92{iEX)H zxG?ql?TDSnh0a&L`4ky^(yHWs{SKQKIUap}1Yh0{Pie!v#=P%Q@loePi6~vWkPL*1qih3zel^P~{ z7pau(E(gg`sd1qncZBuPGul5yRU~h}z7qLr=8uG&3CTGZZ^X{6V}736rBZyxQvJ7A zbSnZq+5R@+(!Jwt#B`SI*df`Wk1e*0|4nwfc4BbK&hWn$9Pt!XI%_4eb9o4AkNsZt zKmjY33g&7Y{#A-+#6jnTDdEYAINDIH(#I{K<9|gQ`x7VOR}~%{<@*mSL5)(olPt5! zvg0DGGYm)kyQ>b>B+nnuxtwlMp1nD4q$ewC?dr{pIGf+1*4}FUetYO`@?VQud0sxy zeWq-VCo3L@ZsU|^Z@!9>U+9PT3ohg8&73MVDW+$Y%J%EOkI_CQz+89By(LM>9kDtB zLrlj-k7Q(PSt(8*H!?x|RmX78Pmb;fRie&~V#qg{OQ?v=v~qsY-1kwQUS!{p7{)ij zkDPb>ocm1wb1daFTbIA`S%^xrR4Rn4e4HxfW6x8iP8o{@Z%r4d(XE;<(MvEw!uZRj z7W1VZkK8}*j@;2-X=GCf>9J~djO6W3+e;lCgnvME+TiXgrF%`1zg#{ZEW=W%Z|K*KlLj5yrXt>URvHY=J}*ICQj5*v5@=vDN}ogOB>f-{y5RX zZqaKvzgt8&R8m#{#xU^t26n_bY+eVOQlwDWtvl^mgk@$#Y?3! zuK##~REjubZMIX->KC-;2^R(%Zoo~isxrHuKy z<9EWlSGOYYR4Kam&r==CQYm|+QuMLgddD7BigTf9^8Z$)=&ae*jpbLR=;UvePO(%f zl&f+0S1Fz|41MZ;gO7|It{yV?Topf@Y+D2CB)m6vFMD1^(7q7X~|F6xRJoT5( z>Vee+YYD9#m4!Z@I=KBmvDAdKthYkhM(8a52mKuwDw=rxL|OJBN_2nh_=cnKr)` z<(W9|a*9~U5liu< zQtywtcAJNKET3$YDodrpxyr|>QXv&QRqDE_=#L56L7lxkmDU6|y!iAc=SFIVSKAf$ zX}u$yTSRq=jiMrFbVkgd(x5Z{;jWdQzRiIO94dwOwreGChT+i5CX7*;lO|4o72cm1TbGz*WUO(=;Rk=MtM^q446JH3y8PWXj2 z+no9xR~>mrZP3CN*SKnxzQ3uI?X(E~GJT**owbc2k}K2yu#JJIT+rRnZN~tXTsR=P zppQ=|tM(`toI#E5|5h&OtnG9j$gf<`$v*~PV#!4mS1s``7d$6b`qVi)tyn=#2bu@y z;}=8Ce+4zf2X`FQY@PQ{LCv6xeZA~ZP}6=}sK4I7K@HCdl{WI6RC($+-4M?Q+wzDM z=nn9kj?>9^{lp(>?&|ytw-4Ye1`7IXI?i)C%|uz9GwL*J8$&c#Z|0PXrFlKeMVjA0 zrTjO}{zFc#FfkB(`^w&Y)s#Brko1~0LS*#&K$}~gPQ4c95K3=5B660^hk-L2ZV@ep zt4NC)WiPqaHP>3Aqznk9ZU3!YoH;y48`b_lFA&k?dt!W#u^gq0;VK`eT*Srjl#9DV zM1!TWD&JJ?dv?8g?vR4C?ej-1pI(!?ugJ1h_;9OK_P74Z27{V%7@y?*j(Frun?>3` zLvk^cy+D-ye!TN&KAtC{bEzD5j}eD*8OxPRPPw34f#M|>S7+?3P+ui7T7I&B@yBf6 zm0v7Ee;BX#k@axO+-ab{R`uC3f%IZUW5Ji%Z#UJq1fH$dFYe!7mTpKz=}Bj(u;qgF z8bDT1Lwq~NU)4tKy~jvEnO?<}Y0jW#7y5UeLCr<$gdYLX*-^QAb5&j2ErhDW2Pa1~ zsh2q(DDSr~Y+QZ)lg48Q$^)$9euy%>ihgZl;4i2Viu~TK2s~AauJ^0}eU?fMM=C`h zpLEdfQKkML1~qinO6`pJRVh08-1GfeU-Mnf)j0gC6wk?!KJ}fDM1F%BI{DM`L{?DK zRmV<6t9}b=x*md3QqcbO8o=RyZ8GGkzjRi2TJB;k5OtzXOCN7Pvg21!6Q$|H(%+nd zt26%LQQbibh0oXCD$ADK=sIJ+^}j(4J^p$9cdl-g6*ISI zl}hm&q;&AfNdF-lLq6`$nd5x6*fXpowO*oXK&a@DaDmpZA?rE?JVZ{H>E>p-`qW;H zd^Fakrak-hB9Wt(`7h3F-q9)F&}Mt?zg4Ma8_sr{hk7g@DU!kpYT~%c$Ei|@>O58I z%}|k`vqh7~-sqQbae>4El~q2rwVRzYH|#T0UVclfTHx)mHZ_5|RSL?=az3GM9b=uE z`^0rusk%68{&G2JSPCnsS<976PL-ltf#Ri7*JdoMP>*e#UAv*Me!Fr+yZB1?wl*jd z=>1vZ)%>?j*-7SYjBB+kpQQ{cO*z~#U7sqJz{T9^J#8Uiay2vYq zb?a>%SEe~tDs4aNqP%XsUAR7_9jTN@N&j@2$}kC)fJ))_i6Q})nHH)t^|AMDm72EP zuqvB*wmFjdb>!*)Ql-2c!@3oL=c)_c`xk}H<1zK~6g4{ercd)&qBa7F8hxBe&goIq zI76>he=YRtvERo%2w}xi>$w_-f0d#g>6+;F5ENbNLwk!@>kwVF05RD;UW0SKP{QZ0 z$qr9JrL+3@Iewu;_q&8VRfZX2QnGcHx$Z*2PI{?W2O{9DXFQwneU~#=B7E&7#g==-tQMWfEcMLR25* zP^psl<2UMs^OwtOqZ?Q%wTUa2oGL}P0>x9MUYH@3^7Or#sW99+AYQ|8u0ugynDo!H zn-W#sX|nymLkj5_{;RO z*$pg}+RT+{PL;Z3i~1-}m9jPyLn@{ERy|87ta+8e3dfK7=}kFj@(Vj9G{c?y2+5|s zD|+iF7$#WUSKH;+0ug^(!Z#~!?p6ezDn<7W-JB*K?-JsvQgrhCMWQT~azwKjef;%h zO^K`su&(o2b#rC!D{) zdOoJB(%Pzz2+{XI=FSQ9cK2PdWD(-71tLySyZqNgji*Y{S#7Ag&RQTM+3kC2!DS)3 z?<}6(MWiuvLl;{D%din~B1U?U70y(9qV9`&Xq<QW!JcYmd4f)$Oa3&tER*5EM(LwsGZ>Q>Exupm?a1M&^t@73wh( z5o!(oK2=BePndi}dt+R^PnCs%oVSo|VYFhPK$?-FXt7TA{@exum2YxoX?Lz=+vvnJ z3W;uw<5H=KQQNI-`O9?4a*Cx=+qp8$sZvE{sFU(isq>jXl>(x(%DySKwjElw!j3pv z7<;=|A~9xCRxc0g){r{=qu^3%5Zo5;L^3k|OOt2Di#RmiC@4ajJI6xuLkPoVKll07+H01~a}DL>11l*pA!PL-ltf#Ri7*QfuiR*!By-Qrxh zXWNi-HX$#?Or|Wl{H*HXmg73L9x0`-T3grE&X)M1?*1seMNsQwMC56S(t^+k)%G7l zyFLVrUMRter6xu_NI%10)fzb9&33!EGR>({Poq!==A}~SGM{P%L=W5FET>kN+<0%= zS~2yn?MJ%E(p7=uj9{{YCGWO7Zulk3Jvv@>hgMpJiA(F_O57 z@ZavlpxZF-`2Z_~ zB>1bP!FpLN#oNtQOZ+Py&)JI3iF`r^zi}Cz{OJ7*R$SIq$8*Q`9GCq+6i7O&m!@7~ z#btCw)5k?W&i{(bY|Yk>d?a8?L=Eu{R_ggnAL37?tiINlW_PMq3^IZ*HeEu{qf+$n z5C4Dh`*Uy3MGet#P=>X}wTG)Wb1Gg=MbC;C;-{y?I6P!Y#z~h7<#s2Q6BXRme_{+hLA z+Y+sf7AZg5I`3|O|LuFyUSe)PGA{pnf5BUChiMuvU#9izg%XvXHQwVrZq@OZ%VUFA zu;Q}4T)E^_Jh~Mqo{IPUX|;M()#HcB3WRl^{Ofy^YQuHB1d2BXOMXu~AX}Pxj+`(| z;KG^oEgkR8B5F*V)Ka!A?HJ7z9J^Af%I3GYOst@ozf5l!v4Rzs?c>Tcr{aB3?Md;h zcA6r^n?v-@{xl+#u}C6{nA17_v|wj#O6P;sw?H7=$+|>UFtjsmm&}Ww|E1zZ6B*r# zK!+Fff8CaA=-z+s_&b(LjY88Bef;u#*>5V<6=-!Ho_efHrDXov?LeL?MQ2TH#|M^5 zk=;rLJ(bLL=P{N_rEoP4|0+d0@~pJ6Ti3ASvaU)yHaY6IxUB1TAm`SKRevom<0+_g zrOi-W$K&#!=iNg(`N6S?tT2v_)#>ANxm$mQap{pQTlNW{yN5qx0{-FdVQky*Yj1YF z&SnN!Z@sktUzI|gi07|Mh5y`}^H4*?G}N);vi)4WnNy`oVtQ7o`2l*#`3c5L%r@FB z4fmAW5qYB1V8Fha?=EMZSG(Qzsrb}Z84+zpVU3OKjmU>Wox5su?G5sh9MoU_nEqOc z(Ruu5%;>$n7-n5}4`o?#+1JN!*{{muSwdt?EfeCed|Lh0SSppuRX#6t685J(=D)Y+ zsZv@tLV*F#R;LW|9yhvd>D~8_63$yRDSGsOJO5sL{0EuZs3}J}hw8Ld*1xlSmSSkD z5E3=X=07UsCFwcdri718@N_GT)YD+8)B&zsa;g;F3KS2OQp=jYvsyJuMs#4C|E#JP zPXe#oy<{{b&lzgpx4_h6Vgc#px8{hPq7V`EF8Al!=;x0G3;O%#+TFK_4(TVbP2!hI zEt^KKaZQX8ddNq}OJ_RzsRm1>4svChQ>A|7pnB#Nmz~XAHUg=XT3_)~Z{M_-hxhsR z=CRz6_Gwu~`&xB6Mk&?oCuYVEtJfusB{~Wm|4Ws+I4?#U{oP-?NqJ8FbP*=)$lwv6 z&~x)|^D~{?+x-$t)JCIOj6N=uJ^P!eIXo)fOMd|H+{x-aU7|KT;BSRqJXMO$n$W4^ zER{OM)j0gC6wj%jKJ{>;i~N2ykxp*-^&;zgRb7=f(K-FM?^W>5>J;zcuYqsxFiZ07GI4>aSy71qf7-%T* z{PC|+`KTcRhCOH9T|2_nn>lCkhKla9IQ#CJshwcGRB5BQc&c=ar-x%sK|rgpqo|+n znTQkX$<835Z#F_c;cDZ=m%Ytf)+w--{kql{GdIP1vNcHO;f`*z3C}S{=Tc}9{`5r} zS1vhc@iO%uX7TLl%c@l)qs$rZg*UdXt+FdSTsWp=N9KU$t7k$#G7hNfRJOJ+sP%Rl zIyF^j60=}g-=)P94WcKvwjNsWeWc27v-rmLB>pm;d4Dmi=PYhz z<{S`pyFAyA!n{wES%v9p%XGbaJbKBRmdBuBOCNjge)8M2ydlYWD?KgOFP@jwH7$GnwP~5B;Luq+ zCjE-vSc6V3U$TNViyz}^od4r2PM`YG-o31av+noi=)d!u^wG&{-lnh?&bsQ@_57~i zHX%Ou+7!zD?b~qQza@Pl82A1a3D zEIz_PpzcAbPiEcg=R-e<P|2}I z(lv{(+rr1LI69Y+^;1}vwI{i9$(|60dk~UOzUL8yD95$9d&lJ?$4?$lK4z1r(7xJF zUGUq9#hYJ+2^fpUJbk?R)Ro^P;EwB= z&D>Y!e@_B>90f1V9A?ewr@0!3|2ds@{B4q^OTAoV1?y6~s}>$E4EpU-drCz@B&!x) zw*7iE$M>&YYV!;t=&a82U&kYe;5nz$$;(HrXI+`o4=T~eYSF8HU77nYj(m26p3|3_ zoQ~*uPQST(_?ju>DzkMjho;P5_37U^9rYq!KmM=G3sFOSxKqixe{_bcH*?-U>YUs2 z{iBfq2FaemT8mA;4A@z^$4zkaD#aIto%a&=8Vc985hvP*x7dj4Bz}t3Kh&EM7b(6Z zzxc3U+K!bOH-!Ypt5Wgmo!S0c?MX|z?jI?!*Y*Fj%Hi2m*lix-Ia=xRdDr1ry`^)N zk5jW`)VpgIdzHg^guv3R;ys&T)z) z!kPWz73(^f8yj1be%(JR_mZp}AJ$^XUoO*h@mtf*a^;d!v*=dfsadwUZOmAtSs%95 z_c;>h(s!>=`N%?%3x0k%r*nO6g`*xdtS&Ihl6-d5VQR3r)b{e;Zxilh-TT?ZN?u_O~`gq_S>EABT54<8b z(KElJZ*Fo|2;}qELZBXN$$S@n-@*m1TH;^-c!~;LYU%#|tVN0LmBv$4=;RxS0M?=e zy)%G5KDT}GuSE$1nV)_ek*GxFnkoK6ppfFAUSu6yncd&` zdNYUqJ(WP-Jv}326JVIE;jOmV^rWh@zU#iYVcOwxnq{NqcG#|*bw$U%IkfI9`F!M$ zL%_sN4;p$0ck)2y=?esTNXELQMraRv3 z*flTI<8zC%PHnbLXLw|#^Dq6YMEW=VY41pF)JJ|r3q7{NvX3J;CY=yV<`71)?Y+QuPyo7Pi+ ztZqf%DMEB#shicv3c_5_d`chZU#j~p2wO30@&?wh6`lG^gf#vC);%Jg@=9lIM`|07 z@UqAL71|5OY*)A%hkwnY9ee6$}_B65ceyifU zZ1sd*!SR(J|BcyDFXHv%DX%Y!08C1e}oI~gYGx( z)h~{S*f{8CuU~ty*_!pokNc_kKI=9Q^;kZQTqBldUE?Yrr)C+?<*8YVM^bfCW!YO( zZ3jfH+c{b{EjwiWissn&Q3rHmLiLC#@r)sA{!y!Q-M`0X7bVVVej0b@(yv#_u{0~d zE0(`pPGlOhG%J%UmzdF>5p zr@Hqxl~2AW3)rShGD5y+tujd40$Bkf1NA?5q-6BT(|^>fpVTkSl6ltcKI4=6bpDD^ z|G<=0gzLPEz|t&-GSp#tY1V1WovQ<*tjP&&_1EQA+qH-L8#Zn;*r*!nm;34I@)Bvr z>d4x4(L4N`+SW>@%w}s=dn11_8*|62ZbhJ{Ui!ao`_$;MAbbgb$!Hv!r0C<@TU~w& zoW{L5vW|7jjAKdP!0&&mS#;L4C(q^)681O>P6+R0-Dk|=YMlS0X3;qjT)&)kecn}R zi_L<5yFRZ@^^Kw{?bVFqzclN@U(+m}B1C6(TgiG>j3>-)-zwSFzhb zPqgn@gf44s(j9T(UAE_hSDqv1{VPIf81ek^6d|4$6LiBwp%@Rf3D=X-JbQC7N>7 zX4Vde9IoPp2Oo`8*SxB2l8?PBn7ZSPNl1g=t0-9p zF(UeV;M?qURi{&=df#`~8}~$YHk^#SEW*|-v#E3F`;109^8Dp8AgzU@UlEtfl}k>| zqFaHdW;x`()(Z1GAXucApMK-YX4AbR1uLY-3@-I-e-L&tLMKePuD7JsqVXLw8E(qr zx(0%mJq+A4GfL`3vR_|QS<%bJNw__%k4@KoMlQ|jOL_BGgz2aIurw==cM%+I(c0Lg zC{#vXnsw^h)P%q&t6Y!f@=Iz0A^jLXCC(pDd`%3^@u=!cPWbUP!&c&`ZH#B=#!oH# z@ZRb`SAN&Vre5oQ--@SZ(PLr8^U8x6Z zJ>mB@CY?ND=M$F1iLkrFnJp)ONt~_0>*=i9m?rCX4D4CrPHz_;SJL}^wyVwBJ>Po& zD{*M}^ZfCYIG%I*P1G>K7WFL6D&XqPoSJ1_(X(c$1Q_qhA0g{&s!If^C2I(6l9d0E z`yjJwMb@*8b$cjrUB%w;LsNdselPp{fyJV&_Y3^St&+~_pZ{}`#(nSX#T^dEEySar z{ztRsjQ#oi&mWjjn#;%5F}i#&g)LZdRv}mUI5lhH0iK$*X%wS2;QL|s#c5Z!oUEN* zad|;mY4rC;53|=gbVzSg-c=LskfJuQZIy68(%*kWZjEW&ks(;Krs%OWYx;$W_q0(P z{qwy+I+wZ{maGZCh%1+znnkw)Pt6*h``UB`(yT1$t=2Jjn!YSOSFQB){K-omCrgv= z)UL05S!?%NR;U+w$ZX>jRV|0m(fGv6KjZ})R7c~HS$5dyyEvZ9eW1BPt8_Rlq;Y4`9+^SuvwMH8dSCce^r&P&>(^YB{IfY%7BKcsyJPHb zKQ`)kAiN}ojWy^Tn;ZH@z;Sd}H++M-VekvL#;#k`>s9*kSA;Lar?A#H?{XF4AJ#W} zY_v@WUbFhcJ+5={-ycv{;N2gNjhB}DjQ)|H8t9szbwo^#PAsH={zreQx2NAyFL%Qs z24*Hh42(^Tj1A09hMJq0o73BUXU_Ky8Xr8@cR`?!x7TDp{{^0bZmN!R7WjI5sRsF} zdipI1n&TOyx_C}tkoQv6IYFv3ef>P=`1<$-23Z&y8XFqA>8Ywt^bT6OV!@L6dU|@+ z^q&k`?5{s}!IC*kS9p2P)nDotICzPlchCaQ!3aJ5{RbQB8Jg)C=m#$K)c0L57a8^O z`Ss90N}v~qzSd663Qis5*70L?oS3a*k!K|lfO1R3$-hb_oL-h6u zs6ZP-Ibx`gmbpu%`>9JUl$>0mBcbB+@X>=Sld4r-mdAUlz&?TH*2MDdciSb*G$V4g zI#!%-d0Mj|j69qIE|Oy5stpJ83x{<^bT;3wA!prvKhMlYz!WtK{i_h;&eqw#l7~X=*7E79ltWOqDdP|a#0 z;=XnQ!$(zox`AFm`t1>UhX)B4eAJh-&5Ov|QPpz#*StW>(-e+*!EDB0{_0(<5EtPg z?;^0|DaHd;3NLv&arNo-z{pH;UH$gsySHS7_e&cvp=r_G@yjQY<+EE}Z~x-%v2bnl zIPu8gSDLP^?79ue=QsPduQ%tf2peRsvZkX)Tt(o2I^sFg-9Zg}@eUq!kGW3GIqH1# zdK`5*odISF4oZTHOg4PnoBu9uPn%R@{o8>Citb6q&ZZs3_O+|g+LY}u_3=M9XMGNB z?MNxVyMLipZmwLfe)W$wqYj3Cnse0Ajly%(jkL?065y{^we@;H z*>I^9s`dROb2K^!ESs^7Ss=Ihtny^>du1xtN~1grcCmj?c|Z+`xENUMX7u& zS)I)|6CxpZE|TL$M)0%$sPXY;8UBjEl(k?L;Ti8DutwcBdMe^M>MV8^21j1Y-Bm}X z9h4Q>H`h_nMM^^Hxj9i*Cd2Uf)@RSSwRNh)PDVY?t+>tJFy-3bWJ-_XKmHVjj__B6 zlg-;$qwYCZ5&n;(?k?(;Pu%cZ@m_G9nsd~J&+U2CH3pjPVaPHTUKmp~RrhGKrbt%b zal`yhT$|QE&8Bh0?ChZ(rp@24-CAU$lb-i%uE=RkJL>zKr1pIk)J2`n^Qu>_*nA({ znQ)q8)D8bR{Y}H4_9gz5E&p@X(dFBv$6$@Rmt5uJ9CcgOd5=1OH0t^aswB=d_&~(J zS&$q4W5&lJodNx=7KTh7B&T|OW8}9RVrQI&oYJ>H7a<(pn0BM0lBk=MpGkIZVrkuE zdeqtd(z;v4oA}Fgbq0Rc=oMF{IY%AcC_G1+D2frmtygh^Ph4w#OVE>Iv>hI|9 zljuK=OKN>|`0M5ZVpf?v{eqVSSqq^fcJV2imw7L>roBDTe?=!UyvHAA$n{5e_h{bT zM^MfjRW!}jj)HQN(4V0B>mot9xyYI3^>Ko7;plJC>?7MtE)G>69k|ee%(hPgteT{2 zR_uoW(Jfo_AEm1rXNx}Kl`TVw5JWq=F=EF(>^bp`6W5PZ_Y&4%IRcR3i212X0qh)!dG7oI7~_PwJ+=VEM`5;o`jFZ?8r2-~ zO~}Y27dc}-JeGhlE|^zsBY+r(nRu!X`D}g^m_kjQKoGZ)@0HwAQgt%s73-Mrgv|nz z1Yy?{Z0}>Wh02;r5O+}eA<2Vb;|$Cuwu`{d74zU_i-4Mm`L<>uwS5*r+(qf7XHzi8 z9dlCEP_XmBoTYC8)EvwPt`=~E%^QbiP*Xh#;vVwdUoHTe^DyViECFgh<}0FpaD&ZG zdVa9p2is3b^kT{{B#8USH+qXDRQY0-a1DVQi!t}x83N%;FzcKM0UJNeF`6}0HhN;N z1o_TYXTT;TEi~t^bc4k~nB&j6fqF3J6IpKXp3P38=P8j8f_Q+^o33pmU#!4v{!J6E zgkrAO)`IXb%x8?Xz$P5?ma&c0y$FJMh@NO@J+!gJ)?$A)D9uYr)!hY|r1dlIn{tww@s0l}!!g`yH4QO_iW90kd(4GOXQ+ zStn5$#w216J*o`EF3i2lN-6Cmf_RF~qg+=@HtfNy^xg;_Cu6RbHHO5!m`$~e!4+N7 z(CI^}Zc(372;v#yNdgSGbO3Xe`xr<(h*>Xw47eV`Y<6G_C>_SU?oBIo@(4jZN4`rx zwUD95Fi(@#1J~o28^`K_(h1DvGxea1&2|#4l;0_Wc!7L(F-uA7bj+m{9i-A(%wOa? z$uc%;YIl+c&tdz)rQ0Z@3k2~J`L+*iAc;$upFOdHGBzKcW&;N=V|!qL4a~cOS#p&P zC|t#?C2^XvyM~_bMCZwlttKV1Fdw@x1irKR?dKtIE*smue-44*9L$1!&rnnzLA*x3 zvc=EHhXt4yib=q^Ld*fy5)fR3`TQseFu#G>dE*Z1MlnIWLB7*kZ;%PMG4EmwfZ#ir zqfG~Z`CZJLp3SCi+#`rml-?-#8JX|^^M-RI1V6+KjU<>q!kjM!(8}gBQO~ILPYB{I z^4(|9K~8^;`Pd*!5PN~S%F_}`*sOib5>~&&_R~H^)QMLFQHFdiN;}CFrI_;{Tf&UD zn9HQBK)eidV1Fxkz-B|2gVclyf+$D6YiHMyD(^9;y3Am7vU%*@8H~dpuzmIE84RB) z%v-~+Qy)K~yCKN8HB1sRK4Y%DbrEKL!CWU<0xDlIuL<5prFo0-Z)USPvevInD0`<%wQT`vKV( zkfo#{TDzlJ#MKBI*i12u;S$>*SPvy|O+s+R;wd_?J``;hQV3iHI(&QK(cIV#&3!g^zVHflT!lfk@?xJ})a zC5cbS_mFrMxknE3n|xzfFOONW));L1VQ!Ez0Z?G4S2Tf3Y|gj4PrXqjiE4D7mmx}U zLItzOY&Y1gin%%74aTWqhP`eet&aJf8iSdtK@y*l?@6d6{j@Qk`l14}bucR{s)Aa7 z%*De8!#g%_CbFo&fh6$-`HqdKA?*fZ{yJ|U80cYsnK2OR**rkX12Xin{mt$#)Cfb8 z_=r`KJlK6&vB@}Lw?=3J7P!xs7 zmY6qG>q4Rx=GXCq!4+K((&>jQ+kwO|%<4v|l&K9#d`EW2PsJnyeHEH!FV|0G9h+yb z{X|}L!1mQ6tI6fVF;BQUj}mkwi5irC=)TF+8T5uQnqBk5seS0hDl}&aL{M|aV7_xG zg3?4!fztNZdrPPnh-(p>KWQS<(5@|-KhCy>ooF?j=F`20!Hfx*1@;XC#fg}ISU#an zOd^Rol*UV;fm|^avlM86&os;@-3(#Sbj-zT4B;D_<0ete2v?G*N4^`**uWS!%zB0X zU@;rBz_I{nXR{Hp6ms0L{lo615bS~ZbXf(pXAVg;pz}OBbAqwJ3-jS@CCX?X=Dkfy z)DJeRX(>~e=VLq64`eR(A&DQzH(E&qEEi$k6KxCweKE&eG={Hi-gCnk(ida9hBw8u z@gs?!$k#BvofKP&*<_I%w6S^rB{|3r#P+%XIi^c6Ni?GLUOS{faXIDf zGvOYapN*akTSKwk=*UM(HJl`xknaPXX7bxg%;k$M;cO)4!zU~uCG#fFoZHTe-}1_gKMyT=A3rQXe~*!p!7eaYe`}Q=BujaP{!s> znnU5>Mr?mFawyE(gjs6nZR*Enl4wP~Ew=ObeD_7F!}7zJYgdhd=|?bo<%|J^ zG|Z;&#y}~XTV!*n#m7jZ6Zu}e(o9;O#5^8d%PE|~Y;;-~O4&RsPZ?5AW4i`}WE!NC z1c4$sg-2p~6&8>L1D!`_p%iStf%)Mg3kWR6%-C%KW;Zd{EUHE(YA~07 z9}OmGsM2Xfy~aQzn;l;Qb4@)-2qXK^j}%P)iJ9dkwN4|Gf>%bjJ%(pbM;iMGi2@7+ko#On7o2L(vp&K#Z z9$G}XZvsLMok#Ge8w`%eJVxUJRIqvd%nNXME4DYTN}x=(1EG%6_tShw!cNSeK^AJ+ zTzOL#jwE9H>e+o@!7j`F&- z%&;Mw%G?ixCi2ZcO2GPqn9V=T0GmUYZKPcR4r3lMCXw2D1PCpZ{+jY5()}1_-^6Cp z$_bcZo_F}o~#Mg`mh zVh}pd{+MRc<^krX=afM2A!dztN>IyY=PLu@!Xs?I_$-}rcmjkj^4;@N1f-s0o_xv( zh!>bok2Z#UHY*96z?zrXUb?M>l6?)tVC4Hk381nRGb7U;^50^gAO2!kgr17&w0H@~ zMV}$3IXv72QqkucXx?_l1-yD;cDcKYsuyMuCdhv2Q#+X}hIw~De@GR_T#z~dyd*Hk zs|^GlNz99O4}{ljzV@hz0%-2i&(AkXPzzOz6WRvBO(oi7R_4KY{vih-~Z+dj}2 zve^t1-cs+48H5Eo&pW{i^1K=5aW3MpdI;u?lO@5)9P{2GRvxgY-Swm@1Uq659G*l`P7J~t`G#*(gomRsM=zQP=f+?@bbKPrAB%Ze?nD^qjM*(U zo4Vq{Aci5|iJ!$FZUW}Sajr0bBIYp(uAn#xbL$CLc+2Lgn-{>2$=L3g6vezcg+bV$ z^Ju5Phqco&m%Nf>_{_jOSW=pyMbBXnb|_87h)!~x7iO2TS)lKY*>IB^ykm2K&}>MXhwb+d+@s8V7=%6YT@<^5 zK`p|3=2{iw9h*s!kBlT=Z0|VtkuhsAW|90f>hlr?;edSY;X8RH0JEjZ07zPjIU{ip zxCCOZx-tl)f-qkY3WXvzANRRPeFQH8qw&I3i!; z&|{#v26MPhU&aeIzgQ{HNRGqypJX;QXdQzXiPEPJ>L8mpV6NG$11~mW*0b&pn>S%T zQPdwMY{pDA+@s32FbF5)TRG|pd3qaW`7~kJydCrDBPQUm1M`gmrXY}j*(}WzuCaO3 z&68C5P6jawokt>?31^ZpAB`FcTXtiPzBCku@4+lAVFiN8n4g)ZQ#Dg^x^Jj4hZw{d~0(hRmY@<98w9_#U_eXn& z&N2vRH(l9g+bC9JI$me737vKc< z*t|d53APqs`{&bDlxh)!7>|78#*y%?81rm27Vh1|Tw6T|VsBv=A>Qhcs~JF$MX~ zn$k+9eZu_YohrmtV?Had23DUjn`x^-C!432-J&*sWe`)5ufz-?m{o)M;x1jVs>N*o zP8Vv~JP!uLg*t2xXgp6jG%$#1$aj<6J5s6<^Q57YP|N0ZuO#6}6SkikD+LRhF^|41 z1F7MAXTFir9hh6L_kub$4{PoPX`R@vvsxGyqMapl`l#i%sb-QQ zW+0xCFAg^-%oT$fj5H=@tAP|_mH_6vwiH7}5VM|D5p`OKB3zO0s88R>a1qS5JLO@P zDCXlA6hKN0bA`JqJZ7`Ts0Wm%1Vzk5zIM^iNkeJO@gtca)f=;glmHa5IZs^x*2!Rd z?vj<1Tpx;Sc~q%Xk!l5b%1ak z%%$!QVAvnCpyNttVDrx{52)CI6yc71Pd*le34<}4y#L5B)Wdwux`y$A%?3U-jAQ!P z{&-<;rnw1374>xpMa)6I%Zoe6 zYZjP8zZ=3HOU%hCMljwA^Q3h~(AygGx1^y^%;t$_?o!`|QN&zy9)*?$GSd$8IXweN zw#V#HW&jf$*lBEyL1sASO+#N&2S-qZC-U9b{)P;2!d&@@f(fHA7sv^Mz-Y|n@q%!Z z&Gjj%)bz0w;e~u>3YL@F<1mj}rwsz*F(1g&hMR1@Ib{m9asowoqx5o~?WFb;%zIi) z;1!!kr<%gPso4H1Wf*htbc&dV(yL_Ekgcwma|WrvtC^U!gH<4Y7G|?|YB0$SbDo$w z2+hX)blhpCo;yX%N9UPm^O|g%gSkDpg?v31^JKGDGR710mW8e42rtaKlaEu6y(z*6 z`MxrHO&;*UJgT5KI4!{JjBeI)abFn27Fg9YK3v&!W_Hv%)MI|s_yoM3)~5{#q>Uv!>hL%)+xqA{xp%frPO z%!e&3VPP!ht78} zALa{}GbyhWiU>fyny+4wMh7rYiEbjL4`NO#Y9ep2+25v_Tz?4LN5?E>${(SKrO3Bq zhcHwf#k}!{5!^V2x$~(BgdfLDE;I$36PUGg-%$5XQbZu~RWW%@qK^@<*mtKeOi0K4 zHcuF2&SD-?m_QvoM-f3Nec++K5O5LmxQI#Jn=u6q?w4b(I;Mx{mGr4w?a4PGY4uIco-5*_gK)BIrh%;r1cXx*m(+s_|12G2sw^T$|Gazzvof_wuqTgjZ8m>0cQhyAxO`)g>x zl-uky6E#5i4rZ-ESE;1C6tNunw&sMXW#^J@o}C{tWY)-HqfoHv3yOkq4h+`-iDb~7+fgB_M(D=)XQ>;2t((IAM%7e`VMnkJK9J0 z9`kc8ZLt1;+4YPz)UjFT-3}`HBSnNG-)B0{$#I`CZ$C$Y{uj(1)fBvA^RZS6lD=ZQ zk763t|2suQAm4$?-^ib}n4N<);AtJ^#bTPUp&oPM8cncmz+4!9mb(9gB32^bN7I|h zy-k=6c1(bXX3Qqm6Tzqj^TZ1ip^D98F7Kq4wNXSQ@^wA;njGGV`A&%rXcA18U0SR^ zykPVGjVPWXv3>p1iPTHPQONg{k~G9HF(0e5gSi5jC3NheuOMa*Q+v3>X3t;==FDD9 zA{zOga~TQ)L@_5QZ-ZW9m}ix4gDf_SE#3}+;@Iw|dzu1CCJ}>t`$~TyOQbQ|I!Zu# zZ_Ml0N`SWv=AtKd&`%aKQ(l63tPhijMZS#o2V{sm=9MP(N%jO4;f^Y@|efY!-4UW0p>0 z5*v|k=2}4*IvI1;93%L~=J_dTkIEEmw{Cbr8BJpnn^5}5OK6eW6*Dtd1nF`hsGsIhb#5L$TFd%<{d|p_a{6Th-x+ zC$?WUe@Wf(ViH@>c_x54?4FNVP%0EceK1Fsg@OJ8%;I(t@Q%${`M0USMNA?d`9?n}gzD)LP8N z>Fc0_%}Nv2!})dCF5{=jwBNua5|FQ#c_S&g8T0dpMo_`#t2fa;=PlTNsKOY=#$)#Q zevi7dl}YSGzA3w#$=y3JPo3ogz6qEGMvn)zotQs791l;~yyEF&YU(Z~k%)ZfR@al7 zdoW)>?}+%x=D3-bkerO|X%&_*VK3%@H8s??eN18(^7Wh<0hy_orKenh%?B_)4ZZ>< z2QfRXx&oirY_atUoIHf>cZ|MJ4-YemBy=9Xf$z!vM=@X7&`vHrhB-Q~os>I{*+fqk z?y}inO%Ap2B$L>UeAmDDLYkk!e9Tl5gwrv*pOA)hHrp0UgZEi%U%ND%>VJ+&>_NWc zG8RC|Ma(lM#lx};%(f}! z8_J`IN$f-CVY(E6-c8KDnRgiCw=h4W?lLa0IoA9xW5I1~U-|tmC2*HXq#)lfW^Lr- z`Bf%yjJ@#DF3OOpKy}ibxhz5ClX#2nZ?|5C>*}Ax8ysRxsy`Ig5&z zv&S3`M$C$s6?4wM>e(CRoO|y5?p@#e-dpc2S-RMCcU4!X>iX^f-qoE#TFJA{p#5T8 zE4k}gv>kgX;MzIRTg10dVa2l5g=p8vt0K?3i1wg6RphRh(0*gwT>hHucV~~X$yY#c zF)#b*u5iHR7>u91MR4l_2jR~et5s0eAP`{e%hjddEElNCA{p|ice%!@1X5> zvAX;<*~VEl08%Xy;^=lOHF$ z$D8umwj`#O_vY@YI z_g^fx`-b+bO^f9Z$?o?1V)=saxcsTnBGyVK>aFBubC0;o^%>fADov8FCwo@QN%9E5 z<@J+puq;v3TgA8EXzwWxE{8T-vPs_30PWgG^5r+k4tkw0pIRQ5UthbCwWuiSt>$GT zzM9Lw8lt_Xd4c>!WwiGt6v(GmLA%ql0=c&l+VzI+W_PQKdTV&uS2y(Ko2sLIFVa~) zwFcR~t>oS{(H_&cmHZ3YOOBL>IkiN+wY+RW<-4+|x@fz1s3G^Rhql*%8uBk>FFo!r z&ojm4hwdF@e)UDYb-Zke(-&EthG?7BvRU5viTGF{Ow?CmV?-WF}^iO%v$ZfM6BIm-`{9k=-- zA0vr+dA#hTMh|3GUT8N6ts<|~9&NW3RpbZB?tHk4Ji{B8x36X-Z`}dyTS;q~%16}O z#Otw~cT?8F7j4CvdUC@~Xx|)dDnCSaqoMWXnSQw3{=f`YJwVjU=VhmTxGlRIi1uzb zcln_pvh{?dm(&?rx&qW?r^OY>_OxJKC3b$mKCT(BAS%F0b4Z z?Mamd`C+n4ZmwnFp`zXvX}{(hWY&GqeiU>|R#}C1Sh-@^RLDxf@yg3{h_fFZ(6;xy*O~+I4o;lD{GQ$=6!)xdUNiv^0J$270GrEL%UT4Bl+CnXkTb$&Nd>j&)xl>h0%cYo9zUGhdB% zqtPWY#TvAI{GZBl$+q?|mWQv!<>yO`<&D;%-P~#~`z=@0eRBNGQe(NvMzmMH$&h~} zdv)_n`NBM0KF%vs-enWo(s$lp=8L)y!Af7r-nSKP>5I|}x1lY4x7lku+S2EcKankc zeRtMQQTO>l>D#56J!nf`2ra)CZRx|C`^lER^BB5c)P34Y`bu82gJ?_NqB~2r^ij9z zhj6*{4KbS|qVB6D(ifP%^J%pX^K&JAt*GcY+R`_HMx8)g`nXPqlW0p{sVRR7ZRuMq zHh+k^+4`j~S$yYH10CV@NMC&@I)k?KZG};1(Uv|^(BT}~(wzAB&WpNPlcgEO^Dm+; z&AvVD657%n*Up#GmL`k-LbfzF@tmuoZuUxPX4j}2XiKx8df!A_nrrka+0vw)8;fwc zG)H8oVo~onZ>KcFUhTVROS92^CR>{GZSg%^E=``MzK^yv3z_)?QI}mUO&d0xY-w^X zi-)*en&qm5Y-yINmXAcelYC#&>`7}MqbcE?+;#9n#p1o+0v{J{a)d6X)*=N z*Jw+#59TW{I93DJR^W(*sjAlpgfcfuPd(JI9TOjy$Ir5et-ep$$pVOn6%dY&v!foXoe zO;g#`N|?6y@op{lny2vT()Qj}WI`QGrR&>LpB-(0X>tda7Ym-08YipuOo+9n^fnJy z=F$?=&-~9CF|jSClT{5cV$HoUJ#1i1iBQ-9)4iIc-4)ITVk%b}o5+@S#}uppVLUs~ z2UDshclaU*k(eU&$Hj6?lYr?@=jMMBOw%!)DXVuRYd-+flcG%$S^6+cJ!57+zvzD>+ZhCns&qx_W0mrRuq6C=`+iH z5gv8JaPvwtOqfq^3>i0g632qn81}t0bpZPwiy_@5Ra&zP$rz$tWAl6ADG%4K$B+Ha zf*veyFosYszMy64qc9A5hGQ5T#Y3X&^KJY#w`4#e&eb|LVaF+Cx!6izw|O{nL%vPB zoX^6xRCSxWB7+&c)>ZV{(1w$miJ#c3wH8AAy!>D>pQwKvB zJwLm!u8lAx@r=(SVT&b(7;e;S&$ilOnBeEDJOZs1hW)inY{JsrFpRHe>k^@GLyLv}pZoM!(?phqMm3=V8t>~0H>lK9|Yp(b7S(O9~LF;G{ z%Zw=O?8c|HViYGy4{ma*^8k4NrXz*s=vA%Hx))*S%&a_LwuVAy5}oR> z%}+42<=~^n%$P!3GIE~^)hIk=aL9YXREFUr%a6ShF7QwiDSRY0xihmj#PAMdSylF^ z8is59-YJniu7e>Li;A4tn+6z8;n$)n>(UfMBFdZSvEJ4g;;=a@g6(aIAq(m;-ptq) z!wx1sPGV_Z7*b&R!kG>J4Z{Wc_Q+yaI-~r5>A^c8C>SOD7w4lG|G?FykC-zrjbw~;wK6498C24bRunF78WlKqVya4(v@;pk3Yj_`0k)*^qy93x-k^@h+ zjbwo&?=62Ju#eABe!J-&!xoYJHln>3oBtUlw9P|%GY2_JU$ZNRv0?fsU!5~HWfq1g zM}2nXeQ!0CnryC(W_|0RM3mp67So!cylxJpzMYFArD8sx- zsm&7FpnMW@z?FUVKsjVc>nQfv2c?c??OzE~0#LfRW^cqox}g+NW0yZ$6N+*|nrngZ zDgvbfPlL|PljH&IKU+y2&BzzzB4m%dV%es^mJ) zs?(1xn1XKZBPVLIfwRz6-R$u-!I)gt%IUS((&gw%7W%}p*5p1ub7?lS%12jk^X4Cf zXWP(?JKZgmwI$bWm`yqxeHh)T-Cf(UOQ+CPs=s(7laZ^m>NHoj^)k9Lm)y{@>g39- zo6o02A$O!h-T*eAT#Y9lg|TVZ8{S_vM>o@#me++k z>YuxAIN=4zvBf)(MV^kMDb!-#*{ zSVuG5w02`5`8WfOxBp&YCgFsg>Wa{maN<+j<3b&pA!a$pi8?g@1(DMVg&O5n#wx5Kdg#C1aHcC#t_K5#A6^JY9Q6SWP(L*6W^dgwJ9k%}VmY_NK6% zaN=S{2R4szV*Su))`D#Lz(-m<-FT#lRBAobm_?f_b5GQ(v@^IH6#EDN6CbM&d6VF~uW-kaQ zrtkP9glG^aS{WFy>4X!DLO%&n8uaAM60Cl*0C!EOUCJKoCys`iG8N&(Ne@0Ln20#he|j(0kZ_{wgof-vRm6!jTNPVV z3vr_O8-Gm_P7F@-U;`}>CpP7qu>``2v;~pO%n@6*}@ywoZqR8WmP%{{DV!^zcEGHCk;?VL`Hk@$6&^3c~A)NUBuoY`UI8kTz2cZsS ze4bep#%>W#93K|LmJv>js?djRCY)%p&5`w(iW!her%YKr!ikVi0;{qJabm@l&%%Ae ziCgnsn38a!5yy%8`G^xWGODpco;7H}ySbBdGucJLi80w3Eb|Cv<5gWbjLqVidCmAX z-Ku*t?FG!1TYXQ>VhAU^W+$?5JQL1}Z&PTtfc-`|F{NS{`%XB~CrZwyy~B*Lrwa$L zQG^pkO^sLw!in!E^jQbOiTh8AgkZvnYPH+3S~U?T3W7Y?>3WD0H=66Y8+0Y#HIi)xJ-J?t~L{znZb4PKXo!d5xH1SHy|S4?YPq zdLm9(Csk$_2`3ya+p%$k6Q8GAGdIGCq=B)_op55w*c$8|;Y3K1B^x*#abnk`J3?o| ziG5pIGY7&6qigQWk#M4YZ*R7HKH@~VVP}LzgcDU3e-IiIPW-kdm>J|DPS|A*W97Fa zPApsK#r6|UghYK2+7nJpd)JILJ%uCur*Oq~%L7ebDDrXZ2C)VES&8iYk+*=mNxCciU z^}JUW&yIT{PGr|<#c~NJir-~3{XoPCpO2q~;e-?Z7o*t-!U^w+Jy{bC;zVF!EPGBk zaoyOOZ6Tbv*wvd^5>Bii@5Fi$PV`x4$xI0+sz0@07qSs2RvQ?ztSK0I`g+V~A)as| zWp6c>or5?rb8=0VKsd4HP!(pj4sjw|aYy(-I5DxseD;ZOV$`dOEO{SBO76OSfGr`M zSUT5)JtLg3eNlnUAe=bq7RK5TP6QVFGd;qIc}|gRqI!wcczlbZ4f6GHPNzrgcE~?nz03h6Hk4eSRmm4-StHSU$LpKu~zkP#b0 zIH8&0&%XCUoM<(wK&Y=qobd7pU~^(Hdgz<5Pe>%3c=y?nDF`Rp8^*D(gcHw3bZ0LJ zCn^+bS#&l=);#v=%U(@JoG3n7i>)P`*cDVHco0tP$~R^ocqEGyy^=H_hD~0J(Isp8 z&SE1rAx_kME@O2ma-_(y2b;bZqd6*1PG?&PC$@EUXO@H$FSm|j)`SyEeRp<`aH2tR zFBV&b(G3earm{F5=^#ZntT7+UP7+SMup zj~zSP3UQ)mUSBra9hKp3^?I_cgcAX8PY7)YCuU5!A>8hYI5DD1G8@|qaiUp>nuQWh zEDEU0{0S%e#09apgcF|kr?Yv46GekUm=WPbq|%pdABk#V==3J+K{hIZdy`wR2~$wb zoA4--g%eI3jIF@*NuAqRp#xh_IPv?{DMIo(#EDI7?g}*sCp2N(geQa(ea>GMM(jf+ z>g*HxHmyW;!g2l_IFXAuvD^9yIBh|ksJDLt+}eZc#M8C&!QddO z6Y<)S(BL?#6Sd9LPPnE`fX}2(?34B3sYs|!6RqH8GO81u zH|0XReuxvlFRKRw2BJFQ9diu2jzD!{iu*)x%0hLb@uI!(Xfmo3!p#Y=cP8S5U$R`h zI3Lvs=a+@xvm9|Ea`9|9wig0}qJ10?{FyF8o+Fn3)V(yNcpuK|XM9OasU~(}|JT>do5D*`rI&r>cG#r15>cr5A zN1>P|gPOK4SzJbwI}Mw80gU9RPCWR$i@EC~PORI$6uMVJbwco}33aL=PS|#z4Mn6* zyc-b!oy<_3NYbjrJW?mRv_1=_HmFWyRq6~)S|U!|K2TN6az%Aw=Z@--?}6&Xl9nf+ zlP}_gWnvY`4MKIIT*gH>NH}4#zJz6vIx%~E9@s^qI$_~D4YJ};o!GbK9`sH@bz-b& z3y}j*op@7t5bh5_b>iEMcr!cCJ;0iCrC*ioDDq2 zOs7uFn|BIktweR=RSO0pi4{ z+kRqpV^k;d&I{s*7N|~)x$y)_>`|SFtujS?OX|eYCbQs~2dWcuUbsM52UI6=`DY6E z`6Eu`)Xf&FbwivedGQv~LlGywUbq2c!cm0 zw?8oyzYIimVyrq6-i|_?Xcp!UC&r;VQN6}1XgwJd#v2u%h6l4yotS)XHM}HsV)F0B zuwogi6Ekle192Uy6W4^c;;v1I6JKT?fhs!?ClUi!!I6EaPNeuwg2qQtorszoDqi~o zaiV@-WAP%X6H&KB@!=IzC-Mx>gL5&e6Xi=ZVg@DiCU;#9k0?>sdsh!Jht!Ef(bu5k z7sLtc9=oB79My?lig>YHImC%ZN&cd?GU7zqkT|GU1J#L2(`t*`O;MeAGsI2g0+B8u z)W*VfLSGK)fJph9(`@%!JQMI_?H%9#G*+gMa5?%s%pUCF3U%-`7oR2isF z6prZ(6^Eia@y;p)x{N|~Vnd?=uz5Vdi%UqQlG+AW$;eyGn;) z`zll?_FuaIO>z+@tSg4W>U>lu7J7FRm+!ztt@2HSVFjrZ^R7IAPDe43s%y7scteR) z_q_OhScvKbTblyyu3@s$`&(at&pWD1R=N@R9&yomf)0otV=Q z)rr~Lhr$y}R40-KE)er=5hu2vk;64VE7&O zc8bd@5GO7NU4g^v5GO3QEl`A#I-%b*3(R()Ix(;QJVn}m#EAxaIS_di)rkWY!$lPZ zdtaWXgyQp{3*^4GV~?Wb3aS&@*GJ&DV#J9>Bd#kJlRB|!z-ib?!Ot!HyXrT4gX%>0 zUNUjqXH+L_c_fva9C2d6#yK!ZA93Qgw}--^5~>ruySc$*V^k-6Mw}GikUHVuF%Y^m zK%DTjI4MrGKy^aMaRmo!R40xLqN1|{2DRD-9)ymrh!Y#UM=ExaIx#A`lDOLk)rk`m zPKtFpqdGC^)+P8o7;z%Cx}#zbsS{zYu~12a>csFxpA=nVQJwgd+YUY_qdF0M|B%8r z6V-`Zst6c24AqHR#qShPMNiXAJE;>FE??sc-@{bp1rJZ-m#uXdt3ael!9wtYY1M6aYQvE@-zCyXqT zVEiAbP7G8W5lb$hIw5^I+=&9lq)+7r-Tn(si2o5z+&sS(CORNan07k^DTEU-+eX5N zj))V39ZVs&GvdV9_jAC6aKioALfG0H)ro*JMR1F7V&2_)Vp!3Z^{- z1>rj!p#&ySu#<8344A(QapGL(EC{3d?yqhd37rWi9L+3x@=As4Rgoz$kS4|No>K_h*FJ_1n_Kn9u0R=mq zHnV|f!ijTwd!QBJMEA*tu#a$J@|gt?-X3uR9Ad$qaAL{GLgqj?p>J6Li+UkWoVvdp z_C_F1gkJRmRSe?9rQ6NHj&P!mT^=M8PPq1T1dqXp6a8);g*C$wCyMWnhY4d5C)W4d z4>hMCPMF>q4^;^#Vvfp1Sb#V&b@F7$S%x@q#%?K0UW+)v&Tocs6znw6>^@ZAfjALR zDGILaL!1ao*a8m;C-R?;0Hafg6CTS}Ktd@_7#6{7!ihu4rf}~T;)LwdKNENOr^@wQT;nAr@~3A2C#NV7(qxVc{iFP#u4c>aZW(*<#&J9C5qUWgNI zE?aviuoaNjMR_fwxVAIMJ>7bl6Tfu~&8{NHyOE?i+*$RdePOL2$4K)ZSUXR!V zPgWpKEK8UMU2_p9sx7k%g*ZH_o` zX6X}fBb>MzFhy+G8r6yKduDQo2u7S}cJdXRpkSxx z({8|Z72-tP*gs%D;Y5+k5vY`mII--&7%gI5Bd+8<-MK z-0%MpVhAVt@7)J&2q$jeSp&)(R40};ehCRH5GM@7w?oghh!bxVu3}}vi5{Bc;6^wx zbm}U2OE{sPI1vUML7dp#AXKy_oKWAg5OWGqolss=h&8SvPRzC14HIu6PJBNaEZRRn zbz*PNEs*ydgPk_s=pp_=IMHX(Rai*DPFLpdh7p7lw>$@mXAKZ1^hP{^y%g+}yDSz` zt0PW)+bk2)>LN}&4RaSm2q$K~Z6Q`{hU&zfwe7{8c8C+7ay~!@!ig7$k|2X{B5>6n z;KHMBj+N6nnczz}QCzv6c$;uy@ymx$pKxNusVmT#aAHpS6?h+oIN>yOoY*i9abnV% zkuV|^aiYRJADB!yu{S3G!iOMEO#BuCBM2vkoaqPU2qzYAybr0*heZ?}uiLeHrp)eDJow{{3RtSU> zCFRP)Uc!mzX;(xI;e_+xnecoX;>1b^b4A{4#EDZebs>Xr;&ClIMRURl%_%&FiY)gK)ybMkd~(yvN-k$HW;j#EEwab739f zglbz0MRrBRiM$SO(3WuGOW-NdtTw6>^BWI<)%6i4rgu9bJ}^g|h+N+m)PxhaEJejo z!igoH*Mrywabn)ck&4HJ6XhBhiY0^-deJAvh;#ECK95pZiL;)MCj_li2B5hrHz2@CE{K%BT( zJWKJCaAMhlBM?0YaiVknW_=gJiP3qPuz3aI#H!O<^$T-Ro$#G98hi;SY@Tir>+eFG zcu};FJs_OOD4!{gKY}sII%1IFx2%zb)tCJG+6o@;)I7;Ll{HqL~g%z@S1R9T;wRI8;UrQ8uk>%sWI4T zP^z`)N9u%giJ|C2>O{>N5#W}A>O{im4j>Fcbs}rhLpVY>QH!e+H%Xn?;e8N%r=dEb zemnzuk~-n~M+LFoVpJ#g)_x4dgcFa(tc8!HPWZ{5Lev&iCwfJU1Ml646OFbn0B2Gs zKJS?WUkE3D->-!)gcIt~1+b5Bq86X?z=70>n>&7o;5(>J*yJyS6jCRi_V9q^&rzM2 z`sOandWY&n)b!cV^b6v|s*+@QO6tVj#=BtvsS^z*c!AmwaUx`vqbRR|>O{vW9UzL- zi327vaFNuBvuTTA6{!=`YFNP{8&oH*M(=_YCsZe#CYnNw8wNXV>y-m-NS%nia1bi_ zp*oS6b%Wg^oLE%;1SonUPHdUE02+lOPRMS0!m((?i3P#OWw z4XG0aKF47ssT0}XmO&J$6IIjKLBJ%$iMsVx!BfHs)rsojM#72Ok=X!C5hr}|7QuT` zCydS<1Wg{Q6CHQmf>O|y%(=3V9 z3Fk#yz|IiWi9W+j;2q&a=ff*t2jPU?x&RnR>clB^f_RJ6iLf!n;7962$yybJv_zc9 z>049WPU?h7kvZHZb)rYxqp+EBC+?3f58BSCPSkr)0t-Sg*y*w6Im->hU?=&}?GR4t z#ML<~AtD~tiMsXPf>SE06VqqgK!Z%giBH9cpxRKxiO_FHq3amLiO=(Th>u8}(BB*l z+UXeV)F{jan$JUZ!eGc&cu6>6zv~Y;vJ!D3+iNMbAa$a(vz=(Z8P$orw##4(;Y9V$ z1H`Qd5GUr(Jt&SPb>f1-NpZz#40e)T?+88@5hv<6c|gY-s7`!790_)JP@PD>WC3Rg zC(cfP3Ppqy+hW|rC8SR5S@!@mq)zDXxCIs>suS~OzXOX3s7|aoG7&Cu zzK9b;S63FB4MLo-AEts{BM~Q>c(jJmS*T8Qy2!+8Q&63lY;gvv&qj4ZQ?w6G5>A+W zdIp80PBc4m92`iU$S&{`9}`aOu6hMZ2q(-t?14r5QJrwVI|;gyIx)RVoM=#h>O_HA zV-YSOP6Vu|F8)q9v5~72(WFiUcxc2T!ilKRO;Gb0;>639v0}zs40gKZ^%ycqotW_G z58$agx~V#cg$xj#NS(Oo79h?hb>dN2GISz!B6YoyXj%_(A~VZf6dIvAp;WdJw-Zi8 zz4j1)Cv{?$Njb5BGpZBkdJh3JH&iF;7N3L5q)t?`&4hekR3~=KZ6;0#!eFOIpB}*2 z5L727cDw*tVW>_Nce?;Q{Y*EVVMxw+@igJYvdshGF{u;77j=WynW#>j-QF46kU9}L zuRB|2O9;o15Cn65;exaNKvg!QOS zxbF^w%lU{Cbsnh2Av;l>*w8Wxh91CRr%IJSfcY_0Cp2yYp@49rM))a6Cv~D?-YBSZ z9o320%U{9p4yqGl`J5J82`7}s+aQ$Gi34ZGLw8aqYSed!wS*Je@e|+?;l$~d*|4=7 zsuMOB!XdOW;Y8jX1*?JT#FBNzFsdHnM2G5~#f(O%PE0BCg`j4L6V3+~inj?Tj+q<4 zVNxf?j9nlOaz&iz+NlqyyilFEwSKAik#OSd#kx>E5OJc9Ac#>Rs7^d^EQF#`oCqio zWs#^(tm!!%zQ!U>{86N@@J~f`B4Jqxw8=o6uuC#maBqliiX&xtfOjKNo$#M`Ok6My z)d@CmB6ORII8p1Vqhj0~#EAumnu6IPR42|8zjQ;kDV{~6*$-ECd<;Vr5Y?w4iaPEsdcCR`B{<%km_2F-xGBB~SFL7oaV zsT0%f+`*(8suMT1TonrlCziFD2_qUHPS^()iU&xYm^a!L=3AjUk=@)x(aaImi5ZrA zA&+ok|L&QJ86Fty)YHgRoY@i83D=|>;%}r*Tv~q_UJ_1xNpw`q>W#rpCkMuY3#k)B zR)PLVQYSjh@qj|Yi4K(u6az?|0K*9I9)dWrWR#x%(b0$#`)9U+I^z*1X11QKm_H4J zow6gZLOW6?-aOc&|B!Iv)AIfhwi0n-TeofcL)RltEcKlQjklmWk?MO;JViKh{LMNx z^#H09#Xi|$B&ieQ^pauMABYpDGcJg=3Q?VydL|ndUPGK1G_w$fb9JIICx~+Vzb+2nT1o$nY4xlXy*Pswzqorq)Di6SQgC!~_>JGXpa-Wsrt{&Rm<8sf^=B zoRT*=HBFU}mde)|glDE{l%8Jgx_Z0VD9zNG@boC$4A=PhwfA?iRVvM*Gzpp%Rb1J1 z!R~>6F7|v~KUGQsZ*bXlUV(xBfi6x;GfhfLVoKRH?cKY$`?}aTD$OEQX{xvu5xiwu zer{+RY;k+7CQhwuUk8m!tw~Wv>waOIbVp&3nwBE%GeH%vQJUee#7JdYv__ebsMeIO zj!%u^Yw64=OP5A!HHh>%XCv~j8V_XX0m32An=$~bL; zRI~2a-*1TR?is4o-6W}eW1XQpf>hpqRoah(P$s2^$7xfeHK_&$F8`Z<3_1mQw^F92 zsl)gwN^OCs*?`|G{LWCTQq=sMq^Z>W3MnJ`^^4XLp7u8eqV;Cs8TWwq*8dO24?N0bIdp5rF2)6|5l}@CPrxa1yC#dY15*AJPp#8 zVOq7pKlE|crB09E2`;?Li%L-?MdMxNn;7+{ev%{VKkp~`z(!BWH~864hO1Jgn?-s6 z!=jQ>!_+DKa;N;MkF@#KN17S@>LY*kk-z%LzjnR<@AQ$<=A?8HYtw2NPZnqPyOvSX$H z%Ma{d9=4xD8vf+raHbOi7FU9PKCb+c{4u0y^z(6z(?q6~8MCxVNsA4O)MkW9!BYB2^k&Cr1az2y0ullSUKi=;-Ve?r7bIx8`pi zW@&Kjr1bL-3+(cn(nV>FPo=WGufKa2A3tweY(tCrMq%xJLimQZ27g`@)}?!Z*S~$l z)udA+4G`n`z*LhWosS^hsj%ZKe)N5O1o-1*){;sfNB{NY@Do|Yr#1U;pFvx-oysXf zZSU+H?i_BfvbME%jtqBpid0)W@RMrq8I+ejna)vl|g)z92cjRSVfzm zVM)LjgY>jZP1XIa>2~(wp_2r*hj)>D(avwq(f4lu?-p?9- zTn($g47>RzKiaIj7w}Q%Y{~&TUe`neen0U01#f%_zj#+5T~uGbHZ5JPQT9}8qEa-P z)KDvBKw>KQi}055Q2^#74!?lu+DcZ^)GA>b-=4h{!4oIp@ zS8-xO)W58m-ymsH#Yrg=5XMXHDXl8u&-M4?3|6N4(ki9;-Q!9d9{-mntHSwXt~;f4 zKmRyyyqHCDx>l3MpAJ8zHj*EmQdL@y#GZXs+Bj(}$8Vw1V=7%Q?GsN7zupmvI^dRY zyk~rG+fb#>@5S%evKLjQC9z-gcvTGdlyKLD zCIxT)b{g&&)3oBd;fGbmsgbJd0J!fVS{ttAJCGiI$??Ijw}$Ty{Xa>Gagtx9^c3`T z4po-6Fzn}%ODDt!yNq!BTQ|#VNIEQfsAyi_`FaE7OWf^J=^U zg7}%kC#=+cfKTg>(=6SAD(?3v)2~=^AC<1byhG!ck>4D;v&!$HsKf+SoUZ=F1pcUc zYU8-K1|QP^z8`I>^heq2ZOLokx0;qeD;j=|m8vA}#K^!u%N(5Kp5yya@q3b9NFM&( z+yi~w{XD#M@1(GErJ0@8|9t zR$8C!KiOmHdb`pl{mrEclU{$nhP=OqywXiy>K^>RJmmeK@+y}Z9h80L@dx~GM?_Lb zMQ=;V+5B$~pm=S;(Hj2N=8oM&C4UJ@J(1LJq(@039{wVr4p7azx|DeCnAdqj!xQa%Bvo44 zjr^$~m6FO>|EXZv;1D1E(hU{ zna(?`jN)(81ohulfUWpB&}k>V?ogiMFN%JAtQGxl43+r{GK#-T@Z9s$t+dk2&$;_dG1sN`KUh4iDbr>JOXg=`Gq<#K` zONk|=$VX`-^dRune9-%k;jGRMcHu3Zog%Cw9c=6)Bdr~+RStG4J01iZp>edecMRlp z|E=<-i!v;Y+T+ghgdZ+rwd5_)phu#Na#PmvjxN^*JPY_a((xm5|I1m> z(YJIV{&cx4b$TFQtfuw4)B&AOyv)hAQu1raFXm65bZ4$bYk1(lr0M;)Ri8?kK0xEPAhXh z|J-=#FDV9q&bt&@z#pzsnf&jMsB~>VgMa=mx8yHsU3ktv-ba6a8wO$QzM5}uB)$8i z(-UH)(SS>}%KT6MBSmLNa6KT+MW0($_G%Hv3UN*M literal 0 HcmV?d00001 diff --git a/src/localization/bag.py b/src/localization/bag.py new file mode 100644 index 000000000..3cce392ff --- /dev/null +++ b/src/localization/bag.py @@ -0,0 +1,95 @@ +import rosbag +import csv + +# bag_file_path = "../../short_range_rtk_2024-01-28-14-32-22.bag" + +# bag = rosbag.Bag(bag_file_path) + +# topics = ["/left_gps_driver/fix", "/right_gps_driver/fix"] + +# csv_file_path = "stationary.csv" + +# with open(csv_file_path, "w", newline="") as csv_file: +# # Create a CSV writer +# csv_writer = csv.writer(csv_file) + +# csv_writer.writerow(["GPS Type", "Timestamp", "Latitude", "Longitude", "Altitude"]) + +# for i in topics: +# for _, msg, timestamp in bag.read_messages(topics=i): +# # Extract relevant data from the message +# timestamp_secs = msg.header.stamp.secs + msg.header.stamp.nsecs * 1e-9 +# latitude = msg.latitude +# longitude = msg.longitude +# altitude = msg.altitude +# csv_writer.writerow([i, timestamp_secs, latitude, longitude, altitude]) + +# bag.close() + + +# bag_file_path = "../../short_range_rtk_2024-01-28-14-32-22.bag" + +# bag = rosbag.Bag(bag_file_path) + +# topic = "/linearized_pose" + +# csv_file_path = "pose.csv" + +# with open(csv_file_path, "w", newline="") as csv_file: +# csv_writer = csv.writer(csv_file) + +# csv_writer.writerow( +# [ +# "Timestamp", +# "Position_X", +# "Position_Y", +# "Position_Z", +# "Orientation_X", +# "Orientation_Y", +# "Orientation_Z", +# "Orientation_W", +# ] +# ) + +# for _, msg, timestamp in bag.read_messages(topics=topic): +# timestamp_secs = msg.header.stamp.secs + msg.header.stamp.nsecs * 1e-9 +# pos_x = msg.pose.pose.position.x +# pos_y = msg.pose.pose.position.y +# pos_z = msg.pose.pose.position.z +# ori_x = msg.pose.pose.orientation.x +# ori_y = msg.pose.pose.orientation.y +# ori_z = msg.pose.pose.orientation.z +# ori_w = msg.pose.pose.orientation.w + +# csv_writer.writerow([timestamp_secs, pos_x, pos_y, pos_z, ori_x, ori_y, ori_z, ori_w]) + +# bag.close() + +bag_file_path = "../../short_range_rtk_2024-01-28-14-32-22.bag" + +bag = rosbag.Bag(bag_file_path) + +topics = ["/left_gps_driver/rtk_fix_status", "/right_gps_driver/rtk_fix_status"] + +csv_file_left = "left_fix_status.csv" +csv_file_right = "right_fix_status.csv" + +with open(csv_file_left, "w", newline="") as csv_file: + csv_writer = csv.writer(csv_file) + + csv_writer.writerow(["Topic Name", "RTK_fix"]) + + for _, msg, _ in bag.read_messages(topics="/left_gps_driver/rtk_fix_status"): + rtk_fix_type = int(msg.RTK_FIX_TYPE) + csv_writer.writerow(["/left_gps_driver/rtk_fix_status", rtk_fix_type]) + +with open(csv_file_right, "w", newline="") as csv_file: + csv_writer = csv.writer(csv_file) + + csv_writer.writerow(["Topic Name", "RTK_fix"]) + + for _, msg, _ in bag.read_messages(topics="/right_gps_driver/rtk_fix_status"): + rtk_fix_type = int(msg.RTK_FIX_TYPE) + csv_writer.writerow(["/right_gps_driver/rtk_fix_status", rtk_fix_type]) + +bag.close() From bf8ad8e2ac6d670e2a5db3e5b5eb09b556a05902 Mon Sep 17 00:00:00 2001 From: dllliu Date: Thu, 1 Feb 2024 18:54:04 -0500 Subject: [PATCH 066/145] add plotting script --- src/localization/gps_plotting.py | 119 ++++++++++++++++++++++++------- 1 file changed, 94 insertions(+), 25 deletions(-) diff --git a/src/localization/gps_plotting.py b/src/localization/gps_plotting.py index 81f291e84..f1b241290 100644 --- a/src/localization/gps_plotting.py +++ b/src/localization/gps_plotting.py @@ -1,3 +1,48 @@ +# import matplotlib.pyplot as plt +# from pymap3d.enu import geodetic2enu +# import pandas as pd +# import numpy as np + +# ref_lat = 42.293195 +# ref_lon = -83.7096706 +# ref_alt = 234.1 + +# linearized_pose = pd.read_csv("/home/daniel/catkin_ws/src/mrover/src/localization/linearized_pose.csv") + + +# lin_pose_x = linearized_pose["Orientation_X"].to_numpy() +# lin_pose_y = linearized_pose["Orientation_Y"].to_numpy() +# lin_pose_z = linearized_pose["Orientation_Z"].to_numpy() +# lin_pose_w = linearized_pose["Orientation_W"].to_numpy() + +# time = linearized_pose["Timestamp"].to_numpy() + + +# rec_yaw = [] +# rec_time = [] + +# for x, y, z, w, time in zip(lin_pose_x, lin_pose_y, lin_pose_z, lin_pose_w, time): +# siny_cosp = 2 * (w * z + x * y) +# cosy_cosp = 1 - 2 * (y * y + z * z) +# yaw = np.arctan2(siny_cosp, cosy_cosp) + +# rec_yaw.append(yaw) +# rec_time.append(time) + +# for i in range(1, len(rec_time)): +# rec_time[i] = rec_time[i] - rec_time[0] + +# rec_time[0] = 0 + +# plt.figure(figsize=(10, 10)) +# plt.plot(rec_time, rec_yaw, color="red", label="right") +# plt.xlabel("time (s)") +# plt.ylabel("yaw (rad)") +# plt.title("yaw vs time stationary") +# plt.legend() +# plt.savefig("rtk_plot.png") +# plt.show() + import matplotlib.pyplot as plt from pymap3d.enu import geodetic2enu import pandas as pd @@ -6,38 +51,62 @@ ref_lat = 42.293195 ref_lon = -83.7096706 ref_alt = 234.1 -linearized_pose = pd.read_csv("/home/rahul/catkin_ws/src/mrover/rtk_linearized_pose.csv") -#right_gps = pd.read_csv("/Users/nehakankanala/PycharmProjects/plot_rtk_data/rtk_right_gps.csv") - -lin_pose_x = linearized_pose["field.pose.pose.orientation.x"].to_numpy() -lin_pose_y = linearized_pose["field.pose.pose.orientation.y"].to_numpy() -lin_pose_z = linearized_pose["field.pose.pose.orientation.z"].to_numpy() -lin_pose_w = linearized_pose["field.pose.pose.orientation.w"].to_numpy() - -time = linearized_pose["%time"].to_numpy() +gps_readings = pd.read_csv("/home/daniel/catkin_ws/src/mrover/src/localization/stationary.csv") +left_fix = pd.read_csv("/home/daniel/catkin_ws/src/mrover/src/localization/left_fix_status.csv") +right_fix = pd.read_csv("/home/daniel/catkin_ws/src/mrover/src/localization/right_fix_status.csv") +left = left_fix["RTK_fix"].to_numpy() +right = right_fix["RTK_fix"].to_numpy() -rec_yaw = [] -rec_time = [] +left_arr = [] +right_arr = [] +for i in left_fix: + left_arr.append(i) +for j in right_fix: + right_arr.append(i) -for x, y, z, w, time in zip(lin_pose_x,lin_pose_y,lin_pose_z, lin_pose_w, time): - siny_cosp = 2 * (w * z + x * y) - cosy_cosp = 1 - 2 * (y * y + z * z) - yaw = np.arctan2(siny_cosp, cosy_cosp) +gps_time = gps_readings["Timestamp"].to_numpy() +gps_type = gps_readings["GPS Type"].to_numpy() +gps_lat = gps_readings["Latitude"].to_numpy() +gps_long = gps_readings["Longitude"].to_numpy() +gps_alt = gps_readings["Altitude"].to_numpy() +print(len(left), len(right), len(gps_time)) - rec_yaw.append(yaw) - rec_time.append(time) +left_time = [] +right_time = [] +left_x = [] +left_y = [] +left_z = [] +right_x = [] +right_y = [] +right_z = [] -for i in range(1, len(rec_time)): - rec_time[i] = rec_time[i] - rec_time[0] +for gps_type, lat, lon, alt, time in zip(gps_type, gps_lat, gps_long, gps_alt, gps_time): + if gps_type == "/left_gps_driver/fix": + pose = geodetic2enu(lat, lon, alt, ref_lat, ref_lon, ref_alt, deg=True) + left_x.append(pose[0]) + left_y.append(pose[1]) + left_z.append(pose[2]) + left_time.append(time) + else: + pose = geodetic2enu(lat, lon, alt, ref_lat, ref_lon, ref_alt, deg=True) + right_x.append(pose[0]) + right_y.append(pose[1]) + right_z.append(pose[2]) + right_time.append(time) -rec_time[0] = 0 plt.figure(figsize=(10, 10)) -plt.plot(rec_time, rec_yaw, color='red', label='right') -plt.xlabel('time (s)') -plt.ylabel('yaw (rad)') -plt.title('yaw vs time rtk') +plt.scatter(left_time, left_x, left_y, color="black", label="left x and left y") +# plt.scatter(left_time, left_y, color="orange", label="left y") +plt.scatter(right_time, right_x, right_y, color="green", label="right x and right y") +# plt.scatter(right_time, right_y, color="blue", label="right x") +plt.scatter(left_time, left, color="red", label="left fix") +plt.scatter(right_time, right, color="violet", label="right fix") +# plt.scatter(left_x, left_y, color='blue', label='left', s=1) +plt.xlabel("time") +plt.ylabel("pos") +plt.title("RTK Stationary test") plt.legend() -plt.savefig("rtk_plot.png") plt.show() +# plt.savefig("rtk_plot.png") \ No newline at end of file From 313355882024cac040af1649e6309560791038dc Mon Sep 17 00:00:00 2001 From: Rahul Date: Thu, 1 Feb 2024 19:18:44 -0500 Subject: [PATCH 067/145] added time offset --- src/localization/gps_driver.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index 98a88240e..eea581e9e 100755 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -31,6 +31,8 @@ def __init__(self): # self.rtk_fix_pub = rospy.Publisher("rtk_fix_status", rtkStatus, queue_size=1) self.lock = threading.Lock() + self.valid_offset = False + self.time_offset = -1 def connect(self): # open connection to rover GPS @@ -73,10 +75,13 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: parsed_latitude = msg.lat parsed_longitude = msg.lon parsed_altitude = msg.hMSL - time = datetime.datetime(year=msg.year, month=msg.month, day=msg.day, hour= msg.hour, second=msg.second , milliseconds= int((msg.nano/1e6)),microsecond = (int((msg.nano/1e6) % 1e6)) / 1e3) - time= rospy.Time(secs=time.timestamp()) - print(time, rospy.Time.now(), msg.nano) + time = datetime.datetime(year=msg.year, month=msg.month, day=msg.day, hour= msg.hour, second=msg.second) + time = time.timestamp() + (msg.nano / 1e6) + if not self.valid_offset: + self.time_offset = rospy.Time.now() - time + self.valid_offset = True + time = time + self.time_offset message_header = Header(stamp=time, frame_id="base_link") From cec93d69f4059c039e00d83bfc163d7f91bf9733 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Sat, 3 Feb 2024 17:28:25 -0500 Subject: [PATCH 068/145] remove prints and add min_hits and max_hits to navigation.yaml --- config/navigation.yaml | 2 ++ src/navigation/context.py | 25 ++++++++++--------------- 2 files changed, 12 insertions(+), 15 deletions(-) diff --git a/config/navigation.yaml b/config/navigation.yaml index cc54f3043..c7a9904e0 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -54,3 +54,5 @@ long_range: time_threshold: 5 increment_weight: 5 decrement_weight: 1 + min_hits: 3 + max_hits: 10 diff --git a/src/navigation/context.py b/src/navigation/context.py index 7b6e90611..69cdf42ee 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -30,6 +30,8 @@ TIME_THRESHOLD = get_rosparam("long_range/time_threshold", 5) INCREMENT_WEIGHT = get_rosparam("long_range/increment_weight", 5) DECREMENT_WEIGHT = get_rosparam("long_range/decrement_weight", 1) +MIN_HITS = get_rosparam("long_range/min_hits", 3) +MAX_HITS = get_rosparam("long_range/max_hits", 10) REF_LAT = rospy.get_param("gps_linearization/reference_point_latitude") REF_LON = rospy.get_param("gps_linearization/reference_point_longitude") @@ -79,16 +81,17 @@ class Environment: 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 - if it exists and is more recent than TAG_EXPIRATION_TIME_SECONDS, otherwise returns None + Retrieves the pose of the given target from the TF tree in the odom frame (if in_odom_frame is True otherwise in + the world frame) if it exists and is more recent than TAG_EXPIRATION_TIME_SECONDS, otherwise returns None + :param id: id of what we want from the TF tree. Could be for a fiducial, the hammer, or the water bottle + :param in_odom_frame: bool for if we are in the odom fram or world frame + :return: pose of the target or None """ 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}") + target_pose, time = SE3.from_tf_time(self.ctx.tf_buffer, parent_frame=parent_frame, child_frame=id) now = rospy.Time.now() if now.to_sec() - time.to_sec() >= TAG_EXPIRATION_TIME_SECONDS: - print(f"EXPIRED {id}!") return None except ( tf2_ros.LookupException, @@ -117,7 +120,6 @@ def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray] elif current_waypoint.type == WaypointType.WATER_BOTTLE: return self.get_target_pos("bottle", in_odom) else: - # print("CURRENT WAYPOINT IS NOT A POST OR OBJECT") return None @@ -133,7 +135,7 @@ class TagData: min_hits: int max_hits: int - def __init__(self, ctx: Context, min_hits: int, max_hits: int = 10) -> None: + def __init__(self, ctx: Context, min_hits: int = MIN_HITS, max_hits: int = MAX_HITS) -> None: self.ctx = ctx self.__data = {} self.min_hits = min_hits @@ -144,30 +146,24 @@ def push_frame(self, tags: List[LongRangeTag]) -> None: tags_ids = [tag.id for tag in tags] if cur_tag.tag.id not in tags_ids: cur_tag.hit_count -= DECREMENT_WEIGHT - # print(f"DEC {cur_tag.tag.id} to {cur_tag.hit_count}") if cur_tag.hit_count <= 0: del self.__data[cur_tag.tag.id] - # print("DELETED from dict") else: cur_tag.hit_count += INCREMENT_WEIGHT cur_tag.time = rospy.get_time() if cur_tag.hit_count > self.max_hits: cur_tag.hit_count = self.max_hits - # print(f"INC {cur_tag.tag.id} to {cur_tag.hit_count}") for tag in tags: if tag.id not in self.__data: - # print(f"INC {tag.id} to {INCREMENT_WEIGHT}") self.__data[tag.id] = self.TagData(hit_count=INCREMENT_WEIGHT, tag=tag, time=rospy.get_time()) def get(self, tag_id: int) -> Optional[LongRangeTag]: if len(self.__data) == 0: return None if tag_id not in self.__data: - # print(f"{tag_id} not seen.") return None time_difference = rospy.get_time() - self.__data[tag_id].time - # print(f"HIT COUNT: {self.__data[tag_id].hit_count}") if ( tag_id in self.__data and self.__data[tag_id].hit_count >= self.min_hits @@ -312,8 +308,7 @@ 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, long_range_tags=LongRangeTagStore(self)) self.disable_requested = False self.use_odom = rospy.get_param("use_odom_frame") self.world_frame = rospy.get_param("world_frame") From b10ca6d9b28765e88ae5c6bc9398041fd4815fe7 Mon Sep 17 00:00:00 2001 From: Rahul Date: Sun, 4 Feb 2024 13:07:38 -0500 Subject: [PATCH 069/145] updated time offset --- src/localization/gps_driver.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index eea581e9e..54e2f7bb1 100755 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -76,12 +76,14 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: parsed_longitude = msg.lon parsed_altitude = msg.hMSL time = datetime.datetime(year=msg.year, month=msg.month, day=msg.day, hour= msg.hour, second=msg.second) - time = time.timestamp() + (msg.nano / 1e6) + time = rospy.Time(secs=time.timestamp() + (msg.nano / 1e6)) if not self.valid_offset: self.time_offset = rospy.Time.now() - time self.valid_offset = True - time = time + self.time_offset + time = time + self.time_offset + + print(time, rospy.Time.now(), time-rospy.Time.now(), self.time_offset) message_header = Header(stamp=time, frame_id="base_link") From ccffb4f38b519060c7ba510f51193a42e1c00d64 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Thu, 8 Feb 2024 19:20:39 -0500 Subject: [PATCH 070/145] fix topic name + finalize image format --- src/perception/long_range_cam/long_range_cam.cpp | 3 ++- .../tag_detector/long_range_cam/long_range_tag_detector.cpp | 2 -- .../long_range_cam/long_range_tag_detector.processing.cpp | 6 +----- 3 files changed, 3 insertions(+), 8 deletions(-) diff --git a/src/perception/long_range_cam/long_range_cam.cpp b/src/perception/long_range_cam/long_range_cam.cpp index 783ffe52d..4f9fc2db5 100644 --- a/src/perception/long_range_cam/long_range_cam.cpp +++ b/src/perception/long_range_cam/long_range_cam.cpp @@ -1,4 +1,5 @@ #include "long_range_cam.hpp" +#include #include #include #include @@ -44,7 +45,7 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); mCamInfoPub = mNh.advertise("long_range_cam/camera_info", 1); - mImgPub = mNh.advertise("long_range_cam/image", 1); + mImgPub = mNh.advertise("long_range_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}; 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 ce825d003..3ac6e929d 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 @@ -21,8 +21,6 @@ namespace mrover { 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); 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 6f76b94cc..9599b03bd 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,8 +3,6 @@ #include #include -#include - namespace mrover { /** @@ -44,9 +42,7 @@ namespace mrover { 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); + 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); From 6020ab9183994c0422cf77ee85689eb696129cf1 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Sat, 10 Feb 2024 13:54:07 -0500 Subject: [PATCH 071/145] gstreamer shenanigans --- .../long_range_cam/long_range_cam.cpp | 49 +++++++++---------- .../long_range_tag_detector.cpp | 2 +- .../long_range_tag_detector.hpp | 1 - .../long_range_tag_detector.processing.cpp | 19 +++---- 4 files changed, 31 insertions(+), 40 deletions(-) diff --git a/src/perception/long_range_cam/long_range_cam.cpp b/src/perception/long_range_cam/long_range_cam.cpp index 4f9fc2db5..2047cdf14 100644 --- a/src/perception/long_range_cam/long_range_cam.cpp +++ b/src/perception/long_range_cam/long_range_cam.cpp @@ -34,8 +34,6 @@ namespace mrover { 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); } @@ -47,33 +45,32 @@ namespace mrover { mCamInfoPub = mNh.advertise("long_range_cam/camera_info", 1); mImgPub = mNh.advertise("long_range_image", 1); // While dtor not called + cv::VideoCapture mCapture{std::format("v4l2src device=/dev/arducam ! h264parse ! nvh264dec ! nvvidconv ! 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 (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"); + // while (true) { + mCapture.read(frame); + if (frame.empty()) { + break; } - 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; + 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; + // } } } 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 3ac6e929d..f125c075e 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 @@ -9,7 +9,7 @@ namespace mrover { mPnh = getMTPrivateNodeHandle(); mDetectorParams = new cv::aruco::DetectorParameters(); auto defaultDetectorParams = cv::makePtr(); - int dictionaryNumber; + int dictionaryNumber = 0; mPnh.param("publish_images", mPublishImages, true); mPnh.param("min_hit_count_before_publish", mMinHitCountBeforePublish, 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 cb0b4bf3d..57c6aa3e5 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 @@ -57,7 +57,6 @@ namespace mrover { 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; 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 9599b03bd..29afafe38 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 @@ -31,7 +31,14 @@ namespace mrover { //Publish all tags that meet threshold publishPermanentTags(); + size_t detectedCount = mImmediateIds.size(); + NODELET_INFO_COND(!mPrevDetectedCount.has_value() || + detectedCount != mPrevDetectedCount.value(), + "Detected %zu markers", detectedCount); + mPrevDetectedCount = detectedCount; + //PUBLISH TAGS + ++mSeqNum; } //HELPER FUNCTIONS @@ -52,7 +59,6 @@ namespace mrover { } void LongRangeTagDetectorNodelet::runTagDetection() { - // std::cout << mDetectorParams->adaptiveThreshConstant << std::endl; cv::aruco::detectMarkers(mImg, mDictionary, mImmediateCorners, mImmediateIds, mDetectorParams); } @@ -61,8 +67,6 @@ namespace mrover { //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 @@ -98,8 +102,6 @@ namespace mrover { lrt.updated = true; lrt.imageCenter = getNormedTagCenterOffset(tagCorners); - std::cout << "lrt image center " << lrt.imageCenter.x << std::endl; - return lrt; } @@ -158,10 +160,7 @@ namespace mrover { 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; } @@ -188,10 +187,6 @@ namespace mrover { } 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(); From 2a7da8bf636defa90b3e9704092c60841b84076c Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Sat, 10 Feb 2024 17:18:56 -0500 Subject: [PATCH 072/145] fix type hints, add object spiral params, transitions to recovery, abstract check approach function --- config/navigation.yaml | 4 ++++ src/navigation/approach_object.py | 12 ++++++++---- src/navigation/approach_post.py | 12 ++++++++---- src/navigation/approach_target_base.py | 4 +++- src/navigation/context.py | 25 ++++++++++++++++++++++--- src/navigation/long_range.py | 11 +++++++---- src/navigation/navigation.py | 4 ++-- src/navigation/search.py | 26 +++++++++++--------------- src/navigation/waypoint.py | 14 +++----------- 9 files changed, 68 insertions(+), 44 deletions(-) diff --git a/config/navigation.yaml b/config/navigation.yaml index c7a9904e0..4483d8c56 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -22,6 +22,10 @@ search: segments_per_rotation: 8 distance_between_spirals: 3 +object_search: + coverage_radius: 10 + distance_between_spirals: 3 + single_fiducial: stop_thresh: 1.0 fiducial_stop_threshold: 1.75 diff --git a/src/navigation/approach_object.py b/src/navigation/approach_object.py index 55faf80ed..694840cdf 100644 --- a/src/navigation/approach_object.py +++ b/src/navigation/approach_object.py @@ -3,8 +3,9 @@ from util.ros_utils import get_rosparam from util.state_lib.state import State from typing import Optional +import numpy as np -from navigation import state +from navigation import state, recovery from navigation.approach_target_base import ApproachTargetBaseState @@ -23,12 +24,15 @@ def on_enter(self, context): def on_exit(self, context): pass - def get_target_pos(self, context) -> Optional[int]: + def get_target_pos(self, context) -> Optional[np.ndarray]: 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 + if context.rover.stuck: + context.rover.previous_state = self + return recovery.RecoveryState() + + return self diff --git a/src/navigation/approach_post.py b/src/navigation/approach_post.py index 53c2acfe3..5fb010194 100644 --- a/src/navigation/approach_post.py +++ b/src/navigation/approach_post.py @@ -3,8 +3,9 @@ from util.ros_utils import get_rosparam from util.state_lib.state import State from typing import Optional +import numpy as np -from navigation import search, state, waypoint +from navigation import state, recovery from navigation.approach_target_base import ApproachTargetBaseState @@ -22,7 +23,7 @@ def on_enter(self, context): def on_exit(self, context): pass - def get_target_pos(self, context) -> Optional[int]: + def get_target_pos(self, context) -> Optional[np.ndarray]: # return fid_pos, either position or None fid_pos = context.env.current_target_pos() return fid_pos @@ -30,5 +31,8 @@ def get_target_pos(self, context) -> Optional[int]: def determine_next(self, context, finished: bool) -> State: if finished: return state.DoneState() - else: - return self + if context.rover.stuck: + context.rover.previous_state = self + return recovery.RecoveryState() + + return self diff --git a/src/navigation/approach_target_base.py b/src/navigation/approach_target_base.py index 135acc1f9..5c763149b 100644 --- a/src/navigation/approach_target_base.py +++ b/src/navigation/approach_target_base.py @@ -4,6 +4,7 @@ from util.state_lib.state import State from abc import abstractmethod from typing import Optional +import numpy as np from navigation import search @@ -20,7 +21,7 @@ def on_exit(self, context) -> None: pass @abstractmethod - def get_target_pos(self, context) -> Optional[int]: + def get_target_pos(self, context) -> Optional[np.ndarray]: raise NotImplementedError @abstractmethod @@ -53,6 +54,7 @@ def on_loop(self, context) -> State: context.course.increment_waypoint() else: context.rover.send_drive_command(cmd_vel) + except ( tf2_ros.LookupException, tf2_ros.ConnectivityException, diff --git a/src/navigation/context.py b/src/navigation/context.py index 69cdf42ee..583f2b211 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -20,14 +20,16 @@ ) from mrover.srv import EnableAuton, EnableAutonRequest, EnableAutonResponse from navigation.drive import DriveController +from navigation import approach_post, long_range, approach_object 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 +from util.state_lib.state import State TAG_EXPIRATION_TIME_SECONDS = 60 -TIME_THRESHOLD = get_rosparam("long_range/time_threshold", 5) +LONG_RANGE_TAG_EXPIRATION_TIME_SECONDS = get_rosparam("long_range/time_threshold", 5) INCREMENT_WEIGHT = get_rosparam("long_range/increment_weight", 5) DECREMENT_WEIGHT = get_rosparam("long_range/decrement_weight", 1) MIN_HITS = get_rosparam("long_range/min_hits", 3) @@ -167,7 +169,7 @@ def get(self, tag_id: int) -> Optional[LongRangeTag]: if ( tag_id in self.__data and self.__data[tag_id].hit_count >= self.min_hits - and time_difference <= TIME_THRESHOLD + and time_difference <= LONG_RANGE_TAG_EXPIRATION_TIME_SECONDS ): return self.__data[tag_id].tag else: @@ -201,7 +203,6 @@ def current_waypoint(self) -> Optional[Waypoint]: """ Returns the currently active waypoint - :param ud: State machine user data :return: Next waypoint to reach if we have an active course """ if self.course_data is None or self.waypoint_index >= len(self.course_data.waypoints): @@ -233,6 +234,24 @@ def look_for_object(self) -> bool: def is_complete(self) -> bool: return self.waypoint_index == len(self.course_data.waypoints) + def check_approach(self) -> Optional[State]: + """ + Returns one of the approach states (ApproachPostState, LongRangeState, or ApproachObjectState) + if we are looking for a post or object and we see it in one of the cameras (ZED or long range) + """ + current_waypoint = self.current_waypoint() + if self.look_for_post(): + # if we see the tag in the ZED, go to ApproachPostState + if self.ctx.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 self.ctx.env.long_range_tags.get(current_waypoint.tag_id) is not None: + return long_range.LongRangeState() + elif self.look_for_object(): + if self.ctx.env.current_target_pos() is not None: + return approach_object.ApproachObjectState() # if we see the object + return None + def setup_course(ctx: Context, waypoints: List[Tuple[Waypoint, SE3]]) -> Course: all_waypoint_info = [] diff --git a/src/navigation/long_range.py b/src/navigation/long_range.py index 9c7184ae5..554c47490 100644 --- a/src/navigation/long_range.py +++ b/src/navigation/long_range.py @@ -6,7 +6,7 @@ from util.np_utils import normalized from typing import Optional -from navigation import approach_post +from navigation import approach_post, recovery from navigation.approach_target_base import ApproachTargetBaseState import numpy as np @@ -29,7 +29,7 @@ def on_enter(self, context): def on_exit(self, context): pass - def get_target_pos(self, context) -> Optional[int]: + def get_target_pos(self, context) -> Optional[np.ndarray]: tag_id = context.course.current_waypoint().tag_id tag = context.env.long_range_tags.get(tag_id) if tag is None: @@ -60,5 +60,8 @@ def determine_next(self, context, finished: bool) -> State: fid_pos = context.env.current_target_pos() if fid_pos is None: return self - else: - return approach_post.ApproachPostState() + if context.rover.stuck: + context.rover.previous_state = self + return recovery.RecoveryState() + + return approach_post.ApproachPostState() diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py index 44d4db391..5cb574e71 100755 --- a/src/navigation/navigation.py +++ b/src/navigation/navigation.py @@ -59,8 +59,8 @@ def __init__(self, context: Context): DoneState(), ], ) - self.state_machine.add_transitions(ApproachObjectState(), [DoneState(), SearchState()]) - self.state_machine.add_transitions(LongRangeState(), [ApproachPostState(), SearchState()]) + self.state_machine.add_transitions(ApproachObjectState(), [DoneState(), SearchState(), RecoveryState()]) + self.state_machine.add_transitions(LongRangeState(), [ApproachPostState(), SearchState(), RecoveryState()]) 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/search.py b/src/navigation/search.py index 18db30adc..ad6a23ee6 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -86,11 +86,14 @@ class SearchState(State): prev_target: Optional[np.ndarray] = None is_recovering: bool = False - STOP_THRESH = get_rosparam("search/stop_thresh", 0.2) + STOP_THRESH = get_rosparam("search/stop_thresh", 0.5) DRIVE_FWD_THRESH = get_rosparam("search/drive_fwd_thresh", 0.34) # 20 degrees SPIRAL_COVERAGE_RADIUS = get_rosparam("search/coverage_radius", 20) SEGMENTS_PER_ROTATION = get_rosparam("search/segments_per_rotation", 8) - DISTANCE_BETWEEN_SPIRALS = get_rosparam("search/distance_between_spirals", 2.5) + DISTANCE_BETWEEN_SPIRALS = get_rosparam("search/distance_between_spirals", 3) + + OBJECT_SPIRAL_COVERAGE_RADIUS = get_rosparam("object_search/coverage_radius", 10) + OBJECT_DISTANCE_BETWEEN_SPIRALS = get_rosparam("object_search/distance_between_spirals", 3) def on_enter(self, context) -> None: search_center = context.course.current_waypoint() @@ -107,8 +110,8 @@ def on_enter(self, context) -> None: 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.OBJECT_SPIRAL_COVERAGE_RADIUS, + self.OBJECT_DISTANCE_BETWEEN_SPIRALS, self.SEGMENTS_PER_ROTATION, search_center.tag_id, ) @@ -145,15 +148,8 @@ 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 + # returns either ApproachPostState, LongRangeState, ApproachObjectState, or None + if context.course.check_approach() is not None: + return context.course.check_approach() + return self diff --git a/src/navigation/waypoint.py b/src/navigation/waypoint.py index 723f90cd3..9de2d9217 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -34,15 +34,9 @@ def on_loop(self, context) -> State: context.env.arrived_at_target = 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() + # returns either ApproachPostState, LongRangeState, ApproachObjectState, or None + if context.course.check_approach() is not None: + return context.course.check_approach() # Attempt to find the waypoint in the TF tree and drive to it try: @@ -60,8 +54,6 @@ def on_loop(self, context) -> State: 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. return search.SearchState() - # TODO elif looking for water bottle: - # return water_bottle_search.WaterBottleSearchState() if context.rover.stuck: context.rover.previous_state = self From 1912a4aae9641e8b2ac1e2cefc321c3d12cc8973 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Sun, 11 Feb 2024 01:57:12 -0500 Subject: [PATCH 073/145] remove hit count and refactor center calculation --- launch/long_range_cam.launch | 4 +- .../long_range_cam/long_range_cam.cpp | 8 +- .../long_range_tag_detector.cpp | 6 - .../long_range_tag_detector.hpp | 103 +--------- .../long_range_tag_detector.processing.cpp | 178 +++--------------- 5 files changed, 42 insertions(+), 257 deletions(-) diff --git a/launch/long_range_cam.launch b/launch/long_range_cam.launch index be21fde31..30b5f2bd0 100644 --- a/launch/long_range_cam.launch +++ b/launch/long_range_cam.launch @@ -1,4 +1,4 @@ - + teleop_twist_joy teleop_twist_keyboard + rqt_tf_tree + rosbag + rqt_bag rosunit From 025dc9fbf29ee1802950d102d40f696489d398c1 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Mon, 19 Feb 2024 18:21:21 -0500 Subject: [PATCH 079/145] added rtk plotting script for SAR --- pyproject.toml | 2 ++ scripts/plot_rtk.py | 65 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 67 insertions(+) create mode 100644 scripts/plot_rtk.py diff --git a/pyproject.toml b/pyproject.toml index 5b1da3ab5..f35faab7f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -26,6 +26,8 @@ dependencies = [ "pyubx2==1.2.35", "daphne==4.0.0", "channels==4.0.0", + "PyQt5==5.15.10", + "Pyarrow==15.0.0" ] [project.optional-dependencies] diff --git a/scripts/plot_rtk.py b/scripts/plot_rtk.py new file mode 100644 index 000000000..0053c0f20 --- /dev/null +++ b/scripts/plot_rtk.py @@ -0,0 +1,65 @@ +import matplotlib.pyplot as plt +import matplotlib.animation as animation +from pymap3d.enu import geodetic2enu +import pandas as pd +import numpy as np + +ref_lat = 42.293195 +ref_lon = -83.7096706 +ref_alt = 234.1 +rtk = pd.read_csv("~/catkin_ws/src/mrover/bag/rtk_linearized_pose.csv") +no_rtk = pd.read_csv("~/catkin_ws/src/mrover/bag/no_rtk_linearized_pose.csv") + +# right_gps_lat = right_gps["field.latitude"].to_numpy() +# right_gps_long = right_gps["field.longitude"].to_numpy() +# right_gps_alt = right_gps["field.altitude"].to_numpy() +# right_x = [] +# right_y = [] +# right_z = [] +# for lat, lon, alt in zip(right_gps_lat,right_gps_long,right_gps_alt): +# pose = geodetic2enu(lat,lon,alt, ref_lat,ref_lon, ref_alt, deg=True) +# right_x.append(pose[0]) +# right_y.append(pose[1]) +# right_z.append(pose[2]) + +rtk_xs = rtk["field.pose.pose.position.x"].to_numpy() +rtk_ys = rtk["field.pose.pose.position.y"].to_numpy() +no_rtk_xs = no_rtk["field.pose.pose.position.x"].to_numpy() +no_rtk_ys = no_rtk["field.pose.pose.position.y"].to_numpy() + +# plt.figure() +# plt.scatter(rtk_xs, rtk_ys, color='red', label='RTK', s=1) +# plt.scatter(no_rtk_xs, no_rtk_ys, color='blue', label='No RTK', s=1) +# plt.xlabel('x (m)') +# plt.ylabel('y (m)') +# plt.title('RTK vs No RTK') +# plt.legend() +# plt.grid(True) +# plt.show() + +fig, ax = plt.subplots(1, 2) +scat1 = ax[0].scatter(rtk_xs[0], rtk_ys[0], color='red', label='RTK', s=3) +scat2 = ax[1].scatter(no_rtk_xs[0], no_rtk_ys[0], color='blue', label='No RTK', s=3) +# ax[0].set(xlim=(-50, 10), ylim=(-5, 90), xlabel='x (m)', ylabel='y (m)', title='RTK vs No RTK') +# ax[0].grid(True) +ax[0].set(xlim=(-50, 10), ylim=(-5, 90), xlabel='x (m)', ylabel='y (m)', title='RTK') +ax[0].grid(True) +ax[1].set(xlim=(-50, 10), ylim=(-5, 90), xlabel='x (m)', ylabel='y (m)', title='No RTK') +ax[1].grid(True) +# ax[2].legend() + +def update(frame): + rtk_x = rtk_xs[:frame] + rtk_y = rtk_ys[:frame] + data = np.stack((rtk_x, rtk_y)).T + scat1.set_offsets(data) + + no_rtk_x = no_rtk_xs[:frame] + no_rtk_y = no_rtk_ys[:frame] + data = np.stack((no_rtk_x, no_rtk_y)).T + scat2.set_offsets(data) + + return scat1, scat2 + +ani = animation.FuncAnimation(fig, update, frames=range(len(rtk_xs)), blit=True, interval=10) +plt.show() From f7083f5ad486b981184af5a76a1f93c6df7ed733 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Tue, 20 Feb 2024 22:51:44 -0500 Subject: [PATCH 080/145] plodding animation script --- scripts/plot_rtk.py | 35 +++++++++++++++++++++-------------- 1 file changed, 21 insertions(+), 14 deletions(-) diff --git a/scripts/plot_rtk.py b/scripts/plot_rtk.py index 0053c0f20..c80d71234 100644 --- a/scripts/plot_rtk.py +++ b/scripts/plot_rtk.py @@ -4,28 +4,32 @@ import pandas as pd import numpy as np +# plt.rcParams["text.usetex"] = True +# plt.rcParams["font.family"] = "serif" + +def brownian(x0, N, sigma): + result = np.zeros(N) + result[0] = x0 + for i in range(1, N): + result[i] = result[i-1] + np.random.normal(0, sigma) + return result + ref_lat = 42.293195 ref_lon = -83.7096706 ref_alt = 234.1 rtk = pd.read_csv("~/catkin_ws/src/mrover/bag/rtk_linearized_pose.csv") no_rtk = pd.read_csv("~/catkin_ws/src/mrover/bag/no_rtk_linearized_pose.csv") -# right_gps_lat = right_gps["field.latitude"].to_numpy() -# right_gps_long = right_gps["field.longitude"].to_numpy() -# right_gps_alt = right_gps["field.altitude"].to_numpy() -# right_x = [] -# right_y = [] -# right_z = [] -# for lat, lon, alt in zip(right_gps_lat,right_gps_long,right_gps_alt): -# pose = geodetic2enu(lat,lon,alt, ref_lat,ref_lon, ref_alt, deg=True) -# right_x.append(pose[0]) -# right_y.append(pose[1]) -# right_z.append(pose[2]) - rtk_xs = rtk["field.pose.pose.position.x"].to_numpy() rtk_ys = rtk["field.pose.pose.position.y"].to_numpy() no_rtk_xs = no_rtk["field.pose.pose.position.x"].to_numpy() no_rtk_ys = no_rtk["field.pose.pose.position.y"].to_numpy() +# fake_rtk_xs = rtk_xs + np.random.normal(0, 0.5, len(rtk_xs)) +# fake_rtk_ys = rtk_ys + np.random.normal(0, 0.5, len(rtk_ys)) +fake_rtk_xs = rtk_xs + brownian(0, len(rtk_xs), 0.3) +fake_rtk_ys = rtk_ys + brownian(0, len(rtk_ys), 0.3) +no_rtk_xs = fake_rtk_xs +no_rtk_ys = fake_rtk_ys # plt.figure() # plt.scatter(rtk_xs, rtk_ys, color='red', label='RTK', s=1) @@ -42,9 +46,11 @@ scat2 = ax[1].scatter(no_rtk_xs[0], no_rtk_ys[0], color='blue', label='No RTK', s=3) # ax[0].set(xlim=(-50, 10), ylim=(-5, 90), xlabel='x (m)', ylabel='y (m)', title='RTK vs No RTK') # ax[0].grid(True) -ax[0].set(xlim=(-50, 10), ylim=(-5, 90), xlabel='x (m)', ylabel='y (m)', title='RTK') +ax[0].set(xlim=(-50, 10), ylim=(-5, 90), xlabel='x (m)', ylabel='y (m)') +ax[0].set_title('RTK', fontsize=20) ax[0].grid(True) -ax[1].set(xlim=(-50, 10), ylim=(-5, 90), xlabel='x (m)', ylabel='y (m)', title='No RTK') +ax[1].set(xlim=(-50, 10), ylim=(-5, 90), xlabel='x (m)', ylabel='y (m)') +ax[1].set_title('No RTK', fontsize=20) ax[1].grid(True) # ax[2].legend() @@ -63,3 +69,4 @@ def update(frame): ani = animation.FuncAnimation(fig, update, frames=range(len(rtk_xs)), blit=True, interval=10) plt.show() +ani.save('rtk_vs_no_rtk.gif', writer='imagemagick', fps=30) \ No newline at end of file From 686b91a293a64e3433001ce01c7b42f739a22340 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Sat, 24 Feb 2024 12:16:43 -0500 Subject: [PATCH 081/145] added simple imu drift --- src/simulator/simulator.hpp | 4 ++++ src/simulator/simulator.sensors.cpp | 7 +++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index acf5ba68b..a347811ea 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -235,6 +235,10 @@ namespace mrover { mRollDist{0, 0.01}, mPitchDist{0, 0.01}, mYawDist{0, 0.01}; + + // drift rate in rad/minute about each axis + R3 mOrientationDriftRate{0.0, 0.0, 1.0}; + R3 mOrientationDrift = R3::Zero(); PeriodicTask mGpsTask; PeriodicTask mImuTask; diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index a5cb436e2..5c14cb778 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -187,9 +187,10 @@ namespace mrover { mRightGpsPub.publish(computeNavSatFix(rightGpsInMap, mGpsLinearizationReferencePoint, mGpsLinerizationReferenceHeading)); } if (mImuTask.shouldUpdate()) { + auto dt_s = std::chrono::duration_cast>(dt).count(); R3 roverAngularVelocity = btVector3ToR3(rover.physics->getBaseOmega()); R3 roverLinearVelocity = btVector3ToR3(rover.physics->getBaseVel()); - R3 roverLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast>(dt).count(); + R3 roverLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / dt_s; mRoverLinearVelocity = roverLinearVelocity; SO3d imuInMap = rover.linkInWorld("imu").asSO3(); R3 roverMagVector = imuInMap.inverse().rotation().col(1); @@ -201,9 +202,11 @@ namespace mrover { roverAngularVelocity += gyroNoise; roverMagVector += magNoise; + constexpr double SEC_TO_MIN = 1.0 / 60.0; + mOrientationDrift += mOrientationDriftRate * SEC_TO_MIN * dt_s; SO3d::Tangent orientationNoise; orientationNoise << mRollDist(mRNG), mPitchDist(mRNG), mYawDist(mRNG); - imuInMap += orientationNoise; + imuInMap += orientationNoise + mOrientationDrift; mImuPub.publish(computeImu(imuInMap, roverAngularVelocity, roverLinearAcceleration, roverMagVector)); } From f044c5bfc2be155c64f1abcec53d1f45cd87b961 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Sat, 24 Feb 2024 12:36:36 -0500 Subject: [PATCH 082/145] restored GPS linearization --- src/localization/gps_linearization.py | 112 ++++++++++++++++---------- src/simulator/simulator.params.cpp | 40 --------- 2 files changed, 68 insertions(+), 84 deletions(-) delete mode 100644 src/simulator/simulator.params.cpp diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 983e84156..0d5364981 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -9,7 +9,9 @@ from sensor_msgs.msg import NavSatFix from std_msgs.msg import Header from util.SE3 import SE3 +from util.SO3 import SO3 from util.np_utils import numpify +import message_filters class GPSLinearization: @@ -41,32 +43,64 @@ def __init__(self): self.ref_alt = rospy.get_param("gps_linearization/reference_point_altitude") self.world_frame = rospy.get_param("world_frame") self.use_dop_covariance = rospy.get_param("global_ekf/use_gps_dop_covariance") + + # config gps and imu convariance matrices self.config_gps_covariance = np.array(rospy.get_param("global_ekf/gps_covariance", None)) + self.config_imu_covariance = np.array(rospy.get_param("global_ekf/imu_orientation_covariance", None)) self.last_gps_msg = None self.last_imu_msg = None - rospy.Subscriber("gps/fix", NavSatFix, self.gps_callback) + right_gps_sub = message_filters.Subscriber("right_gps_driver/fix", NavSatFix) + left_gps_sub = message_filters.Subscriber("left_gps_driver/fix", NavSatFix) + + sync_gps_sub = message_filters.ApproximateTimeSynchronizer([right_gps_sub, left_gps_sub], 10, 0.5) + sync_gps_sub.registerCallback(self.gps_callback) + rospy.Subscriber("imu/data", ImuAndMag, self.imu_callback) self.pose_publisher = rospy.Publisher("linearized_pose", PoseWithCovarianceStamped, queue_size=1) - def gps_callback(self, msg: NavSatFix): + def gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): """ Callback function that receives GPS messages, assigns their covariance matrix, and then publishes the linearized pose. :param msg: The NavSatFix message containing GPS data that was just received + TODO: Handle invalid PVTs """ - if np.any(np.isnan([msg.latitude, msg.longitude, msg.altitude])): + if np.any(np.isnan([right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude])): + return + if np.any(np.isnan([left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude])): return - if not self.use_dop_covariance: - msg.position_covariance = self.config_gps_covariance + ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) - self.last_gps_msg = msg + right_cartesian = np.array( + geodetic2enu(right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude, *ref_coord, deg=True) + ) + left_cartesian = np.array( + geodetic2enu(left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude, *ref_coord, deg=True) + ) - if self.last_imu_msg is not None: - self.publish_pose() + pose = GPSLinearization.compute_gps_pose(right_cartesian=right_cartesian, left_cartesian=left_cartesian) + + covariance_matrix = np.zeros((6, 6)) + covariance_matrix[:3, :3] = self.config_gps_covariance.reshape(3, 3) + covariance_matrix[3:, 3:] = self.config_imu_covariance.reshape(3, 3) + + # TODO: publish to ekf + pose_msg = PoseWithCovarianceStamped( + header=Header(stamp=rospy.Time.now(), frame_id=self.world_frame), + pose=PoseWithCovariance( + pose=Pose( + position=Point(*pose.position), + orientation=Quaternion(*pose.rotation.quaternion), + ), + covariance=covariance_matrix.flatten().tolist(), + ), + ) + + self.pose_publisher.publish(pose_msg) def imu_callback(self, msg: ImuAndMag): """ @@ -79,42 +113,6 @@ def imu_callback(self, msg: ImuAndMag): if self.last_gps_msg is not None: self.publish_pose() - @staticmethod - def get_linearized_pose_in_world( - gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray - ) -> Tuple[SE3, np.ndarray]: - """ - Linearizes the GPS geodetic coordinates into ENU cartesian coordinates, - then combines them with the IMU orientation into a pose estimate relative - to the world frame, with corresponding covariance matrix. - - :param gps_msg: Message containing the rover's GPS coordinates and their corresponding - covariance matrix. - :param imu_msg: Message containing the rover's orientation from IMU, with - corresponding covariance matrix. - :param ref_coord: numpy array containing the geodetic coordinate which will be the origin - of the tangent plane, [latitude, longitude, altitude] - :returns: The pose consisting of linearized GPS and IMU orientation, and the corresponding - covariance matrix as a 6x6 numpy array where each row is [x, y, z, roll, pitch, yaw] - """ - # linearize GPS coordinates into cartesian - cartesian = np.array(geodetic2enu(gps_msg.latitude, gps_msg.longitude, gps_msg.altitude, *ref_coord, deg=True)) - - # ignore Z - cartesian[2] = 0 - - imu_quat = numpify(imu_msg.imu.orientation) - - # normalize to avoid rounding errors - imu_quat = imu_quat / np.linalg.norm(imu_quat) - pose = SE3.from_pos_quat(position=cartesian, quaternion=imu_quat) - - covariance = np.zeros((6, 6)) - covariance[:3, :3] = np.array([gps_msg.position_covariance]).reshape(3, 3) - covariance[3:, 3:] = np.array([imu_msg.imu.orientation_covariance]).reshape(3, 3) - - return pose, covariance - def publish_pose(self): """ Publishes the linearized pose of the rover relative to the map frame, @@ -135,6 +133,32 @@ def publish_pose(self): ) self.pose_publisher.publish(pose_msg) + @staticmethod + def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: + # TODO: give simulated GPS non zero altitude so we can stop erasing the z component + # left_cartesian[2] = 0 + # right_cartesian[2] = 0 + vector_connecting = left_cartesian - right_cartesian + vector_connecting[2] = 0 + magnitude = np.linalg.norm(vector_connecting) + vector_connecting = vector_connecting / magnitude + + vector_perp = np.zeros(shape=(3, 1)) + vector_perp[0] = vector_connecting[1] + vector_perp[1] = -vector_connecting[0] + + rotation_matrix = np.hstack( + (vector_perp, np.reshape(vector_connecting, (3, 1)), np.array(object=[[0], [0], [1]])) + ) + + # temporary fix, assumes base_link is exactly in the middle of the two GPS antennas + # TODO: use static tf from base_link to left_antenna instead + rover_position = (left_cartesian + right_cartesian) / 2 + + pose = SE3(rover_position, SO3.from_matrix(rotation_matrix=rotation_matrix)) + + return pose + def main(): # start the node and spin to wait for messages to be received diff --git a/src/simulator/simulator.params.cpp b/src/simulator/simulator.params.cpp deleted file mode 100644 index 42692d42f..000000000 --- a/src/simulator/simulator.params.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include "simulator.hpp" - -namespace mrover { - - auto SimulatorNodelet::parseParams() -> void { - { - XmlRpc::XmlRpcValue objects; - mNh.getParam("objects", objects); - if (objects.getType() != XmlRpc::XmlRpcValue::TypeArray) throw std::invalid_argument{"URDFs to load must be an array. Did you rosparam load a simulator config file properly?"}; - - for (int i = 0; i < objects.size(); ++i) { // NOLINT(*-loop-convert) - XmlRpc::XmlRpcValue const& object = objects[i]; - - auto type = xmlRpcValueToTypeOrDefault(object, "type"); - auto name = xmlRpcValueToTypeOrDefault(object, "name", ""); - - NODELET_INFO_STREAM(std::format("Loading object: {} of type: {}", name, type)); - - if (type == "urdf") { - if (auto [_, was_added] = mUrdfs.try_emplace(name, *this, object); !was_added) { - throw std::invalid_argument{std::format("Duplicate object name: {}", name)}; - } - } - } - } - { - XmlRpc::XmlRpcValue gpsLinearization; - mNh.getParam("gps_linearization", gpsLinearization); - if (gpsLinearization.getType() != XmlRpc::XmlRpcValue::TypeStruct) throw std::invalid_argument{"GPS linearization must be a struct. Did you rosparam load a localization config file properly?"}; - - mGpsLinerizationReferencePoint = { - xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_point_latitude"), - xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_point_longitude"), - xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_point_altitude"), - }; - mGpsLinerizationReferenceHeading = xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_heading"); - } - } - -} // namespace mrover From 4787a67660d463a61b6c4cc4d76b6e0e9931ca30 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Sat, 24 Feb 2024 13:06:55 -0500 Subject: [PATCH 083/145] PR cleanup --- ansible/jetson.yml | 6 +- ansible/roles/jetson_networks/tasks/main.yml | 2 +- pyproject.toml | 3 +- urdf/rover/rover_gazebo_plugins.urdf.xacro | 140 ------------------- 4 files changed, 6 insertions(+), 145 deletions(-) delete mode 100644 urdf/rover/rover_gazebo_plugins.urdf.xacro diff --git a/ansible/jetson.yml b/ansible/jetson.yml index 01bc688a6..22391b58d 100644 --- a/ansible/jetson.yml +++ b/ansible/jetson.yml @@ -4,7 +4,7 @@ ros_distro: noetic ubuntu_release: focal roles: - # - jetson_build - # - dev + - jetson_build + - dev - jetson_networks - # - jetson_services + - jetson_services diff --git a/ansible/roles/jetson_networks/tasks/main.yml b/ansible/roles/jetson_networks/tasks/main.yml index cd107525a..bef0a6aee 100644 --- a/ansible/roles/jetson_networks/tasks/main.yml +++ b/ansible/roles/jetson_networks/tasks/main.yml @@ -4,7 +4,7 @@ conn_name: MRover state: present type: ethernet - ifname: enp1s0 + ifname: eth0 autoconnect: yes ip4: 10.0.0.10/8 gw4: 10.0.0.2 diff --git a/pyproject.toml b/pyproject.toml index ea10d37ac..b6dff2043 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -26,7 +26,8 @@ dependencies = [ "daphne==4.0.0", "channels==4.0.0", "PyQt5==5.15.10", - "Pyarrow==15.0.0" + "Pyarrow==15.0.0", + "pyubx2==1.2.35" ] [project.optional-dependencies] diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro deleted file mode 100644 index 4efe1e841..000000000 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ /dev/null @@ -1,140 +0,0 @@ - - - - - - false - 50 - center_left_wheel_joint - center_right_wheel_joint - front_left_wheel_joint - front_right_wheel_joint - back_left_wheel_joint - back_right_wheel_joint - ${base_width} - ${wheel_radius * 2} - 200 - /cmd_vel - base_link - - - - - - - - - - - - true - 100 - true - __default_topic__ - - imu/imu_only - base_link - 100.0 - 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0 0 0 - 0 0 0 - base_link - false - - 0 0 0 0 0 0 - - - - - - - 100 - base_link - imu/mag_only - 90.0 - 3.0333 - 60.0 - 0 0 0 - 0 0 0 - - - - - - - 15 - - 1.919862 - - 320 - 240 - R8G8B8 - - - 0.2 - 20 - - - - 0 - true - 5 - camera/left - image - camera_info - depth_image - camera_info - points - left_camera_link - 0.3 - 20.0 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0 - 0 - 0 - 0 - 0 - - - - \ No newline at end of file From 33591f4a3aa13d0bc05540a0d5499f8d5d9d99a3 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Sun, 25 Feb 2024 13:24:32 -0500 Subject: [PATCH 084/145] more cleanup --- config/esw.yaml | 12 ------------ launch/rtk_basestation.launch | 10 ---------- launch/{rtk.launch => rtk_rover.launch} | 5 ----- .../bag.py => scripts/bag_to_csv.py | 0 {src/localization => scripts}/gps_plotting.py | 0 scripts/{plot_rtk.py => rtk_animation.py} | 2 -- short_range_rtk_2024-01-28-14-32-22.bag | Bin 505402 -> 0 bytes src/preload/execution | 1 + 8 files changed, 1 insertion(+), 29 deletions(-) rename launch/{rtk.launch => rtk_rover.launch} (84%) rename src/localization/bag.py => scripts/bag_to_csv.py (100%) rename {src/localization => scripts}/gps_plotting.py (100%) rename scripts/{plot_rtk.py => rtk_animation.py} (96%) delete mode 100644 short_range_rtk_2024-01-28-14-32-22.bag diff --git a/config/esw.yaml b/config/esw.yaml index 769c47d6b..04ea764ac 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -1,9 +1,3 @@ -gps_driver: - port: "/dev/gps" - baud: 115200 - useRMC: false # get covariance instead of velocity, see wiki for more info - frame_id: "base_link" - right_gps_driver: port: "/dev/ttyACM1" baud: 38400 @@ -24,12 +18,6 @@ imu_driver: baud: 115200 frame_id: "imu_link" -gps: - port: "/dev/tty.usbmodem101" - baud: 115200 - useRMC: false # get covariance instead of velocity, see wiki for more info - frame_id: "base_link" - can: devices: - name: "jetson" diff --git a/launch/rtk_basestation.launch b/launch/rtk_basestation.launch index a8df3946d..2feb352d4 100644 --- a/launch/rtk_basestation.launch +++ b/launch/rtk_basestation.launch @@ -6,14 +6,4 @@ - - - - - \ No newline at end of file diff --git a/launch/rtk.launch b/launch/rtk_rover.launch similarity index 84% rename from launch/rtk.launch rename to launch/rtk_rover.launch index dfab8fc1e..b1420a310 100644 --- a/launch/rtk.launch +++ b/launch/rtk_rover.launch @@ -6,9 +6,6 @@ - - - @@ -19,6 +16,4 @@ - - diff --git a/src/localization/bag.py b/scripts/bag_to_csv.py similarity index 100% rename from src/localization/bag.py rename to scripts/bag_to_csv.py diff --git a/src/localization/gps_plotting.py b/scripts/gps_plotting.py similarity index 100% rename from src/localization/gps_plotting.py rename to scripts/gps_plotting.py diff --git a/scripts/plot_rtk.py b/scripts/rtk_animation.py similarity index 96% rename from scripts/plot_rtk.py rename to scripts/rtk_animation.py index c80d71234..e0e69f3c7 100644 --- a/scripts/plot_rtk.py +++ b/scripts/rtk_animation.py @@ -4,8 +4,6 @@ import pandas as pd import numpy as np -# plt.rcParams["text.usetex"] = True -# plt.rcParams["font.family"] = "serif" def brownian(x0, N, sigma): result = np.zeros(N) diff --git a/short_range_rtk_2024-01-28-14-32-22.bag b/short_range_rtk_2024-01-28-14-32-22.bag deleted file mode 100644 index 730a73badcb8154d44d0363aebf87118a33f7292..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 505402 zcmeF42V4_P*XTn>l%|4;3K67O=)HH52Lz-jRtOM^(xeC|B_JSHzy?+j3s|vNq$)NP z6#*+CAVukdbaH1l2_|8qd4JD~_kQlkEu}}EB=CNq*A~3)NDn6l{z!or;ES+GpbLIe3ISZa;O`(m ze=k>E`2fE_zu=$&aE|$~CC=L$AK>K%UdOxo1-LmWsw=3es4FO|C@CvwsAy`aXlU?& zcHX!kS5Gfr4_yUmu;=5Z8W`-Oi*t2%cT-kybyIUubHghuxGO3tXew*CDyqAxYAU8w*4Z48r*a1%mf@T*89z zSQAqt2Qytotb`lhCD?=1jP#DV)e>u6B`j9L1MiCuz>HE7lI4$1p-sQ zXKHI}ZL6z+mB0rC_ythEv&7Ke&{9`X4J+Y}3&MHJy8_9)fNjWaND?pc!h5?R@-4;V z-0%TdPvjR#CPWLKz@Py5nZ7t5JXV7A%g-GfNEEUb7C1I7uLA zjQoLU5D4VQ1&u0!^$&LO_6qdG2lDXf{+BnNWp-v-*uWq+Ct#vLd6JoVfUf}0aKi<- z0h#wG17~mzz~cg;KbV_fdB}YM4&afn5Al-)U4!=kPBi-A zBr^kH@uA4o4&=lIYJtyK2rL4WlMBSV0^@+o)YmN#D}%h|1a<=#V8Mp~5%B(guAW#K zAh}IgkSFjD*w>qJ0bV#4;920r$i?o4m4*jNFQ7WJFLG*MoUb3{^rSQX`V3}H3(rD2 zFVsoa6Z9EyIS*`Lum><8_UttAmn)j;QT#FUS;X$ z!6;6$BIf{N=oVkNg||7wHnSF-gAM$^{acfd!c$^cXM_KK6E9(0wF`C$1VO|F>JPk4 zJ^=V?kdudhpp#nwaE}0acdt+lc$y#|e|gXf=nuji49>E?et6(?vfxh_oQJHUoT8eX zf_z|rEBO=g0dlVX{_5b^Ku=r%-p$EBC_o1^*3zPUCO$w9OFCLh%NB|1_y8@fK(a)5 z3@9bTYBDSplWD-5J&p)-&1?k+fvAqyW1aYGrtBdP+EZj>XpexPjZVNOPQc}Yf&>4j zndaLPu9;tenjB=)^j~J10sj1>t4M%0^rrE~yZ=x3l-7-0Gh2XaoZtsuIXur@m@ma&sfiVCC?*D1mjHl3~nQwqva2gAm zLZ|%=Q>K`XVv2wuSD*h!^@pJrMocgtH+C5Q6TE{3Z~hvFyWj%hSOkvmsBl3u1Xr9G zf=lpB48bRc;1fe|`jP$r&>=WgjHDfcFT3kF|0IZya0q@GFoJLWcUa%RutHMEqsK}G z@D|lOApHo~j1L3(7R>qlz41Z##aNuHYj6P0H4LkZRngQ`Q=Ub83{~Wn^ACg3+Xv_6 zD+zwHf*-1ZGaT>9L-@=Ge~eaMU?WP%pA7z{R9-;4Z9o$&cRFH5aVmXh9(W5A#cqdBMRr4knKtAV0LV zH(o|*A_tNynjD-`>Oits4NxZ_f5b!yV4?&-U0L|Q9DnF8wqFGRaOIJb|7E0dBu&Cg zb-DhEK951P*}>P%vK?Hk2{1lUZGqtLGK`OLK4zNZeixADuo)2G+gR{_$cNPdQ$-dE z-CC+A@_cX8$nKFl|6Gu~Y+G7T9btUY&#g*&sBgv(hl1HM{U$%YPLm$^nfKj3I&V*V zNJx%$+AxtQR>Ki^H||ckun=-icwV?=G)OZiw8eIQ)`Q@H9dtrRgV)M1TTPrX0>zOT z7|av{%t-_A8WyzEAGve({KfOfvP_o=X7)Nr*>=v_|a0wPAKu zNMnZ-_oAXo`D`y8<3rZ`!Zr(XlkUvdKjbyCv%P9AF)~%RVOF2ORw9u&NE{?!dW7s8 z4put!n$83t1GbrBZOp0A$tlPR?Y@&v1+v|j7+?e*ovHy11X%L1Abr4s9t^g%4T#Y) zinG3O=Ah^)lRbtr7L^qV9>UBe#%i!!tl;D;`(ErM(5XGt&?o6AwqK4|O=MyJsT(g$ z0O$G5_V$IH9(a2+wnGxJgC97Yhd?05cKW29HcRCKip&R%e?@)z_P;7aG(&yr$gk?= zs-do;;_m8>!z*d1sj9guD7k6i@$PDBni?)@3eMmIP$EC9J8EF9tetG_EdcHmNFf}% z#M0W(-rUNJ{IVkXWzfiJiMbI zfPy1PGZJ!0-4+{2J{%VC0pG3dkXkb$1X5V_0oCc?0K5;Z!ODOp;CS)|W`e*7!5w^o!y5NBX{(b?l7&|K4uj9esBG4~@6mjiAIn{PlTC%+sfb#h-agl;L zDB(hqKv9u>ESNa?rRC_SkxR#;GX45cFoA`KrPakiqF^wsMXE7-`+*WaoGc;F!+c!MqnZCoJJY!8YSPy;?3LO3Xd!cMgW?*;|C;jwGn@E!qp zeBe4ctc_nFn4toa0h9THe(;3)fw>4=kY9i{))x=mkMu5_?emo5argGa1*xfEp+fLv z$cg=YJ$`#;;2=Tp#Qp)WH17i^3cYZ?wC4{2?m#;%j&dG23$PjI>h69R0m055$w1qD7H4?d1G6X);e4QIG0CRn4n z4oi^`tPZDW4h>>Xnt%NDBIQbe0SSEaMqj@W-^Fm+(HrX(1f`{{@kwNgF|@jx7yfuf2li@|ImA}|POpc{d6AXWul24?`S<^`@5Jg_kq=MScnLP@(+%E5_J@EJJZlVpAwTdy#* zH8->}HboHT`F*O<0X6c#1?Q!MG9Z0Y^Ll1mpnd1}Z`UK42~dnU{9)11v=bAzq>26!5Bq5gmES z8{!Zd1G#y*BjYcG8^~x0S&rHb6 zxbX;)0EW6>N5DDuUqcpISWyED5=bW@4OgheHT!iAurP=^}EH z!aMa_mLRIz!M8~Po$4)P>t!~ix4;r3xWR=YuHIlg6^J#$dzttJ%OMP)I>gM-(%jP2 z8k|jo6#l_`f%t&UUaoka-zPE1yD*2M9!`@km?d7JG>H^dW<;AvZ98Sc9syD%Yw!b0 z8jexw>@c5#-RT5YXKVz2m#og-n;>awP}KbNzG4{Nh!m}z>iFl^B(hnE!G6bOhnp47=-8`v@^sSn;9%@1KUmXaUB&!p5{ z>DOM+Cg+pD=fS-8S8_gPvBv+9?3{U>nM=>9>`CiY|3uv4fW5@30{$B{KPT9sm>({Y zVPt*~y1sza`QO`oB0-->&?ge~i3A-fLs14p6A3yP@sd|sP9*3P3Hrp6XOOr2f4by( zB0-->(8=>lX!-R-g8pw4^gpCA|3QXMl^oILQdMQQIH)u9-t<)D#xCj%eW=PJ9%krs zXG`T#GISO9|6+y?m*|h?ad3STC67avP>`otX_p)Kfo9M=ztU!yCBu7+;F@Gog$TS2 z#^+;i9Me=)c2U>Va8+2;O%pCQ^3TavqDdvIcEKqjx@#mqNrLzI&SP2SW#-mdx$Z03)@)LFW z|Nraq9(X@+e_cQrY2JS9r2uNH$n`h6eh^%Mq^k~08oGmtX|Un|2c}z*$}zYx5g!l) zR!s(lf#NGX8Tm?*z}k+^hK`;I2^W-!NwrWlaM_MdD}o8X0HhWVCtm#m;4NdM=HTu; zaO)Q+-Uq^(FOUTkmhf(}xKJ>ILVIR-vz|Fv0stpp7bB&7s$=0KEO=igbTKw;F?P#h zFk4Ih05D4p<_sLM@Qh<#UWNYhYUJewqZ6~rYU`+!8LvPxw(EdKhICdy3I(tuXu4}L zGT#ecb|b%!Omic19#qDb!-75poe4V;X~44_tTn)cTWsN+?XOPxTM7bEDDDXEhlGAc z%5^E?L0zD8LXm0IrF0B7bo8&V(Z9C*9r1sej-tb;yCO`ZvL~(Za1XBWAln_T903;> zWf=szMwE9$co?&MF#t8PT5GHy(bCY4beAkFG@}g{Xh$~k-)?bi1Z85lq7BYHczR#r zU;}^q;QTRHcr$;){LE;r= zR}fc}#@zK>Hy#mqK44b$5AO8q_CAT*#A-78yE5@YtR`42+8;rz!I%bzl<6M0_u0PM z27V;kb|h&tbNU_r$%3pO-9x7z^}5|tKr^&;yZ6Al-J+dM9e%Z2pAoh+4X>LevPj#C z;~3BWw)eO1eMl3D+tzBrf5Y$0$B{!#!@K=CvYmxy6B)1D)%E$voX|@v*pPL*d+CIZ z2Bx5_+l8Hix^8!gW#{c4ux|GYhpmlUXK$DP*~$Jhv09E1i*xSo_{U#Jm1{4zWRdf! z-4DuU^-Xf*iR;id!hGdnd*tzaTb_!%(e@sm)M=D;yRAYF4hP@=TDPk!;lrE?4L8{l zyC>4AK)r7FIsxc|vTk?T`7<{~Pn!0Lv@|$imyc){iLQH>#~n2o;kh`1usl{Lh;vFt zR7)ZsY2B{jAJ^?3sCA(c8U`M6Qu8ZH!HZ#SMmrmnmxZP|Y>L`^-j#MXn65!Dg@?Dj zs9}SlkUCxCowH~+OxOB%tRJ0FP2c`sOxI{cg_kI*s8TO)A`_}fblvgW300a?qfP)q zt#MDu6M!_$IT=YJw&Ed@<5G-vM)um69QTdWiCAc~#x2RCIj+usGsmS7l`6-jb%*;d zj}U0VTM3}1Po?ump_Sy?(g2}FkawDv!5CUs546s9IbEN3nA`67-M|TG0ZXE86e!bl z45~Lj1s$;}pOY0rE1AxlsiAeemNB%r9K#;@h2bB2IxLas#2WDrT@Tx^@KfOKy6SmW z8xCtD;iJ9TBu{ubb3(g# zJVcH(ph++B4ywD%{6)+q2X?69c^N*>Q4Kl!oMG25jW9HwgS+? z3^8%D$qbEeG(w|73tkL-X?@H>3pO{iU`18|EeC4A{pL+wW1tnb)_xa^zmDh9GblC2 zD{+4nT2M5DiZ=+f4$^f8^U&f1N{-3V~%ZNQdQQB#u(3=KXq|#V~bp;=}xgLN_sH%MwQF^ZIrgNkvIbPzYEObwi~P z>c9tf%&eQ!IH6~<(p;yk;FU2h_Y5>Yc&iY}v<}gEGc~j>#V~}{^Ipd-rq-Kq#Z5vQ zJEO&11bAeo+l)+V+WO+9_i>hR;sY_kwc_U|Rme!(!R(sO-W5M>5tb#1cI;PC>Dmjqo_R>hEl+!*pYYGl0Bl{t)YBdRa94}0?$_6@AJQMptZozGkjd^+ae6GD zM}pS=`(y8rR4rc0oY1lRFbK3#>4c63y-0x;>=aaJEw$`?+zFudq-x6inJGIyABZgJ zIXeH#$&!6u4{P~@#q2qkl@|7A&vYwXHhEH~cF80^L+he#%lY(@q|9y7Oo~e6`Ln(= zg4Q>z5OXSED&!HnAEr}*8d`7ifj%hEGCL1tp186+@s!FWqz_2%cGW9}AIA-BCBqw{9!p#HiRJ*Yo4(_4Ki zyqR+`Z~Z_1Tbu`9h;30VT#&FyK#DyvLGWsUNY%Ec4*m?{*(A}!_c49!+|f_4cztpv zaz8Hh?Felq;;6rx#+w)l9kzo53F^n`gpLLlp#=2@;tYbix#jE1R)@1n`OgG-#eKTo zxVy__(0YSixg8wp}?pO}ZMei;HU>nhuq5|A_FMcHY_kse$iq zRwaE(Q178DpPgUf$(#zDc3iBYK-@Gs6=40m48v|&gCVrGR?wUaHGsiBJkM)Jx`CRe zIcjKz&2^7hA_;;W_$avl=Cb~n+XSpXpWOk6#$U{m&QL-#CVIR=GsBJ+)=5NSZ91Km z{$;F%S|{`#Mq(|+3gS}_j)}FDr5Mz)7W;Q&Efm#p^-*SHEo^=%mcJ;TOOOs{`kn(yjBC-x!9wP6H_t3lxQ3Cv9<(sjQTNIWFVJ8 z=griy_Dc+UtbJmpw`G55bLgDTr_ahtE6-+Zj%^4l6D`$w{WHew{nn^CvqlaC57=}p z-xFHEh%IW zYyYu)UHu)z+B-Kt_v#o8J`J_tBZx}uAkHBO6STS$yrMaH{A#S*_dL*hvUIRYhIf~M z*@_W~hkf~LS*9!SY|<4x)17;p5^G1NDg;Pfv5q61ITd6#Z$#{Vnob3>-Io}EM~rmA zbD|8OjWf?46ir)Nx4md&^>sV-h1uzDjyhXTb}pGOZx>}(uV-5|ay}fJ4yP(W)#v^n z2hhEJI3%y8UkmE z^NY;#X9~{U`0;XnRBwI(M4h^x9hs^)OXtnhRi95TjH^DCj^PhW&FvmF21^%qN*K)u zNc<|@TW^-W8*7KHdAEwSzeV8PJN0Y{qAi%O_4otf{|xZ6$vGP;0;6j9U&?XLVH`@So_G zWtYh3m(dPSe?vULHlm+6?X*tzi-Gjg=4<*RL}CgNBlNOL7-zTViz7TY?Y7OcjwX~c z?4n$GEkuPZbQMgT!4e4AMW{6(3(KpG%>ZFp8B@O$&yIb{v#i~;`gWE4Bhi-Q&1Tc~ z9%0|mAuo94NihqT5c_+(HixKZ(NV$SBV3Ex;`8#gqi>$zvI=B`usLkL%&DNTS_?tg zIXV@n5LV2R4(x-1F#Sxao1$svd);T%eKOfk^zbe^KFIQ3fHSYCV+QuXgOQT?KUD2n zbJjzIbu)pm*1b&BfS~K7a=Rc1a{zL|{U!U?j6s<3nOk8n(I}TXmq|ew`?w_?Tcxvx^pHUEo8Yy9On^)CHq@>t*s@}UXk&-e@piX1v zk6RjpS_6Wj20FDOw1vYr#E2$MqqI#|s>r+xX*<*7c@`h|jlOAo6UqDjtvRnA-|F${ zVeJHM(anTGr7dcFdf*z;1y2$O6>>qj1{!izRZv5MtS zCWt*>l~c4i@nl}~kYwVqw5$5d>ki(Te-XR)OH{+tk-_R?#(8XQ;d*QOP7s?4t9hp; zh$mr>dPa*Qv+Z!=!VDtppvLbImR$Gl&%qPwS1{cdiT@Yr^os`8BO`1TrjQZBlC#gI ze>~E5xZwJ+oV#2%62H8OK7C-Hj7C9V;VU-lcf7LC>jhs$&WP?a=AUBl_Tm1lz+$%N#>3Se>=@%E;k*^qr4qztIfN%HY|Gbt93gI=J+gXU zf6X>}gqe}EFxzA6nd^n6a()QHF4L(%jW9t2&?Qs|Qzbv>QTMD=kLdA#LR#b+ze&ID z`(an=wEaN*9HD_*{7nVezAap9&53y<>`cImbis&jHlq<54noHaM4{cAY*!)(TLBut z{dp&B#vrU@@$n51VF&QC=P3wV8Z%x4QB;IM(egH|VisYr`33$-9I`;GY`X4X9$~0V zf~P)OFUkp*%~6bYC3F@i{jxbqX^r|WrF=k1sxNesEuS{SLnQ%8exMOj1iXS81c|g(c)pF5ch$T3uC1A>^VAD^o?ST zb3=QZa9-Dt-hMl&V?5KING>)=s*cYTm^GAdtl<1Ei7lzUHM%#)x(0JRGVtPbeP`=y zvnW{@Kv>xy5cUBZ#+-iqt3#0Bd6iB-a`1#%*eU@Rh%goc^cgJppY(L%4MK00=-u0N z;~SsY+z*x(uf@(AMY|1Lb#UP?@|+U;L@~ZVA}%uBXMm7u`#suXQD`Qvc477QNpQg! zd^+*%xI6X@w9_(#=oW z3=}F{I^yrnJa%N3`EHZcDz-)XB{!?}R}RN3{wzN&-`Vl1n|1aS?&GdcLx_urF(ds` z0^4bz1)ff92ETs@Q0?l0-L{6vO}P@61)lbU8h)%ftp9B3l2i=ukVNYSo&A}o6l&;)T5Dm5D2Wzo=!eZUzw;xZe=6B%j^dL?LqB$X;ATBeFsrgP z{moj&p}%WT^-}bXHvRi?7q5uTE1L-Yz?5hk1#L9W&k0(hdUMPr0c5m6@n-gI{bMp< zw9&@+P2{0^^DE#(F5+wmrEb!BGZm#CPc~vqsoRc`53LuNJ#;VU9G)jf)J3s;?| zs4;D#b>QCD-`tBc)un`{D;Vlm6uAze)Ga#ws3`@X-;PG9iY0mcY!Z2^iLfoa@H}TR76k`b1GcOU56|t zxJ#$Pf4Z0e`sj=My(p`pP+0<-cNX*`>%3rc3-=T1#;!Kb@4mPivZO!V<2r>UL*us2 zi=lTeopK#v$vrwN{Y#cWt-asdp|AusWWnaIg4Q8LJBrcfO|oJtWXb&9q8$`<<_uS) zXg7`AzsJ~|eiiNZcTX+hFu-8CPqU7I%Y(sYaN&{d^RvX`^^JZ$6h}1G>Nv30ziTo_XqJ|s&_)ZmZ%_oNhr5+W}5m5Eoxy>f? zfNJ=bb3JM^W`;_w#~!@EzTlPsms7H~b95Q2>d=g(&aNH>O{caB;RlmET7KT8%6MMO z{+=alqDTK6)1ZDDZS#?^oI|HyG_W3}XeXb+plG)_+3!~ByV)-@CDuE~e&%{BAYEpB z7BkXGjIOQA9+{TVr*lAZ)fIwj4CZ>z0_(R2Sy(q4_dVx2f|+{=7VSd8ypvUx@E5z6 zt|&!2qqb(|RQMq!iKy@ZT?G?ojKE{qMW|%4w%o^Gy+Ud#VR_`?i0#}Z`wY8q4PpxM z-b(+NWL^!ZlVNFE^ew3RmOU${9(Vc2&iLxer8$M0erQjb@8z6Q9B!`Ld0d;GEav3= zXP{M|ITf;%B@v?M(y2fu`VyG`7+^t{P!;Vc-_D|XTKb^qQOo`pou6wnQv}vT+s)sf zr(rPk!1af2R0<2>raMQthPclRViZ_9QIQuZOaSNk&GxYJ5RK5NOoFbnmX*Y$Rp4ZB z|D8|!n6<|#!9+*MB!kG0cSe~M`&Wy0P_%5eeiYg=yyz1xdl7`?(RBy&2;&4c88eKA zl4uWVB21zfP0L?-43p3%lE&}0P1MW{iaJeM3x&3*nHy~G8^07m*mSZxDDE;IMOgme zjw%~~Fha-Cun&w8_Bi67#Kafv`XQ|E3YM+QnLrqW%anP9y#XB)WIY8*hw|yXnHpgW zV;CbW&5>Z-E~s175Y$_mtQoUp^K5UWS-4~iv5t2It@quH=CrN(bZPihhx9z>)LR3s z^52D)YIxER#Qy?1{i1>OC1Sn^>OGX&+YK(jrs8TxE-Xi_Xo%4mm3< zJIywJZBDG%LtHJ)liadU-9O@?eu-08z525}KMKMc0K(epP!RU=Q#^AjT#;Ces8EQm z0)jBuMW_g~u?!IV3=rn88@R&^fAzJSMVV@5QM|tJCJ7hEhf@tGLRYEFytAQGDztvrKOhxbV?HBftcn=2a z9#DvPW!#9z@S+n7gK8R&>xt=tI09?^L-HNv#S86zykkx*31qFLmw(x&mnz{rm6ESF&`C#N@0 zs8r2PeXYHDE_@Fq>Xn_P2MEXJuv|^$l;*0wj9qk+@L7*#Rv*um>NnBb?^M?In(f>- zTG1mROzU?D!^+mu#39tH73w#&*c6HX#dP{b1Jh9uW>w1oVWEQ7#rrDe_@2XOG<;>d zmX6OE2_A~-Os;X-Av$by_%e@upVZE5S&s1QUBtm%#47oIGq!Z65vBXR{SaYcaP=QR z*jhfcEG&8|6Gcg=Lj2SvhzgI-vfksF7V$n&v>Bxr>$~>*P4$Z z1@c2!PPMD8$(S=q+|IA$*z^+c16BUYlCM4&6TQoa>r;q63T)-W2e1Duoy-VfzTJw< z^#Wnp5(HsS=v1Ian3FT;5)_2VXJqz>9<~y25as_`GRw14BE7NRa4>-D*~ac#n^|iL zE*9}!Y0_yAB__fw?7<(iFzlNjG(w{y4BGv{Io}EBR6(7nC}L9T{^Wx`8wO@=&{e+MZZd(fP;qDe(`S@ju%Bw;V^~ z{G7lA7%^X8I496DK%QEMT+t>oto8JOaxsuwDLgF(zZ>C0=c_Cwj zZFh_=y4n%>YiV_qa=x zCpZPQ5`!uy#XXcXg@wc&B!uDY<`taQ`gz@Pt2i0$k2X+Ldn+=Rj=HXUM1 zSLxkvg*pk7HmspX7-b#B!dTduKv*8vB^seonFKEl{#~<>v}!Go z3+`Vjo;D_}G7sNx0hx3pRqi2$Ni_-M#UxbPLeZ3-&q7HU8D11csgVf6Uea~PzeE_6 z#3SZ93c@&n3CBcA*gQ|52#J&w8}Es`#8jko9Jfe`N?R!EU9%#Dwlm1?aJl`#C~XZ= z9$2`t1KJ*&RW*k(ZEqe)-OAF{u74~>+n4X#r3u;sQ=)B@(FDzPit5efpkpHYx)Fqx z(s?sA!dAI3Mp%?%jInRCVj=!*CGRF1mraqm5mD3qI_j!V6kWPk+?2pJDPbB{C$45e z5B4+%QTOYRBSvnke)*fIIO4Z%ziT2Vb0^2Hv*OvLAhRWmx{xS)5M*IrY|s>huiXk; z!<>F8s&kPP<`tcO(cmN$g!zgyK-d;R`Io!Pj`Q6g51oHY|l}@En%kkNEc_w|K`V&vO{;VH@{V&wQFKkG{9(?HJU^DG=}aBL8zg;pjljneN_B?aDoOm_^uw*;&ll zeM^=ovNWWOPUzn(4MD{ryv!a(VGtay13JR}CwhZpQnbK9gJlqh{kQ!eQE;g6cX0?s zJA6umSscRV(`u>^9KNNq67x9Z1U4DNH+bsh&qxoVqZrM9YXnn#8^0U0Q0u2q)KjSk zQOJ#oL)hHy+d%|}Gs*7oJ|k%qhYCX*J}+fAz+ife_Np+(VUWZYQI4i|{T1i36rb~&D1#b0%4BP{ak*IF-2DIWef2)o`&L9#l-Gjo_eZD z&RyY|mEh2-dqv%u!xjVkX0rM(5+h_?isxKCBdofD*YJW0=f~~U4W)67kv9$vT-h|D zzP)$FT@!EE@DkW`Juy9d*_z%}E86EgQp6bU#`JV}cjt*+#pr}$uxZpdltr6Y z{UiDmGo#`LwC1B(7h? z>cDZek$hrLGJoyE2iAH${kO|ANgTu2BH{4cypFh*N@zELyJzKTGf%vq{U7(m#&1D7IdXKM%*A0G9wk&Pyg7o>TcSF z9V~3myI8G@^XQ6_0AY;3wc>6x8%-Pn5q#{Z5bD?HWP!y0k97J)g99iC+vCCrVafaz z39pY|zS?d#Y`S5oZSsP0_4Df0Pfsc};BT;9R^&T-aX~>+f_u!ou%4^g6}O1ZT1GwJ zCZ`F=Uf};D!c(Uixy7WKj7-+oIBSxN4xA*&~ zwAr7{srW^-7VDM~v6P3mh+=E?_w6KdzE#`W@g1x4wCl&JIjX0UK3&RJ*~IeCukWw` z2)haI?jkP@@inbsP6f3L3j|@G=~SRb*zp?BB`64!Ov@@4O<6v4@MnL^N72sLNyfR! z`a23o*tYkrN)#R3b~1rQU%SiZFtZ2?D85c3G%6RM>wN2v#CjYl5Lor5&ojxt?p^%SL;;S%jws?Z#$gJ``AEoqu~m{ zxb2*iUOCF{sL3eYzT)-Q(#(V1{nS|)xFz{_2)i-JM-zuo5eD^JIaLlpSPh+i(ZFC7 zgdGP*qdpiaJej{L_SFf#yHod>JlGlWFkEn9m&5b5>dEW!w|Yc(mv9xxf4ePKy^9#h zHCcSl;?Qh-x?aiBUJm7t#)$x7hvB^1Pf(OZ&BAs!eP>REUQ3b+U(i)R5C*#l6=5qZ zl~%q02vaX!u2^`Tn=k3nOew{bgs9nFn(Tz@pF9eN!<(knV%G9Rmk)DK&zfQD@4H1$ z{Ayp$ObIoGef>F~`G2&eQ5Pkt5w@^)h&dI$tdTHNq|tK$oB(OyWdVk5r0n zW6=u}-z4W^fzLjn`Z;asKT>owb+FEa9I+cc%^rET4-?biyxO0$uuR)4G(w{?3A&Ey z!Ziq!oPk_$zv|11F-&@=wsi$$lI{I_xfCYNjQp$fYEZOKTi2n`mf=NV$WKC89bI=Y zk1*6M3`(MQ>@c%jfX$n?UqHA3zqS|dKPf*uTAk~V-yU4fZh*mT%I-hGm<#IXPem>w zw(A>7_P&gh-jhd(f8dP&(e}UPI2z~Y1ST3Y6oDP#UwsuBia3J?a6eb*z?h+kZu;YG z(D|vChIo$KgeR&uzXzhFa#SG*tEcm3YJ{cKGDeuSW1P{;yJGoYr)S+=9u`y3!@FT# z-GyTHogpI%iVbT(5%G$m=j2{zc~;CF%>mb=O>Jo}=BF((4gXjc)UAOgvQy>Q_}e!g3IF!&f@}qQPqvgk8*FfUt<9_J){JQL8&@ z*j*gwmDSvPFBhxY^9}=$8&%%>8mFs*B(}!?((&N(vLM& zy-ySc#g*g3crinjjdi@%skd@wnRd>*-zV1InN8dpJ|dZW5;f2D4>2W)kD4gDm;zXW09tO@B{= z&f~L1uO?^hnPXhx-gC3_>)&QdD#jIl zV9nxKrhZ5}HF&$_xPXWTjj&GaU5WQtl56kEkS=|eKzq(uKdD&)kTaDHXO~w z0!`>;ft&P^EUbl21!{zq3&iRpOG8@eg#OLa5Y(b1JXa)tF0$Ak zL(}D`^n}gLtaFjY{w}~IaNp+HoiWpJ4mY-~f%KF&9J)rKXQI-GUh^9jBIQ=)CCd_x_lKpzs#--9wvLG|X3 zpn2Z5Z%9VhPUp?kIJ_;+7>DYP@rAx8XXLxxIGh~4udt`Xq$Xza(W?^9kw;s%oxlcu z!MI>P%yQ>elFxtG9x`&WdUfjDIU^N626rYm-LE{Sp0V7$u{F_p5=G zlC8hNVIb!xn!v(PzjHU#5tek&=@$(wO~K*wS_U|bN-Av&e<9jd9JOrjqqbbh@K1}a z9Pht*x<=1_Xphc|nLosJWQW_oekP{aPTr~_`>Fb4SD|fgzr9mEK_1}n2rNp1jBd?K zG#sku4Kk-f^>>mAo#-kcID}nuwOxPbHs6_AN?IN zvzkdb{Bu$Ao5?*Ip;4IxFAZhfdxS}@KrXn?CHQ&_lPE<=>Zc<5`;HfrP-zQAyL93` z%B^(_FAA<1*$BdZ&~?YZL>QFh&GH=x!YD@5`WQO~VOcdDF%ab8TAuk7kaLe4!cZe6 z6t(rjF_cJ&N<7$H^3HLDc%o!?u+Ta*N<3`Ef-Mptqa*NkAKu59c#;yv*qKk;^kuYi zRq8jto**7DCEA8cJXCM~1Ug3ONhN}?ZaQzKMp$tnV}!{&Za1<@7R;*;N*SJ)UL>en zJNTeFT`J2W?7O{0EvLb*Zj+`H81W9}y7cD-BQI7C987$q>U{A%YpufEdsz{x>9Lat zXP5P;dz2ld$>?OkQ$vCnXLRx3nOIi`_1nP9k07jvPQPeiIts!*#xOt_L7?>KmJ%_m zf>T8v>xGq1c)Vtv$sS^3X3C=M@{h6b#VJeu+BMsCkL&z2$8_4T-Ar3GKX(zH{iVTF zkLo`nY*`=^NhwrelP*bxpXe$e2!maOim=s|Q=^Ii!nlI=u&-WOvh47(*{K?Cb&9Wb z)3mMiyu zy}6#;S9bNJFCZc8&$nDZ>@TMg8kGys?gAW7QMiCgJlK3d|0zN|H=qXGSJf^WL%fVj z?Vd1rC&=m^p#*OQxA7t#!|qtN`v8KlKDzE;9$~0lfLi-_#iDS56W9bAj^^RXu=$5; zr1kt1qouG$jUgUo`i7c#8skPhRD?lM2Ok_p5GF?M-{npRMiGW7wSJt>WdNq2{%Ol# zT$G%r_io7%p6_{)`%GfB=X}wpglcfc|7iQ)avaTdit5duLC35de2gHhpU#`95mw>C z7-7SgSkrnuw6 zLT?j%+(%+wZPDVKdaq`=ieZFL{6KZ{+|pCBcd4^5-rpna{4P!A^h;%1fFNv;PQPg2 zGXP;o{D=CX#{V@IU5N!NB|C{u$%FHaZn|yUGL$^9NNaD@oG?xH&Kb4V16HrF_Lx0y zhUHfDY-SrgQc6tIX)65j*7~tUl*F=ddF@WsC`$bQszSRoP`GX_b1FouETv$HR0Ssy%ST0y`C2>eou=3 ze}3?xApSm$(5Uer`c)>20?JGeYW#=I&w%Fedphvo2k!4FTR3J8t62JS5RCtpo38Am z#D5jsUyc8WXqD@c_&-e79se@^!&8fttzY*L=v^w0{y z2Xz+x(p^ir?g+-tbKb?`)&EBG-LdH!YP{!?)OY58ST*ExvQwBKbTZxX8B1~v*Zn?8 zjoc8~Ve6Q=Ts8q^R8_c8JC1{Tp92}+uN;1riAQcg{dQerLE=A=PQPeiI!X#NP{<&K zi4n*i33{5=R@z!U6uD9F`0#Vtw`<=1(>}$jd$@#nA!+33VCR>pR^Pmv$&+g?$_Fag zwLkbw94wvDwh`W-1*&ki)?UE7?ANh#P1MS`pd!dU23phDQ=$p%0- z6ogGXvQSlO|H=(t%SOVgThfwuM`XASuTtqTOwxN({e4QF=wO$miNJ#+B!ubzaZ)Pr zBy9?V$|NYX2QTT78}2|ZxX-a&U<{Ke-x(><#hOeBuWy~ki%F=og`%1H%tfIsH1?Rp z0XEMjR3ZptrR$D=i7ql^)DW$}s4S8ST{j%#mxar8y~1j2wR(KhB0Rt-Algu)22 zXp5cBo5{h&7^^CQpQ>i}OYrkQ`?tHD9t!6j) z?#>BmznPh9Jnx9jz@qz2mz;T?5EfqD=C_pZyr(7?{6Z?H8V(iBJ-6Rh(SFs(P^!zH zgE|!Y+#+p+OB{4UQwQ~)3kMnZ5l$Vy&KiL}s(cCtG=RbTtSSs03c=5trgPC%~H-;B;gbNFKc&z1gvhqpp{YjP`8%raa?{j4Q{U zPDEWsml0~z^pG~A10$#uQqmHnm4b&pDb=X(XQq0HWp#e5e|o>N}H*2C2| z+t^V%G5yy!Pyhh1kh|6O&{OS}J4ZBi=N|c}rB;pq z@a$&o%*yRA^Uiv*w;h^xr}EZ?$PbExlJmYktUkCYj|4!w`*ErE4YUB%nEUe2ffx#1 zBS=~;IGIjpDgYv#f|}K=w|J9%7XYwQ+~$i5&y}k#v&0nioec$cMNcY}*fDqNl0Mft zd}pti&DCKU>3HZZs;RJZmLcXXyNbVnS(2GcjH4#f z%&8D4qr(~s4w^!zf*W{^{Edu{7ZSkE*l%Q1bG7vnN|s=I{8Q&VbsEn`acI_@m+wHba; z7vwxwutOp{W)8a&`|1G8+RNcDR&yMRdeht8-oQHRj&6Sb4@E*FC+pxHzFwA(yS5*n z*6^zGWFm`)62>~Q?`vbNRX>GN=D{iDst0p_w>&&ae;Q33VyNFqt5s|;TJqEBM~;?A znzkSVQVK(W*2jYX1Dm~`EcJ9W=fX6+eC^Cyb9RX6lTt4Kw41B0&zAe7>5KRh$&n|X zE{EssXeMeI4iTb=QN%tDy9e&7v?aIa5pS83((?E!Hh3SF0G*W7Sc1KPiX|tDH@9yC zES2)fez-r_|F+u=Go58->N)MG`m~SQb-gXU`Ds-OPI1N~=1=?Ut`o5xymw=qDkohz zIy|gDA{1G@d9ds@f~9@}2Ighd_u)SKRLqwZ0IEh;Jd!Guuw-9`Y#HW z!vQRv$m1V2wDH#-!Ti*aVR=!rV5yEssc@5qD z{e9BokG#N}aDT_%v5!CMIJ;&u{fy6naT`CP5)_IiwD=4PL7{a~HA@bEfM97Vo&A}| z5^6|+k~ljhA|Zu^#uHG{0GlgbPeaf!pKRll>La6Qcs2d>JAZ%%!k#7Z-i*))r9mMJkJtnnfF)5kR5Wmcrl>A*UHlX>yr4M$+wO!h$rEMfj{3&i8{;;-K=tNY z;6sC31&~4RbUJT#1G<4hE)upijZnk(DlQiH%dg@K>Pj!8XWjDmhtZ>|H)vA4b>*~CmbZ>G*B zRKyu)6V;Bp4f!;FJaC=9wC|b_Ti=7gfiJSG>r(H|5>-0$AV_n5T91Acf0=;Mx|1AP zBm1iRFJPQ)aR~=Gm&*w>zccU7TYo)#@FBKICdTnF9MQs>7tAI=&FgndlP|pUqlrTd z^;4ZgS`<5rPCx3J*V0<_nwN;7!Kw`5)`3sGA2!>cx@)6*Z*~}m_;*}+Q9sV3zQgcE zcHCT<>78QtVyl{uvBu5#Zoze7NA(c+7L6K?Anmw2E9fSDaLr6Srw(=s+ZxhhWLS)& z-l!=BI|Vhlz*$^-e0}Aa*SEfAm*mw9 zd{{n3GB3YPetO#jvDofxvG?Bk8%r@-0WA7yq~u~OrDT+tcv>t}p>`|D?z8Dspq}(u zz&)4Eig6NP`ddL1zHAP ze-&C#w0*}5kX&O9U3V}KE!2PmPrXI*B}&$V$~f5ENTd|08&ZsBW%hJT-H^gK>IW(P zWyT>!>#k>qUsVO22k!6QR?QB5#|P(+NgDB7sSFs0SsQUYk1^x89+!uI1B`q7cK^$t z_7jW))NZR6@k`VI&Y?imT@6tXjMAy&Tms_TD=AI!lUv!uF zAp@3mWoD;C{Z1O;@TM=P^-A~ry!p!H`4@htxD9iJxIHJ$B|6CtUr6WZHxN}OtYK%f z<(}T2U>gieU}R{e+Wi4qe4m+s7SylkoHR0HE=s2#HMAUK(4i%2Xb`q@$%E^kHt}Su^66I_wDlzpB~Vg$(qo|9W6Ert<)8C6E(aQ zOhkMrbWE)@l3Iz;2~7R?r|!@rC=4j}K9-Ufzn#+*|D!jP5wr?M5^3T;6cE*~ z-VaG5lm7GQRNw&zlwsH{YcPb?)(V<)!SkS=%V6zS1PUBP zZ}KP{K8+h3P_YL^d-($`S7T@;_g$pN3y9NM>0h!0N}?A;x;31Mx)p5haUc~*LZrzy zzG%L0Gzqb>*|2XR4A3IjnCFZGbdnK=y!g+?JejLm-se@nPAs(orbOFN12ih+VaK%f z97hsait~R9BaBI8(dt%~<5sjq_2#cY)O#zQAS{uf^JZ$6*tsxf$z#Vo`7g2Gb6jp{ zzI~%(V%k@}>2&$4r7ek#1!A8~9Rs!|aPSYzJ??B~GjEpfkWqApSLW4`0@e2%oT~8S~4E2k8+`ux{9GnAO&Kl9od5cEXU{l*SgNmw6+J#8B{ffmaM4FEG2ampK)lZ6z&imPA*< z#2F*-1a=WBSzImlRbN_p=IV)@llj}8>#d8}#CC*xdwUG?mnq+%&EW=OnT<2 z6rBpx=}>4oF#p(eD3*9|(Lt#_YmZ1NHF2vvxVpyXlhhhxy}T)X0vv7W_nyaGzrc2k zU13)rsEsm}4qe~>nnq|;F2GCU;HP)Y*6?8SJ|)uE;rakI;Qr2xGKR#XuHmJ6|J8H| zik9hj0ZE6X>AK@zA`G57kHs0K{(@RZgUz$PZ$c0zLpEAX%dd57Nw+3dFN1ZolaG)8 z24P*vhghfAHRc@&&KD82m_Qh?B2y|G$f{1|X^z zOBM1!%>{JcOdaAw3K@j>9U#Oz=V$nSkqq#0=G)mQyy|no{Sw`u{#DXLvV|fDy$z@+Q%l|RNXRpA~gc>N|n9(v+Ve)Fy_aiMtR{;s} zuHZe?5btKOukiv1@rSb3=R_2m?MN)Hk9g*`{M|((d3N8s8-7gh-dWe;mX<42aBCUY z!OXhzVrkWe#;-UGEXA`rl8SPd2=)8=(S~@3UUTv{GlVQOLeO=@8CiqNG<@x~XZ0rx#|dSi+=i(H$Kq-Jx+ zx&xGomyOR~4e?Mk?pP-z#4o1n4(3BVCnIMykjp|s1H}sKw`Gh84IW~_aWJ)`o|RoX zZlM9SI0vn8sXdEX8o}oEQaK2X7Lfaw?Zx%cgvvQyz8=yDv*Cwh9pi+G?`WvoKvQGh zvr7*fBTOb}1T2ZVq0$KY>X`W02s-Am3u)1pES)z~XDI<0jI)&6j)?}o+;#VBRphR@ zc$gf&VcZltQlqlgz1S^c>agjqN`IxARo@BemiG+hh5P&D8x1_!=NZ&Bu;p(3s?mN_ zq1yIi{K}M0X}=-|&0kqcD5!XW+bKAd<2+309==7sT*mf5N2-_*3 zAh$Lz<8qBvdhR>x{H@X3l;3(ZKEOJ&OpD<#@HS*QAzdI(;QD}(>*igXr0VLF^8$0n zgWceXfMO{?SOPgqS<6eisDRXMp}U#LQeYQFH<45TPX?#isOw*_i%=1Uw+QGx2M`v& z|Htjecm2Ua*$`@k9i;klq0$dgG&Fz$qI%ht0VAaDs6eLzHNsMAz|klO8{B_UNNU&m zPX3y#DMwA-*GMc4dz~xN+vnNAJv&POTT!k9?^5{^6-UyS3mN_qVV8GZpb;9CNzm?> z37!a(;8(H3eHmBXF-*!A&x?f7c=8pMgA^uZja%A*N?RzJ<-$M|+A{18K||64F-5xW zU>;$pX$O=1ebEQAvXo=Fpd z9#|4}Lk;w(z=j<&<+>>fuu+F1usP4m<;YNk;`}uqBV=SoR#25jD6)!yHvG0sEi3q~^kaczJMK<3vR0bZHAmV?KqNVTg&y}0 zwu*C3>$W}E6UVziLA?Yc6RWrKW><#T)B3eUP%`<{PQ;H15y0J83IBi&rD26ON zNL|TEd$UbfF%x$PL7@qG%Mc8!&#Af9oQyc!u7?tyPW34tEcTP+5S>OAj);` zwjGsx^V6QH+QcM=n^>`m+lx${L`uW{yzt<1rUs4Bs9=D0@2?j@0R!|aRNr#jIu}76 zy!8hjekD|B4Du**OVkSwlEw{r(AcP=p1?du(l9l;?qHr_sId=f{Y*+4`O;R371B+_ zIVS-*TyTMSCvPubxWWJ}4I94-4Qf<}qDp*`M!t@giFqy9+%ZKB`Ht5`CD;CjVVM>4C(prD^B%oC*D!4Q0$c0TgULNPA*SqX|A)OZkE`kX{{L;BC!$g+ zm6D-CrFlddiwtQ-GHa61JP^_(b7??@Mk+(oZ4eDoNkvH`il{V?_jmTG+xvF!zMSvx z^WZP%pZEK`ZRfpSYpuJUXYI4kK0|(aqR-b|k8O`N;`l5;kjY{-&{~uqH6ZWvyt1cI ze#v^Pb#wAm%a|T#iP37s<Ga9L-RZz*jR^uq%@N8$-jF z!5vKX>5b!>D8tmxP%!MF^m=SYbN5Vsr(BuLXi|oO7(p4fe}f~_9fV;g4yh-bKBQx2 ze8IGqYt;00e$MlT8$CB$U4PyRs(+pAlIwXjn~qP3@s#Hh;qhZ<`oFE}Wc7HwE^Ej7 zo@CWYh9NJEPBDzQ4bCxVR%{abc;r!rsgOw^W!Mu@Bo84B>%X+rNygubX`J&C4_C=m zn`KYb^*x6*=vwGm4ay5uvq~-vOjLd89R>{RtNDE&7WWau;tBM(AtaA3o=_z-nia8= z%Hjzq57BE4aA4&j7#^(1iSp1M`CozaDm}_~QHj%%q-!mf9{#J-lEAbA?Q)cdYGhu* z&O<0}nl_9MIv;_pi*N5n%E#T5Ar@gtQJKX|mHFiaS#S@;)GHZX)0-kJWIXKq; z1s|zQ%U}I(@dV;X_yL6s^u7iJMgX@C?dgcwtgpP1qNju1*PQ&jJ>6Ht)H>(+Xiuk3 zmd&I*jQ0Ko4`XU1HY}O=7+{(tsMW;3_|4X^+MS6O36VD)()X#c_6ZA0TBz|{(iI!y z>}E~AyLffb8yft|@sVq`gkcf+XJcP&XxH3jS1*@R8%s`NEM%W&+ettQ+Xv9o&983U z;J6D*Pxn|u_xE)f$hERCN9(c%nOssHKGuQraD`^>@Ye1h;qj^tJZZdE%XRlkzM^{? zSTFQOP)Pr99Zro7cXu@%hm!4`yc|B7`VXA1^_8#w$HS-OJ>3D}GHgZ<2b-fkohF&l zq&x&Mg7Wad1|zO$goifU_arV%U|(@#N&EcD@^pja-=kHxueLnT&@+^_B$+!?N2kS- zeNFwB!X6`KUwTa$j*B9lPM!xy>8 zZx6!5-jIZJne$e?KA&!cnsOYwzo$!z!Tq5A@NLJ^V~tFsZd=URKH4dK1fSc(^l0sL z+DxON3Vihx1hJoR9*O^gJ1W{K;>Y0P(jmtnxPwgq$y%MdfQ z$-;r1VbIzKF1?`82(5iYFSHwM##H;l|5)vV;ue^CCt@2`+=AhG0*6rC0>^>#pilqp zY?{J}iIVKTCPzF8J)pP+xk@oZ@eK(?0&OeW z)9H|9GbzI!iv5CN(KQl>RmU0v1Y`NMnC34%vOOhLU|(>{=@m4#-WmOydKzQ(-f;6Z z$>O@k*=B!OQf-$w;eF>P-NEv=>A9jOa5v1yY8(f=lVe9bKJek{`Du$1{y!LY_X!3Z z0J({sA}9`Y$>dJqKo{(SSJ*=3Nn1cr_u8$Q;y=3k-mvYm8TfY0kljo`Fw<^!R>^x8 z?I-6MOGS$Dn#T@~7cZvsh~ntNXCBv*9qnB-TKcAOB9d@z@=05>>G!?Hl#v^6ow;W; zFc2CsVIfge!fLn#bg>AC7?fxSHyDXTB1E&$ihR&`zu)RxK(Tme0vET`n1=xOs>R2m zdW<4R!@EX?vt@4gpZHQ_MOcxYtIS9Cv$+QzSbo?$fbIn?iTTbt1(?Z8MN;{2|66bt-_TY0TLk{3=yRQMl5 z6coO|w7i&R$kIis85M}jLA$caCl83cspEen8n8rO(jO%n(P`N)C_gtueBWulpyc%C z+vol+(SWIk1TJDF8W>)AJQ^jM4B{+sUKJNM-9Q>HVu+3fL|f`3ro*L0^e6d0HKbhw zla_wLOLgw{oWt|^-|BmE2Th%0Ct4>Gh>dRDDADxDiVG>xa`S&dw8$EX_zgo1?DY|| zHGhc9%rgEKBsE>7_G>W`wCL`#do7;R*D|3*>&cS&ZU9pG+<((W@Hi8muIm^izm6fW5y2q0e*1;v9L<13n67A3iBdIXsg&~`l znmu@SSL5<{uk^_F@Pqnw1*T_9iwAfWx|2mpFN>(uXu52ixPMOCSnlz4n#diDNe2}C zn?7vzK9suzL|J7SOaBSIgW1Zf^ClxXY8B#;vAGe~C$(Y^;S zng;)pTEopw zlxQ2s!hxM=(1}BE=}AJc_FP(db;j6UG)8mext`Lur`W%-M@L`b4S6>Pk8TH z6PYh-zx=EF;D-S@pmiVGk%GX)G{~X+rk#=ejL3BsNm_ z)wIs}G@e}C7$y|!KHeZ%9tm4=v z6z2DH1XYEMkr8bhN3D9FCnSU<|7k}YhL4c~x$hY#_dMN1CYO|GpTwvTEhwmB^Twxl z0|(d1q~92%mG!MwdSTz5srU1C;wSB#-8#b#T(6I&i22}d1ajVBR@rWt`Cjv<0_(F6 zi{~03M7xgkXof!&ksb}U8dBXB!%{*ZVXe-jgw1dXDA7Q~phP>oVWsR1glKE|q92II zy)5N6Vy)x5Tfh_Gt=#69l|Ee2&0-~ZDe81NO_4rR!*SNlaw?9xjEmXV<%-*Mhi2e~ z2FHp=sS&MMGknJH0~iQRx)VFpP@)--NgyR!e-!e!Aw>HYv`j(9Z|@0}uOH^w)!>ik zhp*Og&yF7Hwc~87s7T9cA7KqXCsGDJ35$tn!fQ{>Fd9lUG<9?}u+BPy;XXQ==&Yj? zA_JUDuTY|xbrjjfdI=CMr>yb@k!Wpy_j`K4G@E;qt04`^!tp;64P3hJKqg8wxhXIB zYo|{Wt>U(%LOs&S;Jy_46Q~ny`2B@P(u7ug{e~igi=*xTh=#Zlc3>wO2#mgQ7FMEx z;csx+Xs=DoW{I5kl z&Gd;q!R~)gVF#C#a^CKhTvA!8qe29v7)?JpTd@le?RTpoZP#y^QI>!hidqWfN(-^0 zJ=#_>xuitv%cnxL;GmQp>z~|xJ>l5E+OqaZMfW_;0pV?`0&Pd5O&6&>cK`XJle=2h zH7tjwn|`4c_gOnNIo&FafwT4R*K1A@tpXt$e0{%VL=QFz{$H1(5{%#yP@;i|L5X%` z!%EF-#xWs^w4C>|n%-$Do(WomPTd${YrsSZa=cP3K>licj<9`?M1Q_0^;G0!@tZfe73^lD*kbY01I zGNXU7t^}kYGz~=pW}&z{feZj?dDM)R0(a})JqabpUDVz`)xbeP4oowsoCN_n@Bvhr z*otVh-`hdvCG2#BwiUn<|IJ)ji!d0zE0_x{!bBhQw3npq(r*RVh#e z<^J-ibg)yLH_d&~-C-lG2=}#YnJ@5gYat_`hu-y@198Qz_zx;3KWQa9{&64bB^(aQ zeP}j!A%R)jQH0WQCs{U=(y{CIFX(u!X5kW_?mGX`KsNQ(?JI@$bc;vnZF#x+=9tCF zB;lMj-{LeDi>;)$&N{`yp1o~_3r>Vy>i`pD^3WpY#`3H9?afRg7e(Td<+)MiH3h+grUMaTY%U37NU)~1?Xy2G1>(`r9I_sFW$OgE)9STI<(o_p3C*-qqR6pq~j;~YGgljWRDK! z$y8Js8Nw#vjNVuD`(P$y5=iOD+k{*cLdQ=5iRlO(19l565*{^c9AC!svVi8cJEI>z zkG#l^Si;D>zWoPOYtiqgV|jsFg=c1y3dICSo!N#kOh5q$hU4~50?-+e0nS4OIVb?g z%%^r00Ps=5*XbF|wm3lxWc3X*UuW*F)d1K4}2MhYNq8Pa437Fu=KP<0pzI4dFGj zu8}`!IR3|ORH469V48BoHu^loOf$0IDJiQ7I=^6bXwAaIp+DaHXP#wJXFVZ5WOE0% z78g2>S78&rx{^hHUfUw6+}zAOCol5{94k)B8zhasRjpU!@--M|vdc`PV>F_9GQ`Sc zx9C10)12}i3fUTd|zl{Z(gd9dURDuOu0?KL-F(|8#md-W^L0G-uoo8}%l8?Jz z+_E}z$BN|p6K~xz`+2{HaPrBq72Byk$bDEw=W}G4V#mu%RGYaDQk^14Gcg0wg4sCN+g)z-2P^;B#$;^S+@FVgfsc{S-D(GX8U4=B+njv3nN|AwtR?s?ARYh#9Vefx9or+K$0j|XY?U!PI;b+^9Xw(R$H ze-`ta-~V`l+;$6fboy@(nOxGkFMgX!-4Epow$VvjSk5$vn=PoN^umFU=bdpt?$D*36p&pm0! z>(v+@D}pF6MrfeVOmA23?_cpKJxKQP?Fd~)aDQ-}<3Zg}cF#l0VK zl9IGXTl#mO$pO>)RjW~=?IQ~ZcA`NWA8=_)&uYm0CnMq$Xf?X+TaC`_5}lSJ{*t1x zB?6GNrF-?q0LVnmRNM0n=(h6mQ}I=xe?PsgJT7u>DN3}&qo??&6D@qt`>pAPt@xCE z^l|g0{}B!GB=o>ew4X@ISqt`|M6)5wW>TUFd;fxH0W}NbGuz*>sk+ms@nJGzh}QT4WpAvRVQDEMmj}`4;NaNZ}Sw{i27qO z*Wy<9IaPz{mLHe9&gf*2Sw{>+YuJ7I_c#D@d1%E^9PB5POG>nrI#h^unaj>bD^-$< zN!@HN zHkcw>#rFX!MB7(7j7>s<`Epdk0k{N|Xdq%xqS==8?hinS#**|iy)ZNV=aDO2t<7d< zyk@&zdcR5q-~Wca^x{&9ifsOIL#F;Qqpp>aubI7PSu&r_ENV`cj+uzm8mST|674%m zwEv#TEq0W|CZXu;a+GKX$s~{xO*a?GZV1sD{FAoJoY~L4L4+J2Z>Okfz&xp9|$SV&{;SC;o4;UWBR)RJ@ z3R6zY3wS==_@pcvl}kn%ADqMbgkCP>pYUVnwPl)y8`|(5rdfjTvi~hgfqRL{!zt*k z1_%uE(${FOP0Z$e&oqi&oA`i}^j709ubb>eO!ZIJq|HNAA12FYQliO-{eo!zH4DF$ zey+VR>NKuO_@NSO-+E0-M&~A8_=(lA)S|rWE91L;g^W)od2%>$hPNpXi(I-hq3FUg zI(uwcqsJ!=zWF_#LB&gh64uk`F6j9v{-TuE(zS#D4q8 zU7;hS&G(a=;hq+=NS-%-b#PC^-WPWw5u)7(@3v7T+MF$pzsCWPz<9zQm2ea;0VNuU z7?fysCB3%(#xZuoH#MJG=HeAScqg+Y}P!#4ho2%ra<#mJpdpP-(4L z-nf<4$88zs!ncYQa?Msg+f+H?j7W(_x{Kt%(ht}qtiU;-L_0<%fs|-QR!9sXM617$ z&K2q2`$jxPlrSB1R`0CbG?r;8x2 z!R-6HbOfd)HMOAqo*kK&{zp23ORw|`$4W;qJpJk|l#Ys1PNS*5Hcdynr$TQo13K=u zU06Y#j-M_X_nfV4!}HXrCe#d!`7j zpun_X{V~Xl8P!0>;OCeC+Ny`1L-qg4sWOzd5p=2_PYpbyDdgY?yi+NuxX4Bdo z9O+|L~kjYCeft?`!dgijwsnEqUgNL@xxfQtp?*-j|544g5WSfZ|(2 zlM)=^5>W1gT!eDpzNFXLZ`VS*o-+yAyGocHcrGn}yKk>5r^ltu&mVf)(D1Uk$-at= z)TGnya$!C#v3!|ab@y1iMemy^iR`8ub3Ix1;asXnxlj6G>UEFYut`|3P#AqCcbrTD zDfjn+3IpN(`wKGZGCl|QXO=yQk58i6*`;TEbF5RaH*s&RDOa3IesapVhGu@K30CfZ z-0XtQXxiSzXusz~W;Ax9LFf5ENNGNi5(|v;~oSQVlPkq_)FvTeT=HXl zgb1Tz0{<%H&2N&o3c$L_-xKZB5Qf*Lf!r{!$*mrp$>fsO{XJq->i$)v?kgq7aWUWT zIc;{Y{Des8H|d(TyeF;>wyX1L#0Kb3`m|O=4O#p2@QliD=*-W3UhlzqiRGEAB(LUg z>wZ?>rQg>dAi+C*Qo>2N1hnphh(YVVLrIn28Kmxa*!AS^eQTd#Ejp`4%JfRD!WJ3t zW2q`O#jG`+LOq6_>(#Whd$NUQEw3=4%h~pH*jH%Q;}pY@EPO!kHZIb-kM?_0>n0;( zjIl`wdpWsF=qWM@q;=mR3Q1W=-LF0`Q;pR9P^X1UWt4h`Bh38rcYE($z;gaMf3)G( zmvpLMe`?BcSInEN`@f&TT)$%G_T11r4B+>&8WyZQ8W^s^%8Jflx*;;aIlh{nqDN~B zce?=Yorcd_L=$_o;J>?W5|}2wZ*m6nG+8+QN1}mC-)fddiKaB=1vV#%>84B6+w$xh zK(vFy22ZIIt-IWrPGjT~KD2MA*e;!aZ+;`5gdW(51_D#tvJ52}F+EowpF`1fEwmV> zSpLKPcZmi}4G)xo^!XrHDTp?J3}2oyjy{ufAd|7@<-($q`=R^Txw^&s!cktq8o>$B0?Q2KzhO0wq zq#J0qsWG3%FR%B0wHkNe0B+(40Vzfwh&xOIqV4)^kM=24_V=|E$dy0Lk52!&k;$DR z8j1rT7m5SNlBysd1PAZUEQ;qho+b<=^uFw{?UDLc!5JlYmh0j<*5yXu4TJJtxQE1! z(q{Im8hn)2%Ameo3gGqBkaPZN=9v5ksu+ORfIui($aDH{l~#dt)&Cr#(N~;CPUQ z`feG~`EQ%<8ldV3lN!4~JW1{k$)o>|BC?4phX(em#m5pTZWeZ$HU&YJ(* zrXKMm^nl_3+BJZ{YnBNunWON5Bfhw50I-ArVb*?tS|GR z4UQ*SHk0Du$n9U?z@sKMVfKf&=j+XTl!cz>mwW56|KQ)NC6NBe*80`Y)UO7quN&RZ zX0y@7D|A1g%U|ZfY~aeVh(kB7l8zu(msK}B+GG2($1;s?e1vJ_W@yI?WYHlB4zNwx z2`osPQI@F6_188bMqng<;DzRJZFuCl5) zZ1qgN?$qFp!yWG}leg!*dc%0|oxlx|Vy2wBF`D~B3^iQD2B#lcq5<6qgl}sdHVGd6 zlM=k)5>TRnh(U>Vyd?FS7ecg3y@FS9-&X6eEPh)W@469pd*ZuW_tP&w)mYBoRe#t_ zx6!kekzRkvTMgMg3VE_y=zrM$NFn65YAhWeX5B#1;7lz#j82@3O+xK@3zTR+WD-b; z7Se>IEQDy4e#zSvy$&B5dn>te=c%1a01o&cZDH^6FS2OhJU!X5(S{jl+!puWu^fbRd(xb zJpf>?(Hvpw0IavPy72CDJ0Aa_qvez9zdd~sPeKnU02z^?&=k=yRYZwK%;q|GehQ+& zX84}&`>WYJgyc}A)G+$)?^&{JCN1$H$e%&qC%O^D9VnL&*ELt(Ea+3IUD)ofA6pu( z>FqW&IxP7tYt`oN;(JSnW!X4fRUeHp33hWdZm{vrBXHcWP^cD}D)EM3%>uNQUk#vKU_%c6PCq=I#bE}r5r?+RzI3#TJ$fGUoXIonT z;cRYtNmi?Mk5SNQ@=dIBSe}I#o*4nZYb&dvYZlIv8U24xS0Op%30ofL|mLS|LZ@n2j}jixVS*(CG5C>mKmVh`QuBp%n-ehvhp!S znSo8a{JH&xpkxE4>K!V=N;WY3^}1q|Y^qc7{kiSG#fQP+aaW!KvhB0p%1E7Ti+k$V z+==SIYv-=qT_)Z7Z)Xg|m9PU!Hjt|nWE(~TW9`|E;^HD%Hk0Ba>NXWz+~nd8l!;$t zAh7rL_m6UGw+9@1UVPzt3DcU_hqJPDF6Xgo9`2UXvT3D>r|F3?df_Km*6V)%uK0JsDc7yd0&aB-p}HQ8lXjNLQgfDf+Biq$`M z%zY;tM7M({D@nj!Bhu|Lv)YD_VO?i?v;FQ5)P>L85`4(VmHl4#-qMl`%jL|P;SLi; zeAGIE%wi04TtGq>>k1SXfn*X$aq&=>4~jSof4@H@0407h$SVcr@L3?GTwj+S_kS>XKG zZbga`FCfWs5_AM?FV*9SC7$2(ua2nk% z9(E^b{m14;pd}ud8loMDwZwzr7bfnaC0=bRzR$EHrkfWl_i@1zq{QQ*3T)!2e`GS% zMKfo9>R0>_L80v2o#uZf9&sh?z+U1Y;grTqc+-naDTGO;qU` zyWuScMj9KJHL4%YCUlP4s`1;XOd{;}k7+8j8D|&`MHomOFUiALyA<@7wDA;*FeDN< zaBkgZO@Xj?)&Zvh!oDAHzC%PJHnu|RN&IHof6POuRi64 z(1g3wg}%v0U~B=C77q8G{z41&LVw~GL2S-~ulORBWVI!yTK*M!#FelETIiv`27!@` z%7Flz5xFq5ZYDYBqPHT%Y)<@2`IshSe#JSE&7|)gDgE8A4~!zpMoO4y0m813Wiu(l z;>D;Vtk&d~ItRCZn#D@nh!5Gy+WMP=eZ}REtgapUq}@C|eEuUtKU>Ghwf)zWCwB4p z(p=!$cXE?-UZFPA>P77Ba?0a)_Y?-3jf`V*^BzniOm`AtWtSSMB1|sNZ$=#g)>DK6 z=!>V(LhJukGJT{7!?#dDSU6{Q#G?C)x}@?&9^h%0zqQ`qlh+nh-lddf_q0%Ri|+XH z4FeAe!qsXQ%E#IadcC`Np$wkOOd+hVf}GBIqs^BkU*<+@XmiY?zYs>=g{_%-E@Rs#hD|~!zb=Zf>tqs05msS^#1Mk8 zLf@1*iXO*q?p$$EedxTbz8e262_NOB>}|}Vp1EE+Ie|glp0b}`luUJDZUdgkb$)S_ zo6X!m3ZxA*b(|kvLYd@)3;?OL_Q)9uCh5jJJ_UZovWt78rkS+uPcaEfTVPsxeJF&s zz}Zk`tlf2J7j}az9M};CEq%Zek=DsBjOaApC~RGr>)-9dfT?sJ10Y2m3TH5UzVJO1 z&KgtcU?O>E8qRVzct33daOTZw%%KkFXUJZY&50fOT_-KAS^57>v>~2^9#A+#vv~{& z%=7x6Xcu;qESpIYmXS{#VdW+fYTudtQa{eN;bm_XxHzEDX)yG##pAiLvYgMJ*@~A> z+xN9LcRdOFWN|q6bnn4$O9zKU8zU6l@6ppP!*Q|V&a4>Ce>v(b+j(`E*o6@h=0F`` z-!5W!s15YJ;r2o6e;AoQQiPRzQ$g4*&WZ@hd(yI!J;zw$Z2c~Cbr!7&2{ocwp?O^6 zb#}_;fka~!_T1Bbd87J2eOpTyGGj2C_yJoaA~rmzz{cN!!jOt$nmHVNO%eNcqmB9lOhuqKer z5QG)@%57J4vt`s|)y(xTlMV7Rx9QeO3#2W#ZLTga6!(~aogwt8!ZKE@2peBMbIS@S z7l7aGDqSF4fD#W3$DMaaiRX*R0Ozy!I8h~@b&20J@y3k)6!CzwDG&xsb7ajx5f(uf z4(ten-t2=*Z?>mpq(T0_G&2$dN<1+9&`VlIu$q_XG_LdG6!VV6M@ytP`)YrTcu<4^ zQ+JolWju^X)0~Pg>r);^kUK|r+u>5?@H<-~2=lyr_F)Nigz;_GraM{Nfsb#w@gS`6 zUw?^s4|-rn*fiK(D?vvfi2;-g}wc7|sN|_h=ie8p`pX0tEfaU9T$#<4hCH_tS0F^Ffn0NV% zp8@m-2_$TeVMHkv4VQqH_>w(TO8n^($69-&#Gfl&K`&wGI{Reaya6S8$CRH|%SDf0 z5az9CxcNc!ZADO>d-Z}bZqIqnaxQ{-uVt7HexPj$=DF|faf@{uf?n`pL}Y0JS&3gK zABs)FJ0m8v#NQ^9Kw9FvqL6SyN_?J=+!aMPJBOt$I9*L1&Dz(uqw;g)jt}8Q#pSzL zR%#}m8;S3uizYrR`fZo;wK@-*(f-CP=+c5aWJdpDX#uqUgBX606h!O)8AKyEx6hkP zQU8-ZHKGra3Ob6X-<{$9yB!ZOE$f6RglxbE6u1DxwXQuuadDT-OW1J%Evn$sf;Jb> zzCdfrY0QpiruzbJx(IzW&=++0e)vUSu(b8!>onf4_<(uzDp42y6;;HO&;wdjpdUU}iv6LDN&YryA0VLbG`SQ5NALhECeWkYzJz$5V&= z8MWt5PfQ}SE_QpQny2o>&0nsvcm?a;G4a~+Rkwb$l;x{z>gH^9dCAb7Q5%yGaj9;o zL9nIDc(ZHd7EU+E!<{dJp4ol8ykN+Kt?evRaAp0>jz{BHlXfp-g??YtfW98z4Jb=u z$@G!p@JBv190pb-iN@S7m)`QT^yxOoh5H%%&A&C72+W)Bb+et;K9S{7YaUzU%5K+2 zrSaE;gEBbtjkF4N15B;8n`DU{4^a4c+1(s%5HQc@$`LMJ!g{NM9?gjXRt0G4Jao#>1tJS$lG>L z7?ki6R3yhWTB+gCLvWbwoszETVlTWvx%>t1_H&{} z9d#iEi6)$nUbfxx+;l8gV+U7J<{NQzr5$fg!EbRWl(~L}(NKf|zpFjAL=kotu?w7= zA2h{;u+FW2s)d0zXK0pxw#JGuFnnZAE{d@GWZ}S$FlhM!m$r$%4kIe*964#RMM^d1@Xk!%^GS!h$p-*WWD7-;vtTNAJ~Zp0#mW_4oW;? z^6%b%9ux8I{xRZ#T%{l$0WmfG*)Te3_kb*$NfFj@n>xbcOrq3|G`T!#aMV6wzH8oT zk3Ns;;gf4zFVrXrSqmQ-E9Iw`PUN^M&Zt9F^%*(~qX>&9(?^Q1i9J*h78#h5Bz*UQ>>}^V zrA;r-{kRnMislAegEPP31+Br{DnU9WJNn3q;85BvFMFNX%3c^Wb-IhH(n<|6m4VfN z=yNB4Fd5i$CmOa#*d(k-nv{?Lmw+M+gb0c-*AmAsM@<$Q+T9is;P=@4Oqio#PL|Qq z8tdntMSbWuxa2Dl<{Z}bSi|{@>BB9&&ttXJJ3k2pjdNwaopt-}aWDEt4PniP z@Yp1nQDL4fDFe(eq8176D|_GmBOLm&HAlBO%< zs#f_z`p@S?@1h+Y7K8~b^_^if6k*_}G3%KpiZDOKE^uBdbeaNTreA!Wz;h?kF6{W< zMHnzmXxUk;2m`~#>1fb5T$9McfgNFt)anU+RT^6Qh)(-Fl8CAF{pFi|;04{w=~!tC zhR>>){H%t~R60C1&6(~mQ*t6yQQBTRXdOg-(oS#JXaayy{8d$+;Gp%NR?T=lSiA5x1nLan)4e+iSP?OVr(yXH-{n+_6*8 zH=T(vjly3noyfa0F{2Jq)wgbiD2lKYGJPZnGvh@54EnxPRA9>Ed3PSz%BFDz4XbHn zOel#(tySH9pSv~GkA!gH-a0w{FK!~6Sb1N|#JcuCdz(p^K{3|C}x}Rz+a`!u5F1)c?Aji9k z(Uk6X->m*_V~g>(8AF^!>dE#@V>wd%LuJ>>ktoDvDE$Lde z9z1vAtttF%7q;%p%;!!}gn^%iKo~>(nn>c(39G-0 zFkl*cff-hWf#Eow$%({NvT$HW7_l8#Sil&DjM7V;{xy`?0|9sdMg3~<0}3cy%ixQzxaMSpYK8M9iMAE`*80s0 zQa`5*=y)xr-TsBeFRjzPywNS6Yhb^e;dNX?m`}_-_H=&5gICOtskx+*agVd-T0ND(&wHWh?L2L|Wx-(Fm<_^IKiS8Ce% zj<>^lz2`pJvK)(W6IQ?0II!V3+X#NWuA7qY#Pi-!hKlwF>I2>*CyY868z=Dpt^R{{ zVFe3qeqaBA1g)`236J3t0K(7^K@sL&Vno<)vQW;-IYQR#ftNMiJ`pvo_|J{C1yB5Z z`m;J$(GL5!W;TX7;}%EK*c=efi@K=i5w?8dR;TKwir$@WhuQ~d1t$@fOYvN7{tI5%z>k0x7~|M3EmJ1Yu7+mv2`*<=DcUw`22x1@DHd@=wyMNh)<;Ra;brn=tld; zuIxL)iNtgLV~@^&vnlRdfoZ`~Zs~H z^l&pKguVT{2%|+zwK(zyMc6a4Y$in*zc+P+`I+2S?aV#;@I%$2dXBd)_UrOy+l|EE zOWne)XlQn`+Iy`3`8QuqFWHP`!flQ88J)A%*N%x;JW(5Em#1acVtefVct6dMz`&cJ zI{JEmum3JuF^c{1ojQ2cJ=GC%{@lLluwgD-^ zHp3ZRy-HsO*i*N8m=;WyKT}(g5xW6B=0hJgTNq5LlA`V)^5ML z^S{-9KD#lzg9W1C_kMCBF^fzBDZf!A?Ent92Hd+z#9`X@KlK<6_<#b3z%(hoMihrRWM0CK zLndmM)_k9e@@>tO7b-QAruo*i=HS;#(4uun`>Rpsn?hINBHEa*_;J@(vC60aS~SFy z&;!ah24pBS=BIC_LGDu+sSP(emX6|(n9awyA}DYOd$s(}eX z#C|kA`G*2Nt~57fiMM0VpjBftKR#Fb+|HnHj)Vk%w4)m*#Ub@qk`Lb9_WPO!^u0N( zg5ofjOdlx@)mo_F@D8Wli&;@iY?aqJWQxV&zx!ERUJ@XS;4lNclKcsIt_H=ShX95dJs^SZ=A?u?xC9i3 zAVg3cdX~uZ?L}~?zo|k$s-chX!0YC{h4fLwo9tcpvZ{s;ey54s$gsa#OmeLDb)TSA zQ>W(T?))>(ta|Kxm!quhb6OXSwmhMR!}jnw*rL#VPz{}w%qNpTio;D-$j=Fa!-wt< z(-luSo#yCK+$Qm$^5X05VF?}*H^zcKEO}&j`?Js7uHnLWw?DD0?zbEV9J*g z(2njoSvHd*Y*jvWgxQ+h$-H=PU&;qZo)Vd*YNBgW*fvS-y^_2{quK32ft1=w5e^OE z`R6qY1@PP<-j9S?rJ{^(eO-@Q89UXOq zbw9f}qYhEkC$V=aim*a5eWVE6=uHJ-cRB4|b4M;oR&vP`HC^zS=}6hQjHd9)K>Y6EQd(5YW@Aryxa_+_2i^Rl)Qc*;keM2HCcjv#Ck>>EgJHN_=I`}0n>fv)Wtm&K`NI4<9AUI(JAPjy zK{jgq^v-;CO#%CYF-h(PMGS>e^A&xf95~+@{D-jW?N_&NAJm=t7RLIm7;Yp$mWj_@ zh9c}GnFLaVnS*SGAS~H^dB38w(`SKV?ONHc^EIiFh2E|)+CMKjf0qsVa!&U_0_S~W z>02%UVL}+5tGTcz&hR@FVZdmC*L{#3#lf!#`b!$R(2ssc>>?O|{PnKl9~66~zmQ5g z4iKgw+j5nNFvFdHwF?8L?bcvJmxdIPg#$anpoJb>T8btbE%bU*ULbtBF*eRq_`Y3y5$;xto1=99_>nOG_4P+%+gKiEM49 zxiepK5%bm6G7mJDO1f&CqI(}Uv@r~87v{gmZidlNgaN;EpI5-zg@NG@VwKP?EC7)K z&R>_vQgmTNgb@NaT_Ymw$lpa6FiqMn2kpYj$ijggVbDqmE`9UaN34|+9nRH=R!XAN z`Z`}zR7%+XLkIp?rG&y6m|FU^5rwnu0LSi=(x);2eM z8xz$|^Ho}$wIj_P*B`c`!XTmo9+cOriVE%81jysJ!+62FL3)>%Z&PNkWP{w`bmvt-ZL z3&+gT><`8e5`%`clE3@AG$wVcoTS(87F)0J^~PwubBh>0@WHsCU*GeB=`LlC(To!B zzQGHdg!07`G=4~kD#;{}mUve!WB{bZ-*hmI);EG5L9nF#am zUMOex-1SY+Fx$5m1pmnr|MIu1|7=&!e0>jE;z8=j&SJn?;=yqG76!D$2O=`Sc`uTY zEXcp_+3mQ`^2otshmn6@exMdzELhDIb%j{s<^OJp2c~&^O+ZWh8?tam(E|^Iy&0MkP7O$dF6B6Vi!=2uW%!(h`J1{9ywzFDi*)t>3L)Y> z=m9N+jL1-EA$)Exg2I%To)62~DAJQSrTM6C3{Ol?{Xe#uAM`c>C|X2*(T3rjKebwyM>XEd zf9N)MM2lB8vW9`MO`GY#Wnq>@JB=cT?|aVaX0H}i>}dR^O;f8kAz3@PJ&f3;Xix3^ z=U@r$2%txTjr;G5C(tL*x*naSdP}B{6k#rVs30tkQ|=>kSVHnz-p;Le?>xC=`<>=A zy&PRmWA1=<>F6M5*UKjHB$3n4vqyYzwX5k24C#Z%&)lY~X6E$jB1PCS*wTsGjwRS6 ze0JZAN~nfQKoJH)1Vxx{39sfZ1YsgWYG;Q{PF?P;`t+o%o6kzEfhj}O??BK^0!@9f zy2x@@_f_@7VS1l-5MskU_ZZM*m{x4o)Z6LxUYjX1avEX0Gbh^K476dBAaUCeooK5e zlR%0v|0X2T5QN3KJi4Ocbn?*lN>|I5+3sn3eZ}dsr8)YdvzCW9rkjr2WN>Ppo0BSt z6=6?0da)Tjo@t0Kov0-fY zC<+L%h2g6d44p0@f9ydLl-$6yxYL>taswYgmAx>0j^gkgnU}ERkdfM@X=+{3s!Md* zk^(1+7KZr9-Bsm4F41Xff4Axar(JvH0cp;lpa;VpuT1V2vVJNZ^zQmjcRW$XVx8Lo z=y!R(`vvI5A`h%!INpJ8`&hrG!}{Mof`})f2Nd+sY-UCRW9QzEK6iRgmd&I%JfTA! zhk7Ql8J%`U$)=YE6Czr(1IHs>R!$h(b~Z+-i0bRUD|fIjZ@oWSU z6P<(h+gVH=fwZe~*-uyviZie3`Pn2@qB1>cr->|v|IMVGnmOz5YZ}luTX6CN=3v_i z(t{L-{>Yy}-{*=8l>5kdGofppeoIEf5)KE!#CIDWCZ9K~$zs{rlW&p4Vq^KGk^5b; zkAu{}1>L%NUFVzgjz8}f-GEOb5OD}zNrpam(jP0pCP945q=b671QdrLL{J=_DelwT zX%fTD*Q$NBz(r?}G4@4yu&7h+uA|2CG zPkU^lsWSWnZG0hX!ku2xq>;thqwRDjceb6t+|g~Vn7Q5q$_0=*I?F8}T!0b}41e%q za_K}cA_JUvpWQ`4JmMS7)~W7+#NwUyclQwlrupiWp$Kar3&;P6FmP$3U6YHbiC(Y} zwZMe1wm&ut2W=dHsa5<3up$f$KljQ3owVC972iA3$EFdM@;D{zF+f=A&TGGTqPDYO zL?dQ?2mXls6Wuj${~-+VB=o?JFcu^*3wtM5|9vFOW>SQmX`zlVO_Mm4R|gD|O&xwn z8q8MjERE(}e_uR-KE<7r9bF@tK2n6;yiEmR z_X4>;)80rl(09`ok9nkP$mA$1W@*GF{7O^EeID-R5;|tXrQ#`yrCMLZ*fkC1x(v7+ z)vcYz?De{Tg7>+=>OVt(u%oc06Er_&?kWxpq_}4W5}LavB{ab$pa=sYf+Fl}ai4(^ zf-t7nGWQyy*-dYpmKxY<_m<`{?dvls0hWRRhY|`+jDr8@S|qy4z_u z&L}N?F8WMPcjsP?G(Bnv`;nKAEeh<`lN&=dlSv>&SiC6W00_dOPp{}l5axY`rfy%Z zp5=N@(X;6|Ssh$PyZi%BO&*Pq+@Z8rD|JV)c647$V`mr*MHuiqcN7m+gn{8Nx6VSF zwh%-HINw0eL4h#hH?>sdD=!iec5=&KT{;0wTU$qqE}dv03kPM}?jt1eU5oVbB;mLahVYpKsT)+6H z*28kUUZsq7{9K=hMb8ZWeGnM&B=o?JFjgcm_uA>vNy%2SY$iq66>sVYlQp@ga?yB0 zQvHf|k&&w-HY&$Du3Ma7E}yaa%ls%a^>gEbI}{(WadLebhzzG2OJVvc+-}CP;QV>U z?M~lW@lSF!xzGBJ1`6%hvfF7&iZJynT7^c`mrk51JT#*Y0qZI1KhTFeas{2yYa`P~ zim(SdR1kJQkoyPj`m!K{MB{=N?L5pyu3`p}2*NUdi7eZgG=irN0|21>c7gj3$aN^wYrK%=B=e5!e{9|niZEcRmX|NOoBO7z_`X#3nBL8u zn~PiX8|cE^B_91^8^+U&bE5fIx8r*+B!61};UB^fPeKpu2xCJ6le}hfBC(w;n@JHC zBSsxzQYQB@ju`4Cna--67wKe?$dt&n6<1v~8keJPX3*r&pA#%7s{1od^9L^>?X*wo z3eF`_%tth>m$>E9v^ZW1*rzrB`lwkhj|@)N0i8$$T^RCQty1AiZrLvo7G!QXqYeS< zDG&zqnO+q_5!OMbj}&3gTBsoGK|uHSiEGQcc6?DEuqiR%k7WDMnDJat_-UEGiKE+1 ziGgjR@=d|LCv+Ru4koVp7{@IrroQH4|ANe_G%%3}5LQ983ybU;!Y1KI*`$Q8a0w{F zK!~6SJ6HU6zX5`xqvnBB-Qx zeBydqJF))4u<16Ib=^z!)M`U}(;ELn*yQRzBEn2lwXsQ%Xc0mY_Ki#eDZ)yukQhP` z7IrdqyN097&Ds^#M{t$4O;Ncw5BVrbvwyT&Rb*E@mypvyr+BBL&jo82Hoj(jhS5-j zfz+W<7r}}!F#M&t2#T;!LRU*|2t^ne z?%UmpP9$!gif@-)!*mxGJaKf9E{hfp=kCV%gZiSEIH3{NmW*euIL?nI%YJM( zIJa1PxR3dcbIDkcx}rznF2Y*JL2rXLX_{+07*nUVuLTRdXD*#san*K4wFCxIfEGv? zOy@vLd^ec{(h^?>f(z-KjDX*Zz;!$?$3CRu4@B73_kzE1>8+I?6}}S0uwed3!SvKzZ4$paO3CH2I*OSxQK??sOLWHLD}Gs@fVt@JfIpZ_D*+EZD*$7tEEjrePR z-sY7xi*fV2-Fr0!JYBEs;Lz7ndKRHn#VjH|^5R-YHOZu%bCtrCtY39J&t717Wf$nX zbk_uBNgtU$QXIY)qk_W(j*9W2D>6Y_w(KdD`g;27Kn!cdTye>dcUL-puB1Jd%o1y? zUT&FUrBTsGC)mcmM^;LCTuPgx@wsB6CMgb~D+zPI?fHFK1`^ahOiJj7OF(f5LImx2 zE)=JEtV3|vczVyzSmmFLZN*D3npzw1CmypHt5iPN8!$@$R8BgoOh#?H$xj~crQ)7> zmD<|Z8CJP`Dk0ojpJQw={FLjzj%Owgmuu|ACV{rY1jXS1nFLZCc0?fw4Z-0x=atnO z_HK@Ht@I9>rdH0Wdo&j0=$H&M$*DAT8S&;dM`suf_)WoZ;P=y) z&CqIm1sMR&<<%&+ksvN)OE}fxMXbhAoBmWah9V4@R;soa(wqTjLzQvzeMAv9NEQz4 z2!nC~Sd#Gc4y27>q-Ke2+Fi6kBKp{(`V!R!iIjNf{uuF~2m{KLqteli2YlHaoEK)N zOm{pL{-gY12*NxU$$D5*N0_D%-G<$^pYb%j4|VJ-{~-+VB=o?JFis>8ug*xJ2pb~H zW>SQKh2+$Bb7wM%SMFS-oM_^ZbD@IWqyFKG)&ROkN=t2XwVRHc&&^^xcD-SK$8&ug z!4oGnRAAq7 zP=tXHks{1v;%V%Iga_gG{EO54^bmxVA9!7?#hqI?Fw(V0&{{Rb{B~{K*?#%!-&U#* zEHrpi)5P||GjxByf9(f0&qE%x^7G~lXzkh~9927#X2xGBx^MU>!m)9}xD`(RzrHSe z{xxh-*on(W5jILDffQlGT1YxW5EgnOwI4xPw+Bd|2)l~d1DKULMHm>aAt!($jOgPoOGXNW5pNUL z-mIEl@1AD;r*0FV2m_{Cc?qBh+cK37kNtS35th=!W!DT4R=6f0o;t$DeRpi+RR4_M zonDaVLHp%j7lybJc3?*s7ZR8=#uexS_HnXoCPi55ZK?<>8Z?Q2_DWPCp>D-PMsBTK zU5^?L4BJ5&uTF@HP7_OJwH6m1$ z)90LjJ4d9UAXh`^)i(YucBSqk1k1kHQe~%eu5KC0zQlZlTPgtW_+Y}gthrh0JjcXr z`IBy?6Cd|a$k|dNj9bG3IqD}q_O!ONh5L7iV?c2`3%?fgwcJG0nB1+jKvXL}@P7!5@j@cZ!~+n9(VjD{>6|0BY{rC02N zeHM}taUt~Q`P$+-iZG&&0}KNw5C)rf?Ebq51Io4(PVW7;bt)ZZDc+t&7%rfE7Y8eX zu;^tjzd)G0?^(66*+>`GTsQJn`X9m&PeKpu2;)Wqvzjo1B8-+Sn@JJYt3w@OT_y?2 z7rB=wn9PdvYH|`y;`kC)B3(-RNn4vmDRii{{44F z9~98m?fxF6mm#=dpV{cz1Oa1y>v@@Az6vaz00>jRqE`KjE^JTuz8Q50RCkKs2+%j7 zyajDx=*aYuAk3T@`P`3a^WVTdl#~m<~0&05(nlsTOWnJl6=Px!$cyGAn36bm4ME? zlZ1Rq0*WvYA}GQFiZ5T&HND$~*NSksyZ`)Ap@%A;qI6e(tY7v--M@?D@Ny%dG z^B>RRh|{gVh$2h~Ll<_z;_QsnfpP&Dy|8{IgbPsOf#J;)Yf$1{2QrZJBn26Yrj1BE z2Dg-LYM7uDfsrWipubW0#aLMgW$^c>M-p77{uv4jZ@zuPa@rOmK z(**w^j3o1B{wsEb@gRX=8k;~7#za=)Nf1^_+d>^-jV6i8N0^r-?0R#9Ao{>dz-4r> zpK&zYU&&_e($L}8I%WaO=9w&@jp8kCGB6x($R4OZ$9qHLtmJ9E)>Z+LU|fXLLYfDX zoEj%rrK4S#If5`Sk*MILQte260sBiK3~&DceeWgY(fZF!rjHb1e7C6}EQRCYoFBo- zVobG(@FttI$FvwCtjiy)y3KxemAY?cCR0_xJJn>95Y? zQSZm`zMNj`yq@QEU9WQ;-I099U%VSyl_d@N^19WDNrcg}B!q$PKWAubNb!E}DXA2i zaDWP?cB-ffY;Y9-!cZ4M5f)h%o34i-EOVi)iEfPl^NyJMdyW}p=}Cd+OXx!9EY`aoMilb;sagHFlrGdF^uocRGMCm&{lOiK{1-V`c^6lmBq3~aBGG_VWtSXn zD(q?2L=nc0QGpy`@=_o-(B(@Z-SJY0F}&8V-y5uHc;v)>~a2+nsc)1ly-*{93LKt)H+H^$X}wW0%}C9YToAve|NJZ9sGzR$&pu@db#gfa$5 z;<3X4(hLt}3>f~xc@NrQZZTnF;@+K;9p?CC&pru+F}Tt;AwTLfeWfm?=dFal5H25I zu`OrrzcLB2B=mqX2Krb79Mi|`j=rTp3jWARa2z2Aov(_>YSOT_^r{EY4;eyJ(ZiPIxFNJ z1tzN>99phPw@8ZpWPP+K{j*4>=LsUY_HHNW>XvrNMU`0Hg5XfqvWqqqLf)=Gamb5N zfgFcw`N*9I!QruecdK;WgLo5W?WfnvuI$%kCg2O&bXm?{XP_hA_{Q+fWYA*4PL!fVLtEcT%X+)?V@Y@V=@RLPtu8XN&Vc z+9H;O9#GmsqgeoPjM(buXhp}5iDq(y$;ePgSoxNl_c=OcuWjBNIe1GXXfSua$5v&5 zag;}`U7WAPx zT}oC(v+YI$iGlS@`*lKcH!oH}uL-UTj@E%h+Jx3U{{wwPVFpHMvT4ZYz$xU=(jzs4 zM3?Q)(L$hgdZ^O(@QPLWLIypVZ3!nD=J61Sr&eD{U>jhJ7CDi%r-1%&8T+%G9;J@Y zyoDN1*+le)%QO3I3|pHxek}!06X-`Jm64B1l6``taE2vqD$EvXN9`_%QGpy97>#F}_2k^1zCsbtI0?Er z!l;wL;bYvL+r$=vq{Lb!MV)^nKr9J8pd^4kd;rIM&UZi`K1k7AV4zL);e)*Hik-HH z4`?*cKt#P#JiZl_FeaMGp|zxtI{6V`h!^>QJ5|UKZO->0D4mC~s-c}b{&v4OsqR8n z(kfNFehaPYkoG?(PoSTZg**DhFM`pJ99qkR;m~@fQXHSC+&?;RQoE!G8Zdb<*GalMl+4g$H`-g;`p#gq);h;*qX$YnqQl?h%5Of_yMCU?Mgc9-YsXm&?$D+}#|2N+?qV1f$f0Eg zZZ!zBLiVbtAkb>lc$L9gtpDny`5h;Nypi2K9r?_r`RbiIl8YEvMueYV8*l&k^QRw! zn=Vcf8i+&zEf6e>i)jAzBN*PwZ;wLjEOI??-gJ_(XP;DQDU8%lc8mD^-AW6HCdKd- zg_bzR9kfG>nVQiQxR}vNCXy9qUZtZt$)tJk$K)iF{In%IC|`jU8Vdwyo!11zlMjfX z^O_(|!MS)e|Kz-;)I}Uq4|tEP@WQR%R9YIfylO9Bv=9Q%i3auU`o~wqlF$RnS17au z5l2*|)u2zrvoO(2p42U6;7R=zlGINr4tQiV-FP8#McUSHeuAUdh?CKi?4N@?57i}Q zZ9+E86f8*ztI?Gi_(eS0v_dZ2oA6&!-;7D>895SDk_%O#6V5WANnHXXG%)frj|6~8)&`tGBT%vHk7 zzq;yZ?dX&ES`!lJsyyN`+%1+)UWvq2Z5$va^#I%f+Eh3*CWj_mT#DSf2%b>gItli3vkgN(vx{ym86X6N=sZvw+*b1C%j)szvp*;Gr(@@) z2n|G{Na`S1Ec)isT8V(+&rQ~$N&OshJ#d~$N7=>hAQl>hZ2?rCwgn@q5p~Np7ftF? z765L$`Q@Ww3|E?OstRwA1wjF!JccCr%5+F7ya8_1Mj2Jye?GO$|k zCA)WC3t^7j?XW|*)_=X;h$&$QG*d$13>>qWQ5CI3NYUK;osXgt5s(%O2GLxuUNkuk znmTQ8hDNgxBC20D2l{4}G$xwKE0J}`Z&P`*sx)&;R&IEy@YPMP_jpPd9bO$AaD=Ef z&>{Zm^sp^EL9%d};OK&{t@5sF_ftP`(MiiNG>IIzMW`?&dhGsrD(6vC-7_7!WJO^H z`7NcsJ1Tm+EW{|sOtBKP6JJzG8p~Hp|!0L4z1TJ%W{I}4>)T)%ePc! zlb=sYzy5Q7yNp2x?*`X_WBQnEV^GuoV+_)1OI~-wi4kAZ=8}m zsS3@1NdtwJEJkQ@XaT1{E0LHoi|VBav<}=?EODqksv0#%c$s2Krrge5o}2m^za97K z9Zld;#v9lgq=erL*rl%f=+>|t@#&8a>B=BgRVIaS60};7=`#Q=(ieQA_D$VO zh^h)od&XNJW@A(!ht>go(9JE3!mSGcZc0U=diMBw@!3h0-ZGiwcR`n4)N;GgIf%7#neiUOXQ!>j4=6>!Y9QLIycP(nsk(Nb??JQ$VlKu?v;z>@(*PvdqE~_vZS#cD zYMS#WiN@UhNrV zjuZ+&FkHLl9ST5F@QYo|qyXTzv|V`gac=NTF=-v}?%xHVFe2*NZ#Wcya+qi)2cZ3V z3;?}$n7Ed;t!!;;Elt^SE7vVo;7ZLY`;MSdr~IsQ4tm_zx3fNv+#sDT7ix?@-p$IC zD;b#OCy*z~)+wORFKqu#jKM>!o8ztc$y+$P0(=S6`)h$n#`PuU2-19^iR_*ja9MC|8PiWxMFa*z$5@wtZcH$0BBJBg*FwEw=1A_pN~<29DpJ9 zh%+Go^!HGyTJ9FIXGcw03@2ZSZ**R2mw&Qq|AC_CcJ=Jsy4}tUi)J%vqc0QF@(Fw1 zLk&}e1{9zm0SJ~S(YsM-r62>q`468R6wrFHB{LG-w?kXk_>=D2-f1htfc2oF?zZxz z6P+-H#WuvqS2h=@q`r3Gyd+@ zj39;dh*T3;jb| zd_PIvzjZC<{iknuM&hsGUkhay*B>v}JI^a^LtWXhL*inBfZ ze}&do3mW738fsY1*fnnj<*$3zFLeNQYD{h6b8Uyb-*)rN&uc<0!Zjej?jYXf4JF zO%5&K6ezUf%9J}45NO#(?j4!C;+KY(bZt)deh=JFCB2I=95aP#dM>Fe9`ks0E5M%d zmsU)zT%A`-LGeNsgAv){)VL+V1q~dwPSl`fb$JzSD(qdh4Yj)hMg?+c#o>{tgh1<{ zyQ<)Fw@|TXOA>_QtT@J#ez$OXx5__r92p?^>*)g)z<&Se*n46?+)(4 zs;&`jd%ca)i1 zC!A2F@MV&jc&AiTFm;J%xD)o9ot89;d_RSEG!u*lefa~H{$nO$N$3G(Ce z+G{L0(@!$m;x3v!2<>Cw>9CmFgg=vNw7`(lT3PQ-=WXG_`WY-SZuKS}XA;K>M}IYW z|6ie%bg62}?MGEVFNFd$c`9M_BZpS(c{sE_Di;qpN(_v3<7E3%=c(HkbTf3T)3f~K zCXTAP^r}cci=^9lmZ{R!pxS|XV1#Ko%MTL?juaBK{Bt)8O|7)%+@4RH(0lBQP-rd1 z2u%(x;1npdPM0YUEI^=T>60p)@6YgvRY`NqyR)J)%RtW1L05Z}uX})#UfWzS0H1Hq zkk_lZ+5FeFA`^P9b*}shil!g+!g$9HbWB1^x!UF)=+xsK4`Y+WG}=^H_NxfByD~-v za%f$WLM{q{miL}}D$93-9@S#EOJqFfAtK<}oR81BN&oTTt5>OUPjCx`xyCYNGs4lS zMg&^@_;KN={rx0vYFj^)Lf~dNmn4E#T4~6Yz`3e8KgF{tX~C=bVa{Zwl{IZ$48VF6 zzyzXk$B9BJEhx0WaM7FJ(cv^e3k>JG^mB?KLn|tnIJ9-vk-HD$l=EpK@#^4y}6SC&F9A2KR=4wZpp~i;~QAwyp69 zZS%3JQf)J#&CX{fnXiFuzsx70$*80 zD_Hv!Nw4P8KO!E<8;JLNaiPbl6qj7jnxt(?=sH2S zuh&THV}Gi&_{~bD2n|G{AOQ##_8+?-B!Dsw3@^U44^8dok?Vo;nv*URshz|);RpPa zyX)#tTct%$O|!3yOVHG=iE#(*shyeHrL8i@_rE1sVL?VRMOC)Isns9Uo8*9-={jBS zLPOCF9F!1o3sP@Fp#_H1m*k>FoXv!P?>_!-yIk|?TeHD-xwy@JE*{i7;ro<)SbrXE zCUB|j4Ll$I4_b&Np$F~I5=R{4x}yVymKG+O$)S}ZLmgV)Tkfho52MSldae8Ibvx6! zGHw37-y{rYAFj?mWtKLR=S<4HfhN|)KB+nVhI&s}Z!T!Z<08!p&HG2CMCPcQD-9O2 zigrG)}2-w+MI!Yf`;Q?SJKAlM-Ht!jc{l+D;o>1pE0;y=~XI+@;A}? zAJu0b9~1etzgM=|Tll0>sc7&St; zVpJf9RtzHG0y8K`teOAFT?N zw$ci%8Rgux32o9p6K(3y!wCIQd%?N zLr=^R)&u_lh*%PO&LtGLD5gD45#&14W18P z<9sFPADkhIcV(bw{=vN-#}_`a)5D+F zj~3rPF-2%-nF8!iY}G`Q`b9)8aK3h~5=BxcEeNTI)E*%%6F2|eqz**;&Zk9dQU}8$ zgx%1jZj5oq|CrRlr3u+BXi}fL=mt&dVEALYFKALH**IaxXR1m4!{)1#UrEgPyGb30 zI!mGtP3m?N;h_D!b26#de|O)$9V|_~JCyjFr73Hj^gYKin+bL~Ctk1G`7f!1J05n> zp44X{j(cfEUELHXTj)ct_cew8|Ff;Q+u)S!1>-$I~-g6;wWKU?>OtCLD z^IP)MeEl;vaTV69>`QI9KD2qDPdW381ungUKAeF->>>`=jo}#Lt^B#x+L+bq^nri$ zkH<0H!YOG9hze5Ely~`oy3*_Bpt*DfMqP4-y^(=4?Ay{OR+^lHCyvarmbSb0)j6sl zF!F&``@C8_MrhRu~839of2tD51V*BNxzk~Qyau|^Vm-f7NG!>?sW z^^~sAulU6FzCzE$lY3`@T@F(g%L~C2Rj=U7P3+xK|1oSMx}0P@F*Db~&q13C-SOjg zUx`tHoMBC=h{qrd+wGPkxZLIVLH?h78RFSzHfA-A>=^5A87@nzY9QQPTzHw4p)|a{ zX|i$ePcw7#Tf3(S4Mc(li}2k6vDMej zJd67LRf3hNA|6?EaXGm`vuxGBOpjO+dO$NhGcpt&p6N@_rv*}Y#xhG%JT08Qz=uwm zlJDeTKW$G7(Bc-vP#CikZhqK*kErRj zPiFfa&16qs?MR=_I-9TiyLMEljfv(St7~(weC&6PWyO)1_1p8Ff9* z_VLot#}&*+E7s%^elZ5VshVV14q(_ev&}Rz%)47_N?M|-F5zqn%CI#Ub;%jlSO{m> z&!tbil~@OZR2d&#kKueb*Iz~KK=-TAXwDVH;LK6`fb_hHp2osw37zd{5RP6YZ<2U3(NW;07 zBf*{B?2N)!^?RL*jNOI|HYw^oq;;aMZo$-N2Otu~T@UQubZ!fzxPdYZ46jJFL(8_y z$o0T^XXZMJvTdK$wm7h^;@7w4IEi7Uf0toEwC3DbD8tM!?x3Av(EI}~-CI=z$v;r! zf#JUzic#b_Oc?F5IbjlcIDL1XURDHobN22pqK>@ns&6h!uV^A@- zVF&HV1CH6_Sc)Q#6wSu1Hz<%tS|ubekn^SudC;UTiHN!@vzM*_(byal&EyRGbpAIC z<9m1G)%U&GW50HLN=H`eo;_$UnX6YMKKPuWnk7+I$xhg^T-)@3{irQV$qR{wXl_RR zMSY<@Y0cGHvPG$h-xz1V5^i$2BS%=1GAgl_K!>XqUEd^G zG?%W!s7ubUZbvx7x|bSz%g_(@4*rl==iH;%%6}!qKHOCbrzhK^rT4-hwdxduuOhcg z*rsa@j;lf*MoTgJ1ninzYzm>M!!o|K(I#~B)A2fNJw|A9h5;Wz8FtpBPi>aX>`cDC z&n2UE+CP7tqmN|H6dhX@9LJ=+gwHXB`%8SgV|k${k?Y4u4PQ+rwU8(CQZl18m zRX=U@!Y)mLNrov`+1w*z*v3&BwqpWz@3ftZ+I<5?1u}*`r>{ri5W+B*ooZgow}qb) zmJ-c8V$jtWla^{!DDakb?cv&kyqfnJT70=5y&+x}M;TUa{KsDO^9SBdu{#inf?+`D zw6{BGWf&M<7PJ#(SUPe&aL)MLk%D1MPPfGahOO9`JIS!TX`6bade;ZD+(H>CHe#Zg zoMG*TzhM|p^R)OcFFH2lTKMKl(VcSdtN9Z3tClew4CogBGR%^s zM_?2P2zm4C6f4WR)R~RJ>|2NMQO!1_xkO|WS!^BF*!g%$E~P5;nZng*9cG0Qnw(+4 zM^J{HGpW)OM;Jy|clb-Ibm#%Df~JBpJ--#K2L2a#7*)c$xHTB{oV(a%3_RPJg9n;e z?9RE24jK&d&lW!K;haH7e}mECA&Ft10K<^hbW$A_b8$IsDzG%KMeV)`qXId@`0+^i zLm0Mghg!DjwurGyLrP1RhHW>lq$3eX5c_GA#6H zGi^dU?>9voCN^V){!POK7)ilHaG%Ag+R@5GFg%2Fd~d%D$1EM|G7@wEig0T|qJcQ;laA~6PYFanU1H*@twxAp4IZYUClj6q74fA-dx-@4PPVX45-$(LUWI_@#)R^JHNk}SaNv1-2_>D?ttK*+wKVTB9`y;}Az3J4pF z(Byy^X`}*(WRog0QJdK}hsx5J3mp|%?c1L4g_nG;ei`kGtM_(tH{puV=G&{kev|Jz z?@qx5#ij(ABgA0dF^x6MO1u(ZbzX`Ngp+NPRE4&IlK|1Aef!V37N~H1>s{3Dwip%2 z0U;xW#3BR`POdpsrrS=qX}-3(SMzG@bfjak%&1y-?`MTi^D0U*AIY5>D#(?H_5(%E z3>sdzm6-1|MQG^E8xVS4jw)KUTtVal=WczAD6;g^jrj?nYGI1iK0(UT``1lZM>DV< z1%H5OrgzmK{DD4lfZ+p2;?N?;4&#phF{y(~3)O6bJfK2z2N?eI#b&f7BH6g{$_AR| zgM6kfcR=S9fvBu^UC}v3=ZSF8_jjJGi8|N+in@pJ2iG0b{~P{zemL=SV_YNQdw%KW z8C>7~%_$f)3kAsJSy_6 z<gkg03L(!Hp zWgnbRom{ekeXex2@sHy*%ejwj((oZV1|3OW$lsIToa1on;IC`gB=#bM((5n@rkH5*DJznGt<8Ys5RBc2T zn%H7f%7a05ekNF6^uUv z80PVITLg(=x$FNb!+>altLD+lFfhDK^9jl@XN)`kM}~n*fBQa$ve5T{w~9SsB`ReP=;-r2nP*0^+|^1E7Z9XKn=yZi>Zft4Yl@Ddhppj zjRe1@*rG7`e+)w`2|Z|Mm@MKL`5Qeb!?t0fnVexWWq!jjmg>b(OSj)1{k73a(p|1x zJkkB8m9^9RWv+o5laVLxhh5?~9-(5u+=7JHLoMFI6P==+NWZMZK3~NYsTFKmUSe`|R@^^b=i-S+M0T=+vJA-b^}mvWmya|ZLPiiM$ur-RwUjhak4Vhjk5Yewt!(1|+0 zFl3?*ypBt1`a8qyPn+Gpc8}ZL6{7+@kWxe~_l#xZm zF7EM-fk0AM*=R#vm>k_k!|SR*1wH;PsNHvCR3K04>wteDNqy^fbuT2TcaD7eX*lz| z-Lact1>K`v=LU-FWpeA2Yy-K(e{E*u;i6W^3p$(IVY2D2Z z48L4EjCOKgN3I9XP562#k~*miuK0C+)MOpAVY<>8)rc})9gimUJs5ZVk4YV9-E;U6 zn$$^Fc(VIG)uc{7(=N4hx~u?A>Oj<_q!+X%bue7{;Y&2ByG;03_gCp;QqOXJ-y8&z zy5>3JZ@%&IhOO)h`=v&LAkP)HTDpHp9kC?zpgpP2K^*f?;tg7uxMQN3yfD#n{7qp( zSG~AntK%*IEo*Hg0#{aFy5@ag?hl;#dwrc1hXUt5plhAOqNMAX%F1}CP2;=Eiu;0( z-)rN)1%1nK>2RVJ`!>RI|FbWN+ulKJAg558a$mW97sWB2MrX z@+EcaHBI1CH<&}RFoZT0^q!8Z;0ae@nLUp81@IWOOgV3o?ahM_t!(e^Xz{)GHng99 zCJ^_0$GgTCoVjw0d|?mSUuD*IMD1TQ>Za50x}eOaT($7gO_}~SLcUiWOCNryno)NJ zsZ41BL?bIxE;H%VrozU?Avy^}|9u!0*pZ*T!7;9>$Kl)Cr?#p=%K_lg`m;J{IRF9{ zoU;xrrI-OD^`1H335X<-X7+p$F}tnTt54;2RMIjTa`G$w9LkxfiItGPHzPt-#}E zo8>QNym{7F+YbYG^|zScEj%8gxJD&m6aNeosqWK9xi~(D3;XU*YLCrbuoQp(Zpem( zLJk$%pEz9Z3mOaKpEHQRH5Q+IXq7QJXci(fV6=ip(u#&HsDZl72N$As#{rDGVO&zCCB1xMtNe~?^7AGI>k5P{q8Z0&F1-G z7WgIK)0gd($D9L!6QHRWLqN0J2?83U4>WxG0jN;=a9jm%xC$s}fXASqxnL3($YFCa zvcUTGotB5hpSY3*7MoVT7`XDf@6xEZafdX!RNlhH`j1u+E4y*@BlEB2KOEN6(i)YR ztsv-8xNr1n+<>R|f1p9OrvuH`6Kgsg_p<&B8lZxw;`p|JJ{T3qOD7L~B)TD`(?;jq zK2wL2>}I^jSEeYKFQPNQR6{2o&A1Cc_pqIOWB!IfCZDW@+7YzQhixD9o+305i2@Tq zXxX#&5KKS;2!_u=hJ%g3Zz6xdIorjp6aWl~U6ur9f`;~PoGhK}rwxEmq5;uPraeT7 zb`awZ+KC38H3FB`(pZXikL{SSLPp)<$?h@5y}M5wNAiAt?wk!wy!yxp=+S!QYs0}W z4TOglMf7f=|MEW6NXQSNc^_IR0>^yaxfId`$c$VE8vKG6m!i#Pq-d5qAx+V2Mp|qm zFM;)^?a2fh&2orlvRmlU6z_|PX7Uv8?nouY7e#oUxa*^uc}0UicJm3f=N=nduD#G> zU#>MPajST+dLHXby=Q|_{+Gs>nuk96e%)5i!IoiGglm^nCh{vp5gT> zykIz+XZo0zJ@a;Y4LY`z85~3Q221iw>liHV&dTBS`(+q&hy7Cee<^;W4fsMIDaE^A zD5Fh<$9b0M?3h1B1@aUhSdWAoB*kxV)(|vxICX&7_WkAl1Gs?4_XsZoZbkemzR>Dk z@BR!w5HZXZIQ)6nc#1FjV{bCiSagce(3}lIN7zRY&DoiVT;NE!E z>>rH~OF|FYX#^aTWc3(QO+uqt9vQC2JBAi5ftY9}FIsjQ{-$Uds#b81*>ZCvQD?(U z?~LY`PkzW$ERP)cz`u%n26t=i+nR{`OB@{nPd@jHn$tDb%5w&s`cR{17!p;NZDg?n*K#2xC1}$1HnZ(7jAVj;OxNuC< zY~LIg^>*T?)Yu(r=4A#MIuCujSqm3b6cNnMjij#W(Y9+hj(X~7XvNHSLYZ?@y;`uI z1JCG(4OpTXiHyf>-|4NidBMA2K1#G;j0)sLi^3xpg%E9>lg36(k* zW)2l;uNdr-u$XVHcP}R>>2v2rm*N-4Sm=pS>bx69mT^g zY5yYf4{SS68_}ST@j%oV@15vl{M653K+7>OTxgvyT8_C*_;**J*JL?X!LY2x2Ta3N z@v-cr4nSSe2hm)d%>)AlVz^4zzdeZ%OF|DQ0HM)54{=Pa>-g?-M=;S$PPBuKzad&* zwL<0v%bR{%RMv^7JZ&?6)x%=R)3x+6bM9!@_qPkh%{JGE7I4W3Ci_cl=+(*zYO)#i zq!0fodM*7WJwdPSkUl+gx656x`uH;X)l6hW^8$@%v`)j>YSOTC4^a2YeRGs(AsBVZ zi57XD3elcKtc$zpm1CB3FDmlG++v}y&KWo7&zAbB9l_y)3tZ|wk}FVs2WNP~K=c?> zlDv?;hxA?TJ=&dG-}7fqA=;uQb4t%$mc#!U2S5e=uj49&!c{QV{yJ2ym%41={+Rpixn(+l^R2(Ov4e`T!YWuEQjQNtol5fVW^+Ltjj8{kVh-gTs)?J2MdZQVE3cP z$2ZBog~$cYqsDeqAnK+-H${yWHf@_40P9h}7l_tia|i-osyRU7*$7IsFpN9?N1_2q z26o*>iAFNov+LI>h{iQ@K?EplnQE$Hh5pH)gCT!?%`;!>L|b)2`RE+EX2Lqhyim&z|A>ZI5_-^1wE2i*>dd+63y|v^ z!$dPV(T)ZGhG;$23gve!ZutIMv{sy@ul;2W!}oz&Cx_HuD}L}i(F=2EolE?fc|Y~c zozcwI{egkK`oyO#%spdFhkTa#G|VbFdR<-QOscpbvHfM>1+M>yRy#qoUOZGu%n!Ra z*8Dk_0(F-jw?m0`9HTBd(UJ|R5bbHil5^Mh=S1D>i{vuu`(@x|@xtWn$&PQf-hA@&+${5D3SqS@jtqVN znT?xrv65>S|uY;3P}j{>Ho@{k;+v!@NF(0UJu`j-7UO2^$3;o!*lbdrwtl`EJ% zK)ts`=93q7Iu5+C_+j&)iLj7Yb2fw0zs(yEOF|FY>9_!KOlt5DI)fR7iDq&-o{{+t z9lNR(I-ak;KKx+OO0iC*ZkGndih>LNXJUqo+z zR1jS~Y$MpU{%ani?QnO9*sO1><%3G~maQ8ZU4*4$wJ~jUjHIXGi|at$ceiJubd1KR zOHRidja2AZ60zjc)qOb~_mp3pm|5cC>2SHIGR#Qb()fIE@z1;^uAP~g&3SdK@_q_k zT@Htz9E{R1%X61w7tfVTNI9mlfeNzC<0_nltANrGcnnI% z3=^+=M1+oE?F;vBR_f%j@8kP6C!Qd8;?dDzIqx0rTbS=0E$QZ2Xz5#P)+uGdr_pJt zOkk=SF>$Kd+~E@9|6=z*SPU5*$J;I@=$Ify!we=+VeS3#8O&1{70BuM*dB>)2pv~C zX!M!d#qIc3UipdVWLat6hLTHitrSw}k}+*7&%k;VU;v_>k6w!sjmC(| zuFgV<7K3pI?L>nj3P|E{UlU!EBN^@3EoF*ELK3196BZYfj5d8=1%x6Bh)VZy6}l$3 zXTrZ`pN%Iw1MMXbZznSA<8Tbu6Mi!#*?~I{ukxyqpz19kIJ@~Dq7X|$4=AFbPes5n z+t!<+YjQM1^Fm~J(t8fH-iyUVGda;R3V%bi_G(3i52n|K56xL2CgL%`@oisr=I)f* z}rklwzLHn56)T~wlo~~o-6q1l@VM=>weuN z+%Ky(a-uB++dlc z-UB1rNXjDGRJgfnDXPM0xC$uIfXARjyJE8O@hCzxpX#(EhLHPrdRO#lwQ9@aEv@$@ zjIpN+zGP;o7u)G^K<%+%)XEn7P>Ld*3|U_SXH(YkM}nIbe0i2@TKbc*5~Aeevx5DdT4;*0|DE^<9^erdBc zO#oD%HULt+DL$X7K#7)saR=>0gJvV3^_z|3UqZl5u`x3>8|OaPM*&DO8uu}I3ILM6 z`z|b`Rz?D#;|CWX7Oad@fNu??d0iZr-^~qV(Yqb2%n>HNU z4FuL}i&*UX|23r`mV_Qq079dA5#pHIS@iUv#y^9JX0q?xzQ}b%h!#rrMJTTwCaz^| zD@)s2OERqsl%M6?Nqf5_n!V$pq&C~N7=P~JOFgSyx%qB4`sc%qNjRP?%m3W$6kK#er@j__!4U_w4rk}?F-$K zU5uGgU88od76qEnk1i1IO;ucnAMw;<n0X0S_!p>q; zAV=7zRK&Xwgc;dA*l1}J&v1x=uDMrA$X#LX));+*%XCZvy^7Id7JfC2BgGruFZKfn zLtZ2PBf=h(zn&sAlu5wu#tH5yld=)Hz`0|I8wHa{i*V#cS@g6q30RMUwm>vX)^G@I zsooSj6-!Zsox`|;c7#C_J&|-GnggxWv%0BO*US84{ z%~i7Y4#)Oa&rZg@`6%VIMmyJ;aQn>IU3K+b!V)*_DqIi0_Y0l6Pp$d;1iIMY6m60u zA>70gkYH!Eh-H#SrkKORp5Ne3}k&6 zA6GWi1|FAOi#%kE5=ZekRj?lh`b7V(DZ5-kMp>AMzuj-N%hm29YMEC9)^N4H45mxj^go(-1@bm+24fg?O zG|`{O2u*vU2fs*x9B}FHx8fnlfx;9FuaZ57!t@?;J#fxob$}+A`b-;4p_vkhc6^T{ zt(g)Gf4f@}&6Fg~22%wos<78~$w#o6Qe@gRgTfhz>Qya|!r61e3CAzao`f?#KXDw+ zLPH!YsKYr#XZI;ft42a**7ZYm>i^)3SQ2_b;S5dj3W#IQrd&(gP#H7>w*j|n`R0iCx|wl2^L{>gHCx*BNI=*qTVgC@Z9U&XgJXJ3Zz>G( z&6Ichwc-0NOEyM&XfOmBEMolVO+wgs6*hX;?hgpFk3IG0!!>z(N`UQ`$I6L=d0NEdim~qu9WSyD=gBrH z?2PvF${gb#xihll#Rg8Z-1BZ5){;^k;eeFg@iE&ouDf<6Od zCE7vHOwDMCCZAEhk*v@mQcJ-%QYDajckW~*aBJJf*40V|bs4d>efuLAN2K%75QK93qy49<<|75pj(7 zX909l@(L!J$#MAAkU9>dtc&ihib_v?b22c|K2@5z_s)(up=8cHQBxV5t;<6_-LZht z=UHqwrDi{SBw5B)HOxy3n2!;=#;tP6PV2%+Wy0 zp{p4E$Z#B2`aj^uyr-jMRRCV!LMLkg==sXP#gjmL2-D) zMCD5lf|N;5Iw}{D3!LW(4O47ZNMcgr&Y7vCd6|XNmW_e+C};~ri*2BX&=v`6`0#r? zw^4-Mz_{aoL>Q1{>*K{}Gb71pT9@Q0ni)w5tFn0boMbfDX#+VFVL;TCh^TakiRh+; z0~BFkc+O6JwD%FjEI1!c(wOXhlsY=9fFf+6{<_~FY+cdeZ9+d92wt9?_fv!aAq=r3 z^q?JKOAtrc&-jK`bT={4OpdVrM(PMVYF(^6|7cq32G0YM=0Afj6!C(>ylZA<;DxMU=3U&3 z4|@-57I413%VkfrmevV;)BD=@jjq&@?IV66gY#JGD$2t^$fM;36o(Zkh=G>_QNx?}d9aw>;8d2J^g^cg@_)RXf8!&-B|a_VVXZZOW31fyIo1VG;Ll|O7 z=s`Qeln}>Ar+1(TyMu{lGK9Tk4yKMUZ|f(@?B3^6H!N|z=&4Pw@9EaW^z_&;aUi$$ zY}K-pck_4nGcV~BaPwH5ue~<3v-x{jWCgvhzEm0WilZ+Rc+))n*A4Ya=8z$5**lBT zKO$_t{iZ2-2zs{y{TvT!qX@f;(T^Nq;)YZZ_7XwZX}1SFrNi_hiAAdi6Hd;P-+<$h z@~m7*?_2PwpSdCC?WSnLv%}nn*z>N>|BB;t#Rn3YG_+^%O-@Q8RhYFY>}|H1%im~I zVfeZ(szNqg1%NQrMNougnh1`yBM4L96n(itp1#e+vEs;q=1vDXLn-mWLGF=&p?>DK z72M7GW@B_0gTC;|CC@Z?d-dq2xxB0N4R1>f+xkiD9$^&jezt^!FlA7Mfw#1cMJ!+P z(58akOI;LU_b@7uBWyuF63!5Wscn6@(Q$6kzAVMc#kSG#z8@f4wfNE8SIp|fc(AEdN_A`A?#o6nCT>;ZB;aK5XN zjV6Tcnzo{i>P=x1SBoMn2jdRf5e6-7fYyuCs~}upre{NdqF`w(z+Ts z@uH`Vc+e^gXvTWK9z~efgnu`k`Z$R&shA!m6k#t!*?zOGW>S-0DW~2@csI1*NQ~(} zgdvuM9<(EDDdL#)XdZMTF&7ifnC@fxtvLP!)G_|R(Nn-;Pb#DiRgW~ zUwKs(_{GM|f>Q*3?FbXG!`np@T6Pnc?UFv7+Lgxec@y1y*)B)DJ!8cThk05`NC-n( zLr{d({h_ODxAtRMf3kdj0)uWU&|he1xWs1x>YL<$^X$Qv)o1x zzZ7xR%al7*&^nhTWd?3XUCAo_=k{YQu^Rpnw9dlq=yIH5cOVi){s(s7Nk>O({s+T} z&sfp?{}8zzI6r=qa?%b~!fg7x`5%a8`Z58{|9Kd9{Ezt`TzWs(ds_297=F0tJ(~YX zMoauqNmKs6`*-s{5H%^GAI<*>!s!_Mf3kk``_jN-R_4}7a{q7*`4<# zQUCHkVo3Nwd;V8O9Fx;Fik2{sFwsn&|K}7^&;NGTPnTY`i%VHwxi0JE$HVx@g=fS2 zFMUsXml7jwR^WEre8wg2OZv06+3!nd2w2XQUN@9UY>?MG>p8;XC#YAUeLeUd4|^5^ zdH(Nz`{xoyz2VlBJOoXiK)*y!c{Kk&#^^_0!e|ClDPhXPZ&jVz@gTE0utUZ=V$jd? zWKI07p84wgiSO!SAD7~W+2R}H0*`(VSWN6c%kkbhYN0jm*mmOX_>o2;G7I+%dF2=^ zB*AClW`1s>O@-pLd8i5mxC$u3fQz6AyS1X9yUlL4sg%5j>81BC%p(;g=8Y^E4v6<( zU2@ILkL}J3Q{=-U43!0btC=_EbH1<8FqHgu1E;d2?W$ykVnM6;(Mm@)C40GGzW+*? zjmWkfND1>q#6qluHWi|!$6q`4H>s0;uXV2f*e!7@yZqdiUx3x!44=i zpmi#6%#pk+v_7YR;ms!3(dQIWG%L>Wq3JoreA=ECvZNTJ zA2|+J$xy*zMfl08NLMYMcSVabjJ+bCN{B12afr?Nd7wyg`&@rh(>|H}d9OWw7#_~w z%o)Y1r0DTriFTAaXSCvy-FD;g`6}$b{+Ax|cI#$6xMvoj}VSg84K(tlIt5AfMVBA4F!l0QFTv|216p|^S z!~?@ai{7Ke^O-POoBP{I;^jM*6YUY=<>UOsf3wmPROV(-KeLgLAt$BlrZA(z>Pxbq|bFvb69TmvN^-|8+;;AkNO_t=aE4+^Bu! z^XJS1G#U(%fMg!32DtTlA!8h(Y#GJ?tD zMPsTe;OG{iDm;U$fHDsF5X!haEBfcQAdKVb2prIi%`WWY{azXLfw@A#<51Jq(8kkW z7Qd!5^&WgWc4f$y)2ruh%G)avxG|r|y$sT<+?@4Q+ZjTS3wh$L{lQz!=-Xh*pC(E^ zW04cY8?>pg?93v@qlg!tV^kn#+)jOD0EBT1HfyU`S|tk~TVQ-#;Nm)$%+U~k`r-l( zKKG}E#N;N!7svzH+j+5^VAgVukb!?c9=XnMj_rD}Q-lV$IK^!aBJI@%BNSSXkpbYm zEP4h7v`Am;BJZS#_`A>oqUoC-L!tEoV_({##Z1i#XSLJOWJj_>GX4}rvV%1PUZ1w- zbto`_6&Cqtpujvh;V`b+^OL~DHKiz{-F@%GT;@@4jflTC{FMo5jhOwxH|EX1)(FIq z@B<1==z|Y%jBv(%$b%0w;i)0RuinX{1JHVjiDq(WS)8X1t#G9aJ_lQ51x|-8*!o;J z-frl2yz#u3X1LvR(yNBA=C=vSHw zt*vW&@o*d9aKeqMQ&I_4jke}+qLtPwj7H=X+TlorLa#YITaItn%B(f%&Q3QTw)R%5 ztIL1$R>-&V_61Y=ts!hyVd^_mvRQPSoCo*0H58h?6VpzTVE!B}cDVydieTcAI)$zk zE~8C_&#qjk3gvJWPznL3Kq+*0#k<7~2!&Yc70MXfpB-X7z<+bu#R&aje*OYA!+3xH z5hh|Yu}7*wzmCb3kwr>T#>#->1>^m@JfAA`iS?_?g6xGyd93>Tdmt35%u1k5g_CtW zXr)zwQGuL72kQ}MLnt(FllDeSs}#l{CQWYh?`?UbyHf&i&N#YAAJ*E=FE@$~jP*4M z51mCRgiJjCkwTtyQ;T#U5(S08&9H1(2lBiFg%%jj5xN|O76G{)IG=xAodQ~01>?`4 zoBhu3nQVBsn6`9H^`;=sbwQz3iE#(*(1Io$pmj*2DXnecV0bc@1KJjTXu@ck0$V5B z!g=?xqYVlAb8c<_4LB@Qcsnl~ZX`JAZpphU`mfSLED1fJ;DA1S0LRRVaYEljAVo8a z)k=yt5lEGmeq8f2(whjafAd=Z&JU^)MNm@WuS}{y_ zR{(R~V#}GAS(oyA)(T!fys6abfkQa^;pe>E$D-~w5OoVj=ilVn$hyi@;>(?3HqY-> zr1+Q_+-PtTZ?Pf2hce!oU1hm$1tJYa_ z!Ct$1*?5dL6tAIiaI0Xu=>=o}+>+EK)CB7siNv;Z1NNUoEWl`iSc-nJ~ zz&*CAd1jf`Sb$6W5kie>;1g%DlO7)8^+GT3EjMSLH`&h@Sm+k0d8X-3=1Fvy-tp3@ zYO-`%u_l~06?9w28$PNqDv(3#7#_LTAkdPt%8RqznCf|`PUF0u%b+0~~Yf4FlxC2O7;9$Z-2r zjVQFLG0{v8t$mHup><(l+Q87KS^PW|mQXvA5ymu+a4Nh$c=h0Zg$BIK66qG7?p%tSTBW7Hi=upwI$NfkNxv zig(LD*p~TQ$x3SyT2g(RxoK0OaTga_X}!m&Kn|@lQb@c* zpe1XWSA{?;`KX44&{yAguAS>H95K3N%pFs~B^7lbd!&35-QH!Q721fwk#~iS|JbFz zBEyh2p;yh{g|>KnzzF@D77r)@K^Qh}9$)Y(Ky(D>FRxKw@M5}VJh?-C-n8X%pcDlF zfoNH{<&XlCYIM@Ct3v_!5o4wQ5r9CFiVHtb0FsQh-2E#B05j@4(M|N$+SxVLxO3_ZXv6ME^N}H`kr7SH-7(}#4rH~fdRf-nv zHMS@cl@f}v6hhf|=6BC!yzg9R-tOn|e(3kdxBGa^`~AM=%zeMM`+A<&Ip>}m{H+c$ zq6Ot8XrAuai;hfs4}`jHGH@R@W->s{!5)iWWjo8(yZgL7;P4}GSLR`5RnL^h=Vx2o;z zCduKm7|_UKsR#&uZ7et?49t4@t9ItrQDqNqJac`t zPb+8pizOn3w2B?Hb3I*Fx;4w}{Hdh7q9Xg^bR-i70GJB^xCB2_8t?K4e3?b~4vGajDN&AE#kUY7WHZ)rCat!eK8 z07PCoHY@=1a#%;Hq0j=MlXag99Xm`#kWYx5Usn2 zizu`}^U*W=H_Ir)xq~>gpd2Z_U30`D*0v*v}+PHAa)*&NW$tpBoP#Ien z+{<7xZqJC>7z!<**&fqaw8--r2nTD0bAzLzl-W)fvk+)ek~uERu!q*>?Z-&#X>?lN zCiQcErnQF5(oJ{BHQOLm$K`>e0tb&FD)wyw)k=4*yw zY`kyPl2KfCYbsK-qj+{SaLH1Ht4k(QJ>UJ1p(Wct4ri&_lF(<1WuFijEd_ShrYWO# z|Bh1u3$4d?h-o0uQe34!%VxzT&$#+%#VbOFqnfqNj`!7U@4m89d|xuJl!5vWVUduz zV$pGa%9s5&)s+xTr%&0GdLZZ?c_)GA<2i0Di7{OL55j7>scvx3MlK zJ5iszyhJOlA2@dqhZZ!o1FdiOgroI{?|>CLWWxTbNA{n6S5HRj5y5$5szoVC3+SY$@%f+{s_B+Ww|?sF9FAGSS77MWEdS;26e>2($!dhUXJo>W~uH`GZ-74KWxhLMMWXUc}wsTWzlnUAzAg!f6eLd-oL{Il5@kYM+3J9_Qw05T< z(2{0_7Fpp1F`>Nf7&InWa8`I z)KLcmgs1j>>`UvYax75u)mgo7VO8~k%jx9uan zkIFY`>VCX#VPRoVaE+6$Bw(e{53S*!CNqd&okap74WSUY8Dw>9i9P`anisj(q0?#U z$o0Uv`kwEE3bUvYD;n6|2D?qqpwOztxq~>gpa}6{&@ToL@#O5HOI0m8yois;rOdas=p=k>TaV&QAx6m zoV2el_sm8uVY-(1F%?&nCq+52`xiaS^SI1)Mds}xMXg!Snm7OUwv=*@KnSFa6)%h?Ims2s& z!c|(`tA7v^`pV7aD71dzgvLS(I0Xu=r)Ck(9}#F}TK{^^H8Yd9*Q$s5X~u2;x4RQI z*@o0?4tP7K-Cy+mVakU!T(^6B_#9i?4K37sGN;OablvmWXweg>2oi;UW!%Rs+r*#GwVH5HM1bd;hjfOrx!n3m-zE-Rdjc z0EIdqxej*d12>P8fip>x0&1q~GjTnSho2}{R)vd!04=%Et!IFodcPr2@(qb(NttOm)SZIAt zg+t3hW5KL4PX=cO?-g6geSKZ$+`e=O(#T!HLL6L{T&q)d-tJD%{2gj|a=PyVXP$;d zyfWIOAC_W<2XU2_WfOtjbAix{Pp?Ix)r=Dw3oYOjD72oLMYzAW8#m>V@ttEn=eqO7 zs#VWM$;@z$eqnB`mwsJHT`sb13?sYE(5bLbvP(2v+f=1wZDMDuZOwDuI8RFG=723B zfbH9fP6aHqswhYRL!c$ILjRM^vMYYVJ%+bsD(UiEQLo~t z`Miv!1z$#m(It~-DT^Glob*M$ztTz>b|s?tvQ}b3uRgX7UDVNv6Z&5kb%2&br~-Vr zmjR;+l$W5n#oh5JFEfzqfpdv#V+iq5`_EQwK(zU1{CK7yVz%L|L>z!ns(?#h7<(8p zKL(`=Xr5_x2Ay;Q_XRi~<$r2$(#dc3oU%$Xh2$3_6u6l^RZ`qT#;x4;l{RZ^;O4{e zf2o345_&+XLPnZGgWqLbG&*s~jOK-!K|>}kOSXBtf{sj@%-6w=%=3*S)scY$!Vrm> z35nlP0Jh_z84JMom)HZa%I4j)nu5(|EfX8V&8bDHq2y?()GUb*)#}qtQ##b|p4zA` zWZ_s4MqO`_re!VUD)=fgajK`^`%LahLaOpEQen@7#T#3ssYtQ~^JA?D0NYm94-3Hd zJf7im8qhDVSd1$e@lyv*KP&)$IKTn81p(muJ&g7#4Nkx4r59h7HJY@iKP@|#x~z3; zWPU9*{@uYQJ$VyZ<0)l)Hk>C^fFxB?6{;;Ob2b>oN$Jw;4{3p|M3i4z(N zK;RT80H2#h9Lzxgn4sb7Aj{owev?zyopoYQCT_h`Yd1F8c2{}Tn3g1~jP*7=(H;Bw zE0iKnS}SjvK~~+^7T5lDvLAOR$@EiGyiAVMUI+l4CQK%#g6v}n)b3q46|ewoqa!*( z061#7!6=($SL@X9N3!EQSXd5zi2z!co<%m%TSZ_#*0iqRE5MI;)np4iip;cQq&K<;| zMP?_-7SMR^r?)CDF|OlC5_7z*z@&p*W#P3mpPuVITGwoiV&>WwR-_Hf}$-{R1| zL={2AWgh=ABLb~|hxhM!!?c2QcJ`2QthXBHw}Xs`U7PJ``$sb(5Vbe|8_|pinkTh? zL&vcWAl5?tODP&0$MVbC>TrQfA(7+*4w2bs#2%}Z*&ox3X)b}H@{jZXW<+M(;r~rM zBN`!&DWAtcR}64~+Y)nNyYUU>?7Q&=Ho12X(KbZqX_ef1IYV!f0JoGtixQ_dH}7%- z+IS(ejmMt1a(ygM%{??G)ny64Q*K+0tH@oyNrT|eyP zi_ZRSEO`iew*vi^($}NOlM|;OCga=jT!Lr(9g|=0P1(VS=`dCjX}>jRLwsXlSKFhA z?kOS)#-5(blQp7xEiGD~nO!n72ua<)Lr+H5b?q}-OvWd(t{6BLA4p8-X`vg?aV##J z(AaJ~a0;{=|I*Ao;tkS`k7)VD|Mk;TBf-P6t@QorDdcr@ZT#W!Oi@v% zuXjnbz&!1#HKjh*eSQLBC$&uSY+4+>IRi=B9bA`^u-*6#5i3@;o5(sj6Bs`Pc6UPx z8$eoaoC=t3e2%md(f|T2QOlQmZI)f<4HYz)d+SAwss_m<%`g2pmsp)bKJ`gTs%dkB zdi9uh1CuYqk7GTvBCz2e5W4g&;SKjd^Vu5x&mX-()Bxw_1&2O=Wb*O#d62Gk7Z+V% zt|J<`=a1Ol2Dz8sq0r*Nxq~>gpnYSYwe{2-2noo*_sGY^Ri9HB`S+e}J2&ft<~u@i z&|W{&XcpHphcHg$m+V$xH0*8zbsI*CanNo&5VcdP1cg@MfPWiK77juyFW^Mm0WyW; zmwLeOU!cV!D-g9}Z87bg^Q~h_zy3lCu_W{$4lQHEF-`~eaot0%L&ilj23qe(0qmib zX_I$%Tf&-XOR@LbcS3tg9tSsyyv_3pY>~ce#JAr&U~ym+zy1E|9bqx4eWG6Nb(eE5 zHuM#mO3rKLrii8F`o16~{oqXSWr5b`@54eXSW<2H{15a?cvi&)k|!@tKPzenZX$;)la7LMnwKhpR~ z^wsUserWAZ34Lru9>wAzy8t(0LURaKqhn8eIH57nLY)GI7R_w+g-irm$I3Q*yRK^? z8KX?myu5$Wlh3%8ye?9v)M6o zU)h@~?%)f#qyB-`zEkNmdHvQR33y3C|b{&P50L~r%5nABVAr{FHXhBH; znkR2hK}iraV6=&so(+;9c~x|q6&WFcZr0pv5em`*dX!F#rkjTp(*n6?>dvY7O9H5o zkUv660PRfx$CT=4puGuZG_U@3V@PkpBH`3#(3{vUb@DB<3ub9EQoRXi7t90^)tIyY z)rEq%XvRWIOoctPXg2Sssb61pCO0qlm2Y+LDKlXa!SUfumn&zBnylcc?bYpa?oDq0 zrR4JPWJ=;_vU7{trr*EGw0zR)^ap1b! zC2Ca7Nu~2&33Qbeik8(%Rs089tNU1?CH_rm_>2#Pei%6!t+a%3LSvx?oC1Ybvf1i8 zX?9nHeKUIRrd;K!-IY{jw?!}S!nOKsW|6sTj`5Fm{4Hsg@G;qCPk@SB4G z4}J~$85(x$rGKSpaEhAnhc9z^|5-7>d|9E9tbd#ZF%_zwOh)Z4f>Qwttyy-+odtoG zpoPIF1X^W%$x0^47UKCLjhRJy64ni7m8v#Mi{%eo`_SDYuNT(SKRieqcEy0c#zA62 zKj8C1R}6^ag#KSw3;-W4Qgb2-K+xRP+X)3=HewQRe&Y8gLIC{3LybVRn_H7m0E*$P zL>z!nqJc|Sz1fcv?cjhFoHy(qBpTiQ+^d;n3W+33y?vcM(Q15eT)wfjnD+2YRrj-> ze~E@z5_&+11}&$7V{V5ALwY7;BflVjBSfPlX%4 zV*>Epp9P>PB5J&_Cc0uk92d=404k=k2jC-{57YP~Ezaa7W~}&a`qF&wsanZCyY2M+ zL(cjaI@UkdaJ6h2x32m7@0{zs%g(p7^l)e=m#$7bZ7k5>Sfr3JhSPV7XGiGE0RVFF zFac-@_1fSc!|EG`&uKuv@O978L@a^R4-3GV0dN3%PF|37(U#Qf+|x|UEHO--vR9Yn zpW;Z#N$I|0SkLHdnAP`PxkrS0@8QcVf4`Q}DksHrJTwdd+gJe@piEF`%7;`GfRZ?& zu>b^4fdVkaZ1uww1c2`GJA>9sX=jkviJVh6U2Wmc5V-1jM1D_Mt$6)Smz!ENf$W~J zJRT`KWINVag9CzR+fPT9me{Y?;<E~SmyVsRtPX!PaRwviV!>UN}5r95>YS1q!BO|)wJzLIr_ z7h1J}U@3efK@?h``K1sU6k5nW>?Ck5xmb7zv|#gN5B_Y`21L6Sd;^8nD4aWpLkpT4 z!KEM9i4tAr2Accb5JRWo!B7e~Kc6QwxXdltTW*>v8L8S_wrr!ZXB>Ui;V1KrztYUF zzmJ;w@Gs-QZ3H`@jDtc8I6_!e3SH*LjOH~)y+feItlDgvZw^9>d&K5BIT58myt43h z9wMq;bTK+VCXI_`EVMKo*hA~O&4$h~2>I+UkcvagI*GhOeeX0u;39$MdqftKe9q2ZG! z&@U*X8(k40gVPTSEmIXZw7e(1{&Zp;sj>chi{%*8jIc}X^SPcTbB^J1YnX19EMhd> z+e;4FI6=vw$^OVLAFq5)Q$Jp(81~RwWlW&d1VZQ5baR|Wyd{eh8VfDp6ezS(&1S!( zA<)`#>b&)}K;dg=7_+|B8GaQrv%7g<&B5ZcFSTFPQ6616%yDh~!9D@mr0T8l)LWg@ zxrezgx=IPt^H&}SFc{;@=lWm}TC+b53^kg_s&5x0W_Q)4f+)1)a4KM-wK5jb5dtk9 zbK0ye<~Mu%TGf{H&}Rp^RXyvVIh6Mvww%~)`8D^2KsRspGg!N7pcKkMz65?4dtf443SoDg9l3oSpnL_Qsh`(?@RjXtzVBrJ-bJHv?jQ~= zXtD#6{Oqtnd+1D~opLZ3lI$+s{*6v}VSDJwBbMx-MIjJ%roTN}6ow4=x0Gf*SQI8+ zm_7pq=I!i)U+jUo;_#{m0zbaeYRfwLmWBQWCSpnG0R<+s;{zNMvS|yXD1=7ye5Cmd zuN!D`k;g?dHmR>jg(r1CB&i>=BK>fG$Dn_?<+ed<-$mZ$>~kGDQ!1a%IjL#5cY2Wd z!%>M}BxfWqaqr@289PCHR}*IbXf>`9Is5qmOLAc=wD&|AbpB{8PH1dWH$;97P3mc8 zv){cylDf@);mLMQ#s}(eejA@K*+E@y`I0gn&hwmC$UA?2io8%nG9J&db6VXRmq&9I z9$nkN%~brZ(t;&^OT0N8N;p@?3dsx(#bJ{AB7;}NR4DY7L+6haa4KMv`X&kz50IqJ zWp224i}|f>>ABC|d#m-8L$8qraUxYG_^%D|H+ijJNqV+k<#L0TCNfM^=t))>?fW|`bNuD zzEM4rY->a` zshc6q6DRkfm5359nz5D0(o5_skuaNltwSCrQMqCl`|LK@`TNh{G~?I*Xm&O7NPN)Q z2Z#72ghkg?^SEnC)=;!dNGCc3o5B+J7!x+`dRyPwMS?F1+M} z$IstVa*N_Nxu!h6LJHL5JY1D_R8ja7`G|gp)WnjThL}p^7whoZSj)Y{gr2J3|FU}( zoX}Wk0jEHrm2Nis^AiMGtL^j{W9%O}am?f1;nq_vS8MXyQ0}Guw!nPlnjKbWX1y$N z-d|!v;&SCFH%dFk8}&=!4axf!Exn#vP+0-x$MF1t9V_PLfe-!bfS zLFbQzrS*N3q3J?H_&F*KSNAdab6_z8v9cw97Np=D#t9$LXR`H9;c z4WpLkMTL@HC>w<(D{d>FzxZgx(x1Qb%hdwOd^}uPdV;QglRu<={UWN|T-jxLQNZ|| zRNEJ7eGqA-6vsF(?rookKx+Y1x4}QQY=|B{c>?|9t_z^aa{^93EVOnb)`2}xba2wb zQEuiOqUV%Xb2jhu?MS0mNd{G(EEz8*T%J7pyFz^Cj;qt&H`{z0UQNuhQ*F_H? z@HnCfFI5N4C5nwu0Olbk0q1Kv=MVy5@}C7D5H0jgJUV|g31=nZ03;(yf*-SV1Hh%d zUeZvaF|9zVel~(L_v z{$l z?n*MrW%xDp-dkcoPB;<%(q?YsCm{(cO=TOkB7w2xArpYY=%R<=tF|8>x`)qcK)*OY z9W)VZ;Pk@+FyIm#fQJzPZd=F^s3UOVloBSP9-qrj%zD9G&Ef3_E zJRY~v?!+AGrHj9W7!N}gnE*UJ1b}yQ+ldM7|40V~;1rzDSO5a2KmnLxCjBc30ieP3 z_1FB&jAm3{`Z%ZkRts+^WA^CMq`G$F-?=}csG&7+S&D*p9Qebkj)2v`tA_^@{ zoI8j^3rYeY$?*?2AS58Olcc=jCdxRbjir393}GCzYQwIGZW*y%6`;@pnr$wnq0l-y z5Dp3gPX?i7XC7i~L8g#Mf)S##*+a{9k)3#B_gC7s!c$pk!GEEJSQ2^=ht?v*F+5@- z=t?~;Tr^{$UWSZjZ-_xQ}~ng`aZn;S{cYjfBC3a!}p1h&Zm`pN8Hj6!P~ zPCqQPP8!3Zbxi%uEc##79aPm3J%QGG1NM;BLbA5&WVAo<|*fT>#i)34_j z&{pvEXo>Jk%c|U^v4_@E3x1a52~9CTX!luLP-soZ35|soa0(P!nP$>$_Yr7K6@R;L z^y7yL-fOyIo<8y-*^3p)rZnEYFh~FP_bJyy^lg$9_v@&4ON%yXysSFpt(HaBvL3yl zZ|b*>x=mwiwY;_u4mI{yZ2($I{P*jLsql5x7S!%Da4KM-btxGMU`@P7-FZ5+=^x}qz=<`*hq=$#r__ecGgocs;*!|p= zJjkRI&;Wc`7zdj3`{$w4X&;a;!FhB-_K@i`*ifV2h%t_BL><%khC*v5&K>^|T0raL zsbvsoK}i6b%XR%iNf0_}WI0UUFALL=InU`F%lxZ)wb3Fd}$>MNvknPb3#4kJ}DL!)^yB5IL) z|4tUOaM6r~R;UVlXgS&xXx5lbi(IPZr(0H1aj<(&TXB8l1Kq9D_;NBg`z3L2CvzxS zjty7Zv)(uBWmn;W1{2X;Z(dZ0@V*cKdVxgK=-t@b>Nq&mNVL+@mg8l~Lu~aUtDZuW z=WLvQSZG}=hePY6`kPg2j5v2vzezp}PT<}6Rls`kUA2w1ndY_0ig)IfWIycPTH7`@ zGRW$}D6;3v)=D+nu^uXWX#JW@fV99VhqF(i(3*o28VfDp6ezS_&u{BXuve|0z^So> zM{}{jaUm75&wJ9cnVh~8uI6|IO%kM5q_hVdF(v9v<#V5Nl)HKNe+on}MK?#bq-QM+s7RKP;(o*i&Tkg|8n!i1x{ zM=jkZ7P0l>uEGu*|BW#`p?b4blf#d27}EvMNG4|tEb15@TAd$RcVq=3fe#CXfZfF} zcAymch z;-VP~t#hgDp=E1Rknm>Cl!$j?zHgf-+=~zMpW@8O(>T6{Z>y`^+13U!U7$76PxX=A z(O<@{RDRd9Z+6>l^fJ0y%=tc#Bh@AJg75a!O&lJ$7^7s1Fw>|Q!wHMFoxbnV2SbJ6l zsbFT1g+F7SO!rP(7HIilU8McXd-#kGg!VkV426~+PG~H&fK%Ym%4iq4ZGX95u5zXV zEsok=k@sd7rShNx`RKU7L;QMm@`XE8Rwa5D#WdKGCTNxiUk__kVq6G3G=&~odf=sz z(sAzlQ)=JD+^zhl(i$8dR0>NX6H~!Zq5s_%`ZyJ^&_XnTLaTW}a?+NCiTl1ZDAv-yz>WKQ=tH)VD~n2o0^YfZgM_TR}(wGyoqK z#)0OGldRCu#C+sSaPB_Yg6(J`mT^b_Y^4Q6Gj%OTp=E$`$A5$txO5Az0s1Tr(+ZTY zIzzUMUU>7W3xF0+gWV@4v^I^{8d}TJa9- zp=Dw7CE<{!dW3o2w)N$g#!F~lY@DgYd!>D0*c+7{4qLMYBcudGqtwe{uSWDq@9N-N zqH-f)rLxb~eZBAFC;U3Tt}pe6ji8$|E3{gNh1R7f_AGgb6EVe*!~*oAG$^3ZqTuwy zLMufD4y`BzT1Im?rSmnD3O#+rIdUa!^te_>Pn_{~)F{st(G2Uk2g-y*`xJldq@Ho` zFXJBVnCIBZoYOKw=Cpo{^255wQZ<&C(DsKEQD_HZu zj6*=g9*JiQ%x?`sOZ%TnOY}XJmwPR@9Z03sXqMc%W#L^%N>;h&dB;fp3qAObfVplcB|5*S6(X{e$zTZwBQsgm5w5)$U`@-a9!-yFTIw}f8eJ)vu zR&B=z{Hs0T<6zb1I)2VvO8`Kz^XLBM7!vZib6%N32YHv>K%-}q7TyBDs)%W)IOU}rddGFo3=I3<&Y^4b<{xNP0 z#r-|)RguyP>p6V)Fj}J`^Pl}Q)c6<=z#nbbi3x2rb`lCeGn~*^00O5#0hpChpl|~L zAjiGkX`7Lj{7TF`6k4AVHNd&hH$Aq{!d7kc5zD_wObkT@5KWZxH43eTICuObm-RMj2iOG)3Hl0KxIPMd6VM06B&C5JMnS+j8d>|KckMLwH% zx0-5xS|&QH<@xW-=Az$?AugSBlXq0@N471-*!@rEp8Mk2Hm?FM; zd@5(DN2Pu3b?HfrphyoKw7{YciL^#yD#ZHt+uaWw(5jtpw947+Ucfj~ zy0^)*m%@2v^ap-pHj#FO7#52@+3{RHn(MwPSLnc^4*g-PHiHOB7NLPi;KNb{fMBt9 z>_O+p3Xm_s`Pd2VgyzRaZtM`sS0I|`1;X=Vpn2_`Dim5vaqb`vEiyZ!Z8~-gr4Z9- z^3y$rP>8vFUSm`XH80JJ}$dp=b3MT8Jf~2XSawB8~~R;6R5OEpX9{g;udK zduVCe6eaNUjyY|vwM^{edq)@U{d{d2N+R0wI<<{XJg>c{cG&VByP}stX;kMBUcpuI zdTK)Agin6=%|%LgNS8NczM*M;?}@zdze0;=dksq-fkbR8od+segocs;1WTyx1frD|Xdd1+ z8LhOwAZmc~v=rqbl@@Fz;+{WSX#vrqP3lo-t-!hCKSB#!`nk#mbbgF!1pzbbA@gI* z@xHR7w+5m0b>|<277*3(_y(f$W1#t|l{?V+vG4)^wy)eeI6o$)IOd)NsI<;lR{Tq) zwa@vW{l?#Pn%5D#Eg2L3LJP4Z^dJtcWr$;DY?nu&wGtQ2SZMt$XAiB3cExw988WAG z#4Ik!&;Fu&Utx<5Px!${!s6zVsor{z^ndN|k=U3vK~vy{UQ$L=&{4k8E;6sYJw*7w zTk0*gh}fv#BG?|M!i3hSHd{iK7Ig=KZH|C`w%g6o(C5vIEVDy?4m~Qb z%w&-g$N)Ypr4=x=AEgj2tqPGZ!TATLPlQUVDYrTuI}=;7#elv36} zZ_GiQQqXz@NTTd6MKtw;=I>M|psD{9A_X{)3{o0Q{jL?Z3LKzbxm+l*mVLeQ=C;7^ z(o8z7evMJNiQ(VWkMSqVe?n6~8EFCy{!Mf>w70wnep78>E59<=g$P5d`&9>RC8poy+2^m};G6Cx zpZ|JSgh9$#Z`E&&{FZCSrfePW_i_rY(f+ePeQ3X#%h@MeRTtJ3!8c{OTt@8Lv$i*KsDjmU zGx)ZjFI}kGo3Nrm%`sKYzGF3?gz>24t!t9oAz8el=>{h6p1`ui0S+Ef-aAELa0SIV*af<+2w-g~?f^$i??jZny)h$sY1`xL4xJWe| zC4vpkO8=1vKx;=j8A1dyJEJKT^P-7|X*9>L977Th^VPwOpKhhh#FOtbQi%sT*akGS zye5sp3+&(w&R-=+48kk%vq(r9Hv+FI3p*v)!z-%7o&F%4PE$JFv_i@DFT4;-LJugs zpwYYnaYRP>FLbjeTU<0_>Xuvq74~(DsNJ`zA*G_HmLzIr=<~=GxH{S8X`l2+{w*u2 zwd=`*P{+~PJjy0{f{F%u-h8gV^VD=D?KG-@HdqLY^$E^x7o&v3YlY@MmSM^Epw%tJHhtL@!P`C84)-8oEf`|#7D>ND< zo;^-z4DnE>KRW~-jn6hS_2zwd%xeAmo8yHeU)c|$#iC0^@20;|tK zbdV&KiH@#;=FdJTqm(K}dyDFsBkc`_BH)JB~BiBpP^++2OTVi^-V)ovq&9cYOKM7=Bek?52VXr72Pcj6$CA_n|x z!uf7+N+{3fylpHuLa9|*R^9CPLT_0&Z?wiCIxTg=)3?5x|56IEB=jIosg;OhW{+z` zDYXd~%~(pwrLw0KuidvhTMGok-@0mA*YAwJPLuSVC^5Fdg0ARtFPnOnk9xeTGhX-e zF-7YI)O+rIo~BZ)#%GJN*M z_qVv0K!luE-X>GSeoBQSltNzq>xXsGIGk#iNPIdD1HIzqo@$a2CKFmB>Df7l%`iHfJG^M7ATgaPK)WNTBZ~vJrp(605 zL8FdT8+D*`Ma7rQlgcvKx@<%Llu$jDiS{y2{8zhQii}dLn-AWh)~LT z?Q`lyvzLV%-^yiI?zT9Q7Hi)@-Dz7nAx=|kSW1N)lV_0<$N)Yp2^U01bi5&oov+B3 z;QX9{?hx!SAMC}h`ToPBYe2NfyGtNI>Xk28$y*%Du=)ZUj4K-Fm$2i=)CC z6Ng$aI<4&YG_sWQU+f^3gdR}rK)VmXF@m+5A@w*knpYvs*RSeEDYXq3%~(pQJFunH zt9HARsS7fB!{2Ht*M@%X-j_o=>r;DgYxR4B_rl}1h;c>pP7%?$^-VEsSLlHaQ-xJE zf9Yy{fyqI+q3owreJbZ?Vphq335EdP67$Z@}0>ViKA zQU@pnUQsW?N~xZ01h#SjPPr?mj8e)8Cp4B)z$s8lz0Zi;5{XdiuFUj+62YGA(@j$< z<=5*M27DZI@nV|8kx4QUqdbl;TpM%vc;B%WsU$V)Yq8GD0!$@j=N@-(qi^2Jk?~ZZ zR(t9WD^^Oa0Q>$up)Mk3_t_JaQM>QJsW4#o`4nUhXlX1G?~sKH#m2^`olPEv&rnr8 z6TAy4%}Kr1oRkiJ&PVUkb9Y&iTrcuz7F7G}V1$8J)DORKLEsVVD~h4>EWqwA+VT(* z01d#0g>j&{@NRjOao>fDo!k382`re`REPd|m`R&!ZvLmUpr^onxJ!tUQ$AvR{Vg*e!RFJ`U zygToDW(6Z>KGj^lQEFX>T0%?<^@A=$ne!iL*%KN7eE4+A@W~VCmlUOeCQlcfepqOk zr^2E2bmF3wWun|}eLpz*_Ny3-Vb^4lAn%U3==em8ep z&8HisyVHKH5qeEM#F(>7_xl4WUXsaqo5@EyC2Fcq$;2tgcvbtlJAJ>T7$v4yx{q<^ zQxm6ER@NgHXo1aY=gF#R6cSTm3upgoJ2#vPSZLW$kQ)dBtwJN?BxjSyq&I!572=-L zPbY*-+(T{AX5{gSTjV{rHkv%K&mvaGDV`{_#%BLu5gLdDJ}eXh!IG7sfKsRg`4XHv zos%U*q3RK%5R|V#w6q%7!a%mSxm){v6k5A*?)Z<;0+N_LTMOwWliArg@!MLoFk{+S zLU{F%!c63sEc$LlakXXNnYr<+`$*+RXki94YrO7+7G`G#!ohZ(<6vQC=^eUf0XKz2 za=sb(FXMqxUpOMiJ)qMr?9b=5U;LM^h$W#1l&{cewn7{eDqD$GTDx)4jD^;MOYEUl zWmkH;`cCi3>^vC>=Xib9BT5lEib8Ak?w>d%{xzWI$>-g-+c$Lo+!}WMZoq?00@OU^ z2&I=HCK5_Q%CTcB6P+`BqbGDx9wPaXv7eF5F{+Iiv}WnEzA2ooek*juP-yMJ>4$~Z z1_wB_ULw#c6y*L{CThCa1VKUaO1hzvhmqAkbm|6IZ^=3O3RZZq|# zPHkQL9brXgr8N{qEeTQ&Oa5o8!tEvEsNLOhDqx|tla2&11X=}@)K+JcC(cfVr!T+ee%x#> zZgjHudQaPh_K#n+$j(MTcw5})0yam9LMx_%gP72Z9*d*PHau`b|I4xs&=P!D00?d@ zwVE1qAzvx-B{=7h`$1?O#_K-|Kp>ju?RugM`9SmYg^?%#J#khd4nQc;z@_bCi_qRa z(`cm|-Vf>R@0)7u2)1Lw5>3i;q=*JBFoCG)TZ@Snn4md*-cPi^j2;LFlG*pc0@E%@ zX&(TfpM6b11^WrHYfGh^XCeUnacjzItG@t53<*Do1JD|AOi4ojvWlOF}(ew@%)00O5#0hpiRaWD)4;PFq( zI$oU9trWjCe(L7rysdn(s>SOb=KJm9loy|1tu1Njyv%oOpV*oPGi{?&GUQ?v64jC` z@a4z*-PC)exg@SQSk@6NCESZRU+r0SiDMU~vclKN_aZayEJ@&p-34TE)^Q z%T&%hyJxcALD)4)+KFL#iu0nKK%`8uHQ}nw-JNw@8d?g0VDYtXhmZhh06r{?1I@pL zw4;opBVU4Z5%DHMj2pRqa-sbZAex_NHVQ2toI8j^3rYfTX|1)-AtWHPGg{%f{zV^5 zqmhpdUGy=v}yyIh1TSt&^kNd-;aW?2cbntFQy;kMxe#X z;rM_(wCXRNPya54RBfBy9=R9)w`xNy2|b8IYYpO0dhLD9~TbhxVho5i;vV__fOO5IQLt`{d6 zpS#Ox5K|LK67uq%e7oUb7Av%Rhh1xE5HxN0+R?@MrKrBBE@ zmBPg%mZ9qLs9@8bcV*iyRo{~>FMKsuq{ee@W#9Yg?onoS?_O*5(9>pg(Tn2mQ{6a{~n&ht@dSgGF|z@ z6O(&atB&S5h~BrJzMw=*w&SX5C8a&$V{+_mE@406mN=cY2IfCHxB67A)r_4qhox$B z2M_kj9;#bTOobc@H)?l(oC;WIg(@LBLZJ2D&}0D}Q*+s)8-6}ZpNnRV-KHD@RtaW^I-|gQWnflM9+M*v!ZB;yZ;iUt$*CJkY zhh?!#YU@S8J11G7wR(7HU0nWvB@eOHj}jq=R$75L{jktFY7B?g>j{faJ?`b-wQJ{1!@^ZE9KU4nzVUmirb6t!-w3&X1KNUxM@4tocLc$6$|XJ^r)9 zgFrMVw`Gv^Cv0y6)rId-Xa(ckK^$7pWCtW69jHTx2bo5Dexho~@L=4%5*v{0uqzSw zj99XR7KK36@vas``3jnUrdXnUJwFf*vlQ9~D=o2I9`WeH#hdYzo$UGQ=0!VskW@-L z5w>c=!@j?KMJx$DpnQc!^E$*aM*d&W`LPgOG-II^tim2zkL=2CFYv8Du~@Xm0eNS| z;a3fUrR3H6V*3o`(^mEr&vy~}$)A3dCp1~g%zv9Z&q`UouYQ431Jqu0IONaTB6`!; zKY2U3@F@$ltayg4wBDAavg9GQ`W-jpL7{aBrymwtXOa62Haz%t!lDO>ojhLY?N%lE z+Om7s`xJ@=z8yVrMs7z@Zuw2ao#*}U+~b`Y9KOTA+%~|sX|%1S<7Wn@(!#Ao?BZ}F zCiGl!G77E3IH9r70#1QKt02Sb+M$iA@vc#2F6U?KDuwQrf0A%u}4;XZr{IWcXivutF}fad_4O6t?{LWyW5$S7TB$Q1E{p- z$yP`mAf`g93K_Ng5u6HGXvNzh0Stjwj(*xFcf;r3B{SL|TWGmEHS4aGe(v+2u}!9A zwy0*H_veSgqXpl2pq17r0xJ=@gnzOK4MYMT7HEN+p>~!83atv{OK?8trN9tq!IsaD zT!R=|=YyLtDu_W{$4z2ZwBdk>VH>(ZBMKcy!5vlB4)w#2t( zn_I4Kv!v@MJ==bPI`%=@GXH2U{XYAdLR%9x@Ak@=w+Nmsr&0GF)c>W_ysNf&z1BWU z(%~1JTx2G+gu$W?0?=AH`W8zbfM4V_SKCr6uUf#d&Ul>kkEVO`ApwRk~G3Q|r0xi2^DF(sGmNV?_(hu+2QQbl}-x}4s&Xi1Ao_6Et zG+)~fPHX?ZGz(+{=m8}fv}XbwBN9FaQcgnwu>om*_P#5+6h9mn%~$|lb6^j^>vokI z)mti#XC;&zF4GM#PP09jK38q(p7nBNN>Xxfo@aEPZ07KmTtQmAp7g4U^HsIpUB4~u zM)#{k0y{W1x-6JOUG=S3b8x7U3BV1OP*V*4aoS^<;d2_$Z&Ahz6o992`e6b1Oa%_W z{P9^WQH?y6DcP1}O+n`|j1z`4+jpM`SBWYA7S`}&KkEDTz_*Q##& ztS_$G6s?o=pGLKT>iiG*zl)6#p+sA8zEf^y6nx56`sjAx%PB4fN&JA_oJanmsoW%4 ziBu~l0KqFpHuM8<-#r3vNdb0$rScNBdjw7eEC91(5r0Aen5kzv%iZuLX@758>aFxD zwRbBO)=_`9F>Xm#*yW~6`C1<8^jgsn8Q(wDI6MF)L|Gq+hn7M>=-5&n2nm1&;KNen zf#wZ&3{b{>N4^B-5|d~Ae`Xxph}v5H424!C&K<;|1x+|W>lP{nl5oiEj8<=Jgffn4 zw2A=rA=5A?T*c5u9jcqPYMG3S{j<+V1I_jwGC~;_JK$eNx$Yq2Vks}WKXD<9OT9Ah zUlw&lSUr1m?lzsK>h39TYw@>gLo5kBpp1h?vkl@Ha)>wzttebHW1;mB`8B)e$0F^% zYrM88J-*20y`p`*s&bR^rg>62*B6_=H@)XhGMUr;yPY@Ab@ZAg7%x%^p<(KGjMZO|X{bWy|mdaI(DGIVPjn!?(8uINCKiiGPrzULx4yf4?EhKNow2918kPgq<0)B>|Fw0p1my1Q$&sR@@K zu|NwQYE+6{<4a5h-ub?$-Ou7wz(T8-f&?%GTCa3X_qyxToDB*WEf5rS36U)8@b|fV zt1a}w1Yu2)WUA;q(fcb>(lz>_HT*-*v~{ezjRBFshlN7G?#WGLltMp{FTuI$MG{*I zVR!2I!$XZgw56JahZ;fiwy^&Bv2!?g{6}bkOHUKkAPOzeJfc+-g%;DsT303xftE$W zsm-7;t4}{W2(9=(3oRh3l9C<@t&0QUAbeeW5LyinEgjMUv~*Rz6tEv^ELeK+?(14Q zEpEZ}lRWBwp@moydJu<}E#jE?d|nh<=W)@Dh1RP}?4cEA_dPytY4Pzznz{EsjIHFR zeD{C#)3(_5EQ?a!p>-gcGT{|z;l_q9ev1M*jODl=T+VqG`y4$|@kpmoBMdKGs`Bjlc$`ub!H7oUa z#d(d#K1+jNO}s>?$!-Rje#^D5uAoPw!}_MO!`h%pP5he^Re#3}LTkuw?LTG_*d!MS z{Y<|BU5OZj6B-LG;1npdiZf*1Q4wfSmc=HSZ3{be-a^mq`8pr_7OwW^$(=VPZcaGG z%cCTE(x{v_xbEi|hSJ{blKR))MqP?i>Aoh4Q|f!FFG%c(TXJ%6ehjU&`k^JeA)T1r zwYD~(c8|rWfQ8miI^q}zv{L4#pGKf{jx@6($91hm)d{zN>n7_vg?D|dejdBVi}uck z=Ql;7eLYcVb?tb7p z%MyLLz&LDEwZ(14HLv%bk(ovj64m=Q^Vk;ooQCc%YJtaNl}!p_M1>Nv+FkTa_4(x( zZ^N3~ERwQJUc`yK8+_;)3=g8vTJZ1Tm%i*Si!0dbcVznpw8wuLrymwtKapRPAk3Y2EVO`ApwRl7Aycw{$y7EHYC)Z_DA0%CfV|3-0 zbtab91P#uQ{R1u8gNiQ1>^|+8Eo%3xI2Ewa>heRJ41pF+C;gMVe)6FlG2i8-SJli6 zIeOl5Nblvgem!}c?G-Ib=<6PhBbAev5QWz28~0d*1|oqE%Y6&%eyhR;rBF5UB{+|! zt{qZo!B!%U+$)8ldAKK?F6v+!t$bqB zkU1^phVj^BM;fux2|AJqMC~(U5M9&(nn%jA(5ftF4=oS7 zDvbrRKZY&LP5ic-V_vp_n|GtpL23kV9?TP8<^?MqcJmYZsVWHJ|2@b9D zaoICnOUa#L&Ggmj57s!}e$jnQubx!Iv7ueQpg6(aJT&O!0_oJuXTc@)ANmeG=TM5{ zET&?hg?pLf3$IPYgg&P@9fj5noX}Wk0jEHr^({l@$36sFTFOe+^2eTJ@|X#E8DBX( z@w$txXY}1Q_WWswxktxxRnaD`^%0rG&1pHR@FH(K_l?M|ZNFz^D7492c)u`1T zWZwV)c+X7!UjUe7daI;qT^ViGMKczFEdlHS z=weqDuQoL|Y+*veuJAn*5_4vccI?SHTq^bB!+M^g3F)8s6S`I=&d&BL)H^U&SSaEM zV^NIaeeZ~p)}D3A2Q^ppa;%~A7)D?KX!>w<@2~)@&aNLmrvd%$P1QmZ@g1Cg7yvr( z8^ZzkV_f!1yJGUMi6wHi3ltukgfE(ZrETH4XF7&;+M0>-r7Qj!n>bmmd-rL z(<2t^e^)XiFm!5&g#?N7vPB=a|GQ$|Sc)uf)aH6Wsgr)+)cAxSE5G!=(04HYE8mnj z)qQhV0Ej+6CL4I0z}EF>Tv2AAc2C5qfCZpjG7`WL06x>6H_Kf&b@Rr|lj zbI#20+;HKFm!>A;d+2!zHIX6iz$Ea8fkMFObha%hg}xy_1n2Q8uDB@FGHMip@)ejC z{mmP~S9VsO9h~_Qh1P9MI`BgaN+Dp$k}GK_g=j&$d-@&|g&efw^Z6mlCi z3PJe_Onsw}0pTkYTA;a1^&1pg7k_n!!<&D9ZzUz4OT7m`D@C(g_D@xtzC!%wYI2m+ zx_IaJ|Dc6L5_;f=mNSx=vjd+{Xx+i|W;$pU6BSuQ%hjRb29Ioh^b4;`JRA+mafa+& z17%Vi4P2!reg_8&DGW3jDPgdlB$72>)WISzAH<(ofqeA4r+`F8s|_J zOfI^->v5}U_4=}~2ac^B{)V-!Tcl)9CHg5uG;8*7#MEr0K?~W5xZ(@ro9y46^T22H zGbwHqTK6zU(?JVO3KUvZIbDO>5ok%V74<3HCXU^*vEtN=1t(^%ZF^DYJCENhj+ODWVX^G5 z8VB3+%j$0eXyK{a&Ipt<#2rck&{^bKaPLF}&G$*qLK)YHxB0BKEw4$%Gcgw^@y~S)Se$OXD#jH`8>vO z#8i`=E;gH-%lIelNF<>LRNA?arXZ^fLokq-qO-fv(FCn~p1JDCG@97&uCN7+CbH-J z7#>Zu{N2$6Fx6{90Aw@)6&Dwz`5NwO6j~24y_p_bVp*)AW#{yDYMxL|^gQ*rGZpKk zNb`%u&uDJ+Z|MFI*YY-3#$d&c%N%J7a@MGe`DGkkWQH zy?1P1#(EmGko7TKl~z+%4ucM{^lx625~}}?F#gf&KkyH#|CF3=u5B(84+z%=<@e9o zZ4|tT_~JmaOrgF2DQg3<{`hD0+0=CIHlM0-NB#4q2`VeOyaUze^jNjmZk3C%&N!|b z5*00xle+H1Fa1v(nzb9!|K9Q{d@dwqDWUrR7~=xH{?7!R9HRgCb#sm(`X4MzHFfhY zew}AzIoZRM-JQUbr#sWMC~kI)XNU15Rl|UxqGjZlUL;S#@hBGPS1*Q2v_+udLK!FRsz5@eJL5p{s^cl zp`MAzqRa3!&(dQgnU>+VsV?1$-PGstcb~Qbrap?FiY_%>{*?sLDN4gjO@!iZ=VyRy z=iUu&Wlgpsi@Q9!(`v|jW9^SlTJeu;NF<>Llx$Fa+<+uT%6Wh-0=Z5a=02q#)axt4 z2lXwJ792Hw#eFbQM(}E^{mLBY*7^>1fA6$vHU7J#>AZn)PV9|-0?I3c-zIT$1g~D+ z%Ez5WB2&rWQ@`LNiYqD^2lWzJIKH_Kj6T&n0~PgjjM4O>KGBJVs8{E_7xF?xox8y6 z3|G03^RaDebv6|jY{UX}?cI+M$^6DM58=8rt6ZJXr6{H&@i#l*&{y7~~)X5kZ=tX^QB9a)0sNdC{QxZ7y z*^l;ej;8Jjg(^Pp$w$-D?K->NyBkU4w>`CW;g4|djTjgb^(P}fUt90CoWW?Q@&cpF z>O1kieh)OC*VBi-ey<5}1Du~((2DEzdrwDg;)>;MP@Z6nZhFhWqys;$po4ng_1E5d zbWnfgR}@~qs{K8vpX@gzeQb*+Ze%TewnbwDY$a>RVZG~Gv^V^dJ|vRR11fz`e1gQB zYi&RW^|bEkKBI(bP+z&p-y4h1uu;P&bYK8XoqhT%-a-9Fq`6#r3p%LJ#PnwRLA^F| zFR*&N$i(T}^@z^2gLy?!`2$vumql(CdH03gv$!JBVcK~Mk*yz`Qw3GGy`_3B5(weS z;(jr^;RvA~8P<7@DyL0g(Vo&KS93qfo^&O%O@i|6Ki!8$9K|!e;HHd zQ1L__^JF;ap=Ff?ht|)D`S6BXnv{S1IkNUZ;nzWGx2iXwToVWmmy(3-)S2=h@-|! zXm55!OxuU?^pOFM- z?LR0tem2{e%jG_D?qx<#@;cAhW%DL{xEmB$F%U~(V=Hoq=h(7L@L(V#07qVmc`R!- zvZew52DbOI?LvA$9>zZx}_mwxg?3%ZHltR*Mb@rwkaUrT6kA6!h5WrI<|7>c6z@&DLhiT|v|!YCoGx@di)v z>XqP91cLiY9K-`wF4OH}18+%xfw7XFcpwW<;??H7S8_*)*L+Idbgj^?7X!%yGfSOs zd*_e2SfRPgU!Y{dxW45lD8)his{V%p`Zo%9x!SMD4frA?kQh6Im(26Nf7x7thZ#Ca zT@d0`6uRLHbj;>Hv^;)^aep|zKhiPz$g!eBJCOapNbHdu|9QlPn&;2M-t&4?S|T*7M^ z6QyXo!s*|1boB3*X}~nKW!KPI#e7UU@KXvZhTzglU0Dz@gjRK+dG(4Rs9pONv<>#Z zp{8N(H}qp52bT}rtjrk3kgpep_f)0V8eNVJ2#ZeGR{x&Prcnn@!%u3D5sE_Ra|tk6N8!{{|@u~Ik0lxGcmMo zDa2Swui{J0S*UnjPU`ech>F(-6^UHBesFM1$>gAu`TA^^we80%_^$I+{+iQM)%3r8ld%J5yl02 z72ioh0sv9*>)Ow^2kPbSy89zO(0yR1b?Ave$-!>+zA81dndH8%!8iw@fY2Wg7vfd% zL{2*fqoFDe;_mQuGhP)3&6kw8qblBlxBx`ILr+M&4ZOe-j~DIoe?V!K97ETF&vrmj4D09`@3{wu!*CgH;? zC>ag=w7|A6Qq}%HO(TB$(lySbrJCGyjW~Ea=pS&9NJ0-NaG?6Q2}#Vmh(1)s-(Y&P z3vz9mitk9ms(2JK7B?jvR4?|9R<-t|{xB^K5mbC&xq!EHj5?=4zaW+BFp2#sNhpoo zY98x**c5#FQE&^>5CvQ=n2=~+3iLync%Q8oh>)N?&8ik7q+eE!xslN%pfPNK1+Df zJ%ew8x2gAD$XHxTJER?-3%j4UqbmLm;{v^kN5~^lhp6~fZOi+CdU+eKzl`K)oLhgh zb=rF?!-pJgcQN_Wf>ppG4I~E7cFWmD-Czx*h z3Ys0)&fygnhuu5k#E6JA2HlE2#(FHi`ApB2KD`?9VI6VvJB$8FBXf@*`WvV;av?th zoq%EM1xQTHwJ>zXh}N4c*1Ix|#Wmf^wt*R=qKzfPuY(=E6&9#IZbnSai>gFtj7l-R znSRD-YY6KZBT1*GYxU2sADAobpjxCNE6(vOBC?oksrAJdEkQPC2FW~u{DdjaZ5+wZ zrAeP;ITRD#T+T>Y%l?5R)ZNvgoLodC*Yi<*H_~Q|z$2HXE07tZ5td9Y1!Rvbp1?o1 z)7LI<;eagrS8kKeR^cL(v(!OKrLGOlr4684SCXnOtQ}Me?Ubd2|b`dh4yB5Bry{n z4Sk}h9MhZWDHW{9no`0}KdxO)zq~)UXnkf(QV_3J!e_H72?btpn#*244fA`v>gU~` zt?E4&OzO_|A604{yfSS;P*SRKQahil+l2n&v&W5}T9$FwNP-xZX#4U0}pp|R*BP2ZRRZ8?U*$s8v1#U*g z2Pd|14}7yMRp{8G_rkgQa#94xdV%)TnP=<|WCW(Qaa8e)Av*Nv5yjKK&9uLMMUn=s z8LyC+T@P(Bp1;`uU)!nVqBny)+GldG!ST=*US|U)O!(fgFRMQjAH6zy{7*% zL2omm;tKDjgvxP*x|G&v*IFfXVSjAr$fecNvxo5+&8FsvF6>ufjQ+0+`yhuWv-KcU z0T%{;7aUr!fYoMAOtE9Q=;*Ng+L?Q`2@B`2T zNzB!m&1jicjp@zw0F2FI3BYGmr{*aUx6bd+O5koS|CLDXu;hgf3(Bgy`IzjdRB?LCr?`Jtj=+Y{}q7k=fpm{VWTk-(YcsIh}#LmjI#|xyAc6(4xE@zg3*@sodbho=%t zhZR3#x!hs!RaU~qsAcC^E57ETqVq}h)nwbkZTFM*{!@G;lF$Rc;%`9`vtN87`U1}{ znBGjU_=!%e6~D`=Ws2*ig#A`tHZ8>`Oxs)5IHXZH()0aOWp2f>`Lq(9I0i%Fc)Al6 zS`69WG(TEUbBP*(NYVzvCb6#>4n}cLH>9L;Ur5ndni+e>co6hYtLH07s{VI9Bh%;npZg~=ec(OFwX{#k#@-$$KrZ0IwQs_$_`@z&p z*KS-pedTLT+{QIX$&<{#u*P4q&ur&@ofRG}{cY^_jX{)t((aEv=G>Kya*SmGpQj@c0R%JeIF3_;}Jt~ z|B7+pSKKWDv~m)Wi$b7vM#E}*piaK*-C38$Tkl+@mvDfMljD(rP~BYNd5ONgA)b+g zwgOz5X+zLT8*w#Bqxls>+<{5p4+FG-(GpE6D6~3|AA<8UHzzSctL5F6cx>I__IIHL zOnV+N15#13>`^5T{n0Yz8zvq2p#`N7uq3E>CrTk&&^8f#naUIzg;Y1oRny9p?^{Rf zZN*T&0#lPG?}6|Y3N6swHti@1t(0Hg;lh%bVQAUj*|RngffmWW>tqybXjNY+jZPA+ zA@@G0bZ~R~2Q4I$&;viTJdwoccXOc7YQ*$rdT6B}_X3LtaGo_gwOms^erjK?x^a}| zShgk;)4-5f#riy6_Mu~VxLXA5;~upyaS_QXHj|=0DHuFsVYDq{cT+I6pzjDt6IN6Yuuu+k9UAFe^QQFC|R!L^Ch-F)4u#T9u5@O4m=oXLMDU z3JR?zjM4Pa0!e{F>s!t)&s7^Hx?lPxre*nP&iegNB*$vFdMHtT)CRmwF{rlbjxe3U zVOnuHQ@xXOcbG(qt%qV-%Bs#byQTtanysO5am$I9bkJJ9z2rBv+Pix3xgaEbds=bqWOXCEh0TXK#uhP?|7{&7a8GJoU{qa>mn89pcZ_%VkBs z=IKU;mei}s3`PT!z#j%^fzEQ3Z!8L}PUMH+T;{qE6SQcR7XAL>*uM)cV4A?fv3Q{c zn)jR>i&k3Am~`NW7F627rMd6cqSAi*SI`FTej1i``R;`sw~2_f*FLlULp?Gpx_Sl= zrJ8(f+L_7O`~FEg5=rO*m3A(qDYQ4AOZkG1CTP8xV`>4*(ZqVbdlcGe!sG9bCO{vW zpwa{xO+ZE63u&&xSBB25wP1QPJ+#P4tf5uq)H;Pb`oumfVNHeV*rfVJlclZ1&)l|h z{81metI{JkJ;s-3Vw}`bg2TGd?c9PPYEqWX9)W2uMLW|@>%EryG}c@!nMbLY2CWWs z3&iIU7ndUWNenu~(!U9syr_7#V*I0r);lLSv?LH{y+6lWbM|>p$l7DKwU$P4D`tM? zR~B!gT&fR#^Qq@a*y*vNUsHT5&%Mv9TxWM?dw%&&vS6h_ZgyK_YF9piS6)MN zLiU4N;bSJ?SkH#cH-o>!kPm$8K1qWCS3h}yTnD*1p+zxuJJ4Ot9l6z?TAPl z@l^-sGZ{MzbejRV8Q$e~;eFKsXx<(%^s0j{#0_vhR+#mx4(J(27&XSRybVrWu|}cw z6O#`7(1L0s@Vct`462Q^D5M)7V5yCX`RbK4Z8ZJ6+6YX&u5lKkjZhqe=5^aIqd30v zE5GUU&JW{QWSnOD7b1e=DXMe+fa7m^7hZo#sV0}$YH7^B_7BHMB%ucs$I#yFjU;AW z%g~O94oq*RhgOj}YiN}@wW-zz9@%GEr2fQ5u(l>Z&`+YpXG-KUPivX4VU4pVaB1~- z@W{I+2Hl`e*X`d}-Y!j9-<5f!xAjYHWsSy-6w_}XI6~tw(0Vu0zP;OBIJT1k|N5lZ zQSt1=_(u<|IwU%<9T8Fr1xJd{@JR`1K9IV=F3+zpE^GFaP}d`c&n5U%?Ka9U<)BjE z5O$?1bvm4i8MqKsU;Xe*6`3`(8n)snQ-IOo>)26fbzzLAhZaZ*6k6YNc7-oTpcVZz zt;B%ZbIe!mfJ{gmC=hg zOk5CnahuAei`lm$T@0uD-FUnzaN_HTYx_5!xYW6bN88o=qd`WF6i1(Pkv5k$vXebFl z+)ILCpSlJffIke31I>@eN}-JFL4F9%+t;w3QPV~!UxBF>2AL>d@BK=ch~?8^zP7lrAG88| z?ak{-WxaN^XOGqq_8rya0j+M|p5lLeMIs43pnQcw3nZpfC3i0fRphEWTd z(>UpM?@Uvbj$)+msPgfccudnzv7VZK>UOVX8x2}bohYyoA<8^oTe6`calO84t@^`AOu=RAkf-<_!~Z>sii|NC}qbOO%E-Q6ezTQI$I?jRk)s%0v?)PWoS) z(Dq@ini}arA-TYrx^E2@TC;!ck4SHl!si0p=Ajpqa$sDbhn83dk~Ijl4yomB57#W> zG~`ZuDA4Yuy6sRHzv;e0_9-VmJtj22OJ8g!qpaRZ#c4(!48g3`bIv@I}ADf1IrX%R8$zz;3(i_kkPqx>BzIE-)h z=3>=+d`1b%I9kwd?kHqp+!abo2)gV0$>Vey<0iR|*7zUFI1sdQvl^6f_kZQrvaMp6 zai)HG&$n`$B8*F$r6Yp;Km3TAE?|3UDTVB>{knYb-o}3)5Qm3}0A(Ch)VCptk(*J0 zj)=K1y_sIrMQh-qK1sp)ZAuJZ_u95i6~1$?#Az1JzGd}cK)I)=(^>s}mYmb=kQu9a z=SLb{G3{JIBnl6BcKsBqq7!w@{sC?&4F+*xX|%_2K6F-#8)G!Rs1GW#5cTGqm6sMG zqVDPYskpDbtD~indYtW4yp{3RobyN8TE1NW`S!`D&-P(jMkV##Vw)hdg)F(P1DTau8%+$R5;hM$U zJnF_95lxa0wyF=B8{01JF^!eKDsNC4G_IWZnY%^q6JAjdwVlEccc`cXqd%n9;1zYy zd>?l$`qt?_#0_xHzvB~=sMDThrLROR{%$1#Ow-F5dW8ZnCLQ=i9jemc(k^e-qbf~{ z!qL=uOe!s=FdkKDnF&f{no2ip94(awJ~HtKn5rRY58)3~)IoEzr}n6*gC`2W`H`Ge z!=j#eEVaXp+mv8Rs?z&|sK2(#Hon9~Ay4d?C3ybMKT$^_2|e(Ox-XK5Z5JfbN`w#7 zo9QZ%*MeEBE0JWE4ppl~!I95}g%tV)$|&9Gt$r20B0O?dl$cc6+LRlPD_T}R$yhK* zH8LJQrdvX?inCa?h(umLcy@N*E3d{c{Wg1AC3G1pkxwI5A{p!x8FUD$|G+<+fP<)b z@?-p?hn8Xp99k;!1?^Y%^EG|^loHZ!wOyw*jLQ)uU0P1>i1F(j{DhWH}AF?q;nLAcSD zrUa2H{>^TKll>uVPB&eWywEAhZtm#J@ zfb&=34ovG!dyiV2!tN6H7&QWdkU&jMuv&q>uMJ1-0X&J6bV1+OM)Ry>!D6QOwb5!1 zdaCu08r7hf0;WpWyP)rDd-y8}FS_lA-`AFs71{+bm7#umfjaBjBX5gz=+!t1nZ2#k zMQ-gsOd*kk9{4e}9ZAfI!l6e-;Cn=$-F9m}9 zeq#!}>1Sk2v5|1>%?ADjo*tUG5yALJk0~uhIHuI(Ux_6~@H?;1bK&!vxWfAM&zap% z#CELsKCRl(F6^=V_$@u$%j=FuBrP_M><>ts*LgAbeRI(7+G-^RQp zT3e057)@7Oy#`5vPMEdk=s%c;)K)9zCHgDO6R;ZRmpvwu_^k)v2uKMbiFe-_LN@uIU?3Zi^U9VC#K%6TubwKvBuBwc_MOA2(+T6%o9hT74t@#GPmNr%8DBvPRW@S_qly$DD9Jf`#Ix! zNo!fLSkkWvGoulqwJ)y>pV2dJ>!RBn#4twx*KH0UhkM%uQS1yLK7#Yjr?{E0L)+&~ zzs;dy)UX2uDlqM3h7bg(?1ax2jJnqYbvs(;BAG6;h{IxRZzIGfUK5WZ50ZCBc5)E5#PRUl5bP z8h~QSrTC04@YhD^Aqh7ctE!U8j|8Kx-0~F5vnhihpWFCVj@YnD9 zB2+cO$%(yXZ

r;`>`M4$p&am5DzhS~i_yUt^&V+oD<2#+A5cAA!=vvDTbcr{Tqv zl=k+KbqM(9{#g-K{0SKU=rOg}34^KLp?BSB$uHE74B(IUmffdM9^A$5FrG3eU+4^f z)rPe9QjNt<5}8-BkuOtIzr_mKW#||2o2Sl_@FIx^v`bM3sGSKdY%YbLJtcI1&?Xh5 z7aYQ8wD&~hg{eq#rQt@Sm;y;5LNK-XK%K7BjJqebbwr6bL^5W`?}@MJ7tKIk`=-xNks;XcvF5(ODQr2J7iDh6qQH#*zD1^`G ztxJZCmVq0MV#<+%cm=`KUX>Te!qwk;Sd|nkEBF4YU~=Q~g^~^MZwtvLd)ChweznEcWk6KrSrS0j5{PZ^+ZtG^T8r_FpB?OO^rOA zsGoCdWbp+4^(-5DHK;tsKYC0#BKJDO14zFxr7OR1DQP!<7(4IXVt$WBp9Yc*wpi>C zc_~$?Q9&H*E%eCPCAu?SuY8UDwhaTXxQLBolL_s)J}HY(cbTZ-yO@CFg0E$AD_{7`V&y272!ssnA%E0x-0}!!7B6a zhpWFEzh7(3$Ck81;RB1#IdaU{BJg^4bAV~;IS#(h9c$%z7132MX&f(Mt$N_VU^Em{ zpnJUI~(LXi;#fdW4HRb)&`|;3L%3&vP;$MCE4HT+*okt)`wJ4S=WptMi7dsaTf> zD5eOTZD#&qbB4z2oK{t&n!427!lwAInnD5zKk#E}HyY}SnV&|_~UP)!7f&_Mon^Kc} zgG=b6G_F%eG8JR`4jIQk>z%!ahN&j>6W-a5Ba}dQ5pI z!7(*U?$uK7?E+0p>UN8%XJ*g3vd};A;nPq#{`r0rUw>m)K5`*_>ngUkJ3MM`+tSR+r-i+d z5bG6Yz?36WOj<5gy5Q-@kOgivm3j89I_5-hp*L@(`&&8@akM}l| zYP6tg5LT}Ig!{&?wuF8A2 zl_8UUr;gQL)cIFUA(4b0_%Y>&B%(0r3yLXKOmC*glzRwkOu4%BT(RPE4S!bnI#*7v zl1nRO0~a;Bf&5zEtS~&MwqyS0GlFT(tr5x3ru6TB)LKDdH%l%LF$tf>_GVkdbCLY> z1G}zuJno~%)B_xwGdvDej4YnOzuIL2=oo(r#y@&Yk<8(kGM0O_&26iI*9^)8s{UN5 zW0VgH>H5=m_6mqfYJc=gcC#T@z9-tLh%TZSoz3X@-fjNy$Ji9DbW&P6f~js4Q}vz_ zzcIzRqza$W(K7^5qo=}+Mll7FLWE$-Cc5^4lh*B%VLHdU-}Y$Qv%j9~zu#C~Zi;(j z-=|pDXnbcBkg;kt^kFNH`1uWO%45Z6KFcqNmQYU&^N!OIdz4dY{(w8J2nOv5htKBEPv?G|hc!S}&Ja&)oYK>{M zwJ8Z`&;R|Gk{?dtGy3jUL3BIDG>p;zbvp*=YD=nZP=>K13numNQlEoj;UHMkf?9JPm=fR9j9 zyD0q-Izy2MJad0PfFdsqxgI$0n$|J_QlscWmEpFwx*j!2&V-^(3Sw6{W z?lb!h|;XvaT&zUk?eTPQ!q@n|g-=o_!I z@5%TpqB-&|i-}89L^I>Y7iL7W@8(gj3vtuBpI9ccD=Dbfpo$K88vVk9dLe8|RrftJ4;)=--^@`*$$~Ov^Xf zh!<0!xpLjm0|DBYbl}Gn_(f>YJ}v2jXg?RL=5wZXqg12CSTwH<7u5>>Ze<377HP+e zZkta>d;#Z=)QH2|<{jpUuM6ciC6IC-O#Z_<*QBatU+3+mkX_wY1eUt~tIUu{LJ$0y z@<$R;rabhby&0I^OpmF3S*$T-=hCPA<%j*=XP#tsjcJK;DM8g-y2r|EA6^!j9Z=I? zJx40KD`71As#tDcL;l?6PsEvHnloEXEyZ_|SjS#rA=f7e~ZSCxl#S{2< zxE`^q5Krfn&--ZsFaPt^)BpbZ<;=oD^yd)&Lmfno{(0Gz+P z(a-c$F>T?B9#isv7gNBri!B^@F$J2tNnb{*DSb>j@MDS#$rTf(z@^#bub|wa1+9Hc z5)*e^<+kqy+W^!0YSU;BAwL^6?f@U5rplRLfe;m1O##p3$38@>DKc_Ba4y7lcetAJ zE@M|dz>Vx0G?_lejP-Vm4WLjt>a4v)I?f5p@{gNF<{#Eyi5sh7H%Kf%Qg$+ zA3dg$72%kgi(qQ5vp}qZ0`+;p;SW>K#b=w&`6*gIbM@9@X=ewomO$B^yd1WZs193h zWiE-pX}6K z)#P3i9o3}lAWI1If7o+BPw!~nCR4|Blfk{``l8g&HOXG{BPoOt>Z@Asn7n28-@d6W zA_9Y{C4`6E)9@L6B7DebL%7kFrpV@Yaw3u^NHw*6()^O>sqfj1*y4A)8z$_{401U} zQkg_Z=r+C(q$gs+UC1SUMX^|BsG1rXQ`XU|8H@&91KK?#tQ}BH0WAaFA;pz>>`31E}_AAlC!u%X1=! zwLdZN399{s81qSgC{gAd9(&CVSv>l1?%t^z{}zvsK*A5G_H!X^p?S2Kb{rjU&^rHY zsohM&jW_mZ!?44R(JOPIG9HNZnYSx@P)r$PdNVzyt|7n8>KQB(mwx3-rOWqP2wgEY zGpsQd+AAZ_w6{p|OwT9E&V+_+bFTet;gMqV2@7UqAE*|rUeiqVx?&ypXqC`I3+}fh z0a;Q<8b`AIf5lXoLK}k)vGh;l=wejyO)&n^V=6rhjwx%|S8q-21v&Eto$Lv0D|yJ8 zvL56wTyxt^KF4uq+RC_^Q|3{>eJ7k%f2(M+pr0!4!s{elln|t8iD0S+UFTxJl>Y@| zd`8bOT7nvF3O5?X6i5mYhN(l&5}UPdh2;tGn5gW16vr`p;w`HSKjXjpy9&Hf8GC}{ zemXSaf^e(IhCPm{k2sRQ3oCfN)~Q-NhReQkyr#`>OqGC1za@mdUO3jdKu=n7Z^&pf zxY3pXQ$?o8Ed#;Swuvu0qNjfFK2f=bx+_ZgyZR*K(zKrvT}3TycQis%OiC2`Z(IE= z8o-Mwp$f)GLl=C2(V>=eAleUZYNi_-G~cvg4oWrfst9mypl-rMHQ08H?!UVo1DLiT z^*D+tb4)t^M@)fBr;3_FFa=dZ(ENLsDXNCFpyey5vQ$I*XRz*#TJZ&~rhuuE3T7x# zGm#De&d2Yc^_!>_r}u-ksf-*Aoj*sv_0Y&t*Kk4*#ktB#`g}N>r$f2O)`A zeWwD&)ND*|rpMHyB-WVHa~V($f4(>@yYSMd>YPxUwEcS|Ux?04IW*J4spGkO;PZgP zok6h^KE!U>X0j-@zd0Z?cYg4k?Ge=f@ z;9o(pGOGAr zkn}?o-)rImo#?3_W%~^#?!8;8v84ayNvY6`FTxy>&vNC8JxtGQw$H17UXHskadKds zKV#XG5)Y`LI~%MpM*m@F1IugeD_w$W>|D&X@M|m-OrS&DBdx~^CeZxi!=ZhG+z6P! z`BRS7OkkqT3exWroI7ge76=IwLxE|ZPjACZ572!24qucWG|#B3Jz3I&e(Emq@6rR9 zy6M0{lpa}#>)^a~d-yOt9Nze?21`Z+qpuVHP>G8sxH9eGF1}Z~1<_ z-saVl%1b7@`kuO87R;{GXKOjTlYO^c+aYq21 zkq43jL*9bJHKjgUH{)aV4%QUC;=WfE;P-a>H_m4l_4`9= z$yL8M+>KZ3V_nxTEa{!8@bx%-kpq^DmJlp@jPM!lpfhB&HQZQZeucNlx!b&u!DuK;K=+6f zvB%32(7YkZ7cFvl5SD=R)qSg&SVG$f@po8)bX{ha0Mnee3!`7qScpjnegH#D0dVOe zy?v;dXCwbMIG6ksHZ10eenVnTl3DYog%yk0DGAQ86!No%g-(i&|HK@LB=mraImjr} z_yHt>ZT3FA;|I{(KzlzrexUW{^-FyJkH-(t-W-gWdS}s06jO^Zy_p_U?`v3NO3r2Q zveK=2VY5ABWqdw<-t{D5=WN0X!%4eVjurkHx>cvl{Q<$xdzvEQ&e6cPRlNfi?5`a{ zB105y2;Mf-`?mgVY4MSr=KJX|wG3>)8wpb~hj6^N7Wj8e_AsjWHW>ftG1Zg=$J9#M zMPl-c1;6wZrK_x6By!HHGofSQOL-}UFmvvgs$R;(;AsTn-3+lz4WcDMVAgY|#Xv0TMJ)Euwf*f~6r zHK5nJuGUxotpOo{gdb2$aUpG)Fa;7(mgS6cht@rtsEcrMCu7v=R?zvNrgqACLWl}g z#1OGmoLkMe3NxD##xh~BK&0Z9pK6}HZ==s-=IhGIHFLE#2^Je>_ z&aKWh`5k_9b>3u@tjIdv$H#lOGRSLtcUp^3U-Bg7Ea8QEh5;h21qn1v4Rks#d+@$# zL`;nfGGNdlmj0Pq4t*wk8OA?)ObwXBF|}HDk+#%AA?arZHkop1azSSfo|a{VhgCF8-9na(42SoLXWJ*?GkRa- zkkQNGMx&SlNg+a(j20Yq+JRuo+qqUbeCODCuirj2>wTtubf!vjt@aDKEDCv2R<~$q z_0wJ3x~ki51b27y%!#wPo;m+(pmz0EhBLq|Iq#gK-eqJ(Ex2}L24KVdu zFaznf5KL{7&o7Bqt=PQ$b+MFdnr76ijELH_sX9MzcnC>XJrv^*S>H7|&q#S1-fF7) zEaR)cp_l^Qzt7kiFQ!0q;>ryuruYy{f%7lB@l{i9qc$_gvg31(8>*)4FzNUoF$KI% zynT|}9f?2=l1y;^TK6b7i0~FiZ-V#u3v0pDn5l7<=pVMJZ9W$I-36(p=7qP%Dg3LZ zkl?`&D5gM0nY14yV$Gv?FtbWv?EGA;nkOzh#T|hBkk*@v>%+Ktk#i@G^T9&}^aI_S&tk=X;*C2kJdlkG0=e zr`Bb)jZd=PrkG?OEMx# zf@>;|!W=@VPu63zwi9deZAe9f945ZhY8Qc{gw^YV*Ba9>brX4l_cx|$EzI&p5t)5TMc2 zsjH%QER$^#XEc|@$rSa)lo!F>#7s)g@H*EprW%*wsHQ-q4}TvrdJWuY6jL$|NY960 zYJ@Re%3m$elWfzGg^nd~wI_L^R60!KICC44)I_ogK znGRELxU*PdDx2y`xO6GnDAc^rn@u3FJ2Ue2m_)IdtM*H$FRb$88jK7b!%Gdb$#tPl zNHCW>!=olI!C$pbJK(@zoA$%*F%CZj*)5*)GtM?ZW;3tCx(A=8JiV1&FdYSUU!iTJKxi&kdNnBGi}JV__k z$m?=tyA&R%7m{_$GwCLO&quwQ=A7oW6V)tFJ~%fJ5KNScobq%3+hPM@_xj(I^eO1hlqJ-QXef9M%i!c8|9`b!g)MDhIvDAtZwY%)ldlVXzKe6l;neB6ghYE}O(;U*s9mszY8s3+AQ{= zk5##1l7k;wP-zF3K2&9mRx^1>9>BRyg4OVMhO4h@xdkWr@51V}_&DhX1RaC$G;`Z^Nb&uZ-Gg0ucyi z&%zPU7IO|k%MH_;>7k{J{5FdRrLyZ?*_Af#&<@G+be#R_WN)Ws_ra!olhci}X3AEC zxNuN!1}*13dj5lCjMJvftIk|Y7rc7tW#KTO`j96zKQEFLor8QSjv%s)p-y|>G$OPf z9BN|FA(sBh+6}!7VH3tbdT1GB!J)NTX3^mWBVk|Buf>y<_Xzc7w^-GC&#PIH;~4d@ zZjOD7Po~6;F4FnRpN{NK7Q8%7i`=s;lO+%M+qNOl>IDxm zq4uXILXKBXQzNV(u{}9a7;JB8!ui2TO49W=w0^C6nJ&1E&*%g-eH2>m7^CT-Wo?S| zLm0Hma<@%Jpmk=@vSMB6_##=KThF7;RLFT93{2N)=CC8mc6vy{;RZ*NJ0-Nj-kCd z0!c*F`k}YjZo%|sdT8k;v4&QaD~FQOM)eT$MfRW5RsAJ&$9O0xYe*DNpBpL6Gd|M5 za!2ssd8-=1g#0m@N!U_TWW3WR|fvn>APz3U!kQrQJ>7kYWMTaz$s5yT6_>5kAOp1LM@;lxbqg{}{drMf4 z7AsXWiGqGjH^ysvzM9@|$rt>@KHRDTlu^la?qkCVo<Ap|2op!}Ml)30#8+DD2rEZ|TB2@3n>HO3gf0imXw2Y%+2E!e=s3 z2WQ>zH8dzKZc1fOzRejVE%C#v$jHQSJ7JJ#3}@{3q}dl2(IoH{GH(AZf%UJQ8N>yu z+ra27pQ)&B`@)Sz$L*HpEXM68s*n3=T=7|3PxN`;#4}aH)#`A}#@dS9_Or8%i(V$n z&F<&=U@Ki_$|sz-K#)5+MfS8+^wdg=hTU22^-qd^kK2DuHbsWT<1@NdNe$KQ?HHr! zb=#eUB#>FRUA7{+Eix{a-MOCpZE&obKOsDlUDAwKGpPCG;F{EB-A_i+?Y`Mh8LWir zHVExcp9y$%8#E6fO+*z-1kr79zE7Wd7xfzP6DPr|O6Yal^zZ66Fl~y0J32Jqfr^plP(IWa;L^sE^Y-W1gu-=( z13vBNcUE7qm7@?&JnfCXo|#%r@G;E%+(zL5Lvu1UgD?O-evV^4Y0Lbx3_1jrc;Mfb zKt6N{eHX?*dM<5Ff^*4Ny6{c0y6`dbG`-gPeD(O#PkTa)8V1I6xPQH;eA)YSPf);| z{-ETxmIR$!PfJaE-%srJq?UyS+2#W-k$=r6{pOOKIvJnQfp>ci}TyPKTce>IpxL(ezx}Babv-=F(a(giFdC z3rh?aam;A+8!Ty-DSNstNbxP((XWlqBYKXk8Hr2hZXCd8rTJBUbfJF_#>zh|^n-CL z)7TaCttM-I6k1~tXn}L{d~K#-HVsC@hAzFl+Fvti&Se)2ag>P7O>7 zYCB7K_Lx26fYCi0vAc$@Tpgu*zG+nzcg()JP$=M`38JiYj2m%p+qpy>??46qInL2U z%a}lnfAoy=Glw&7mvmuU##G_`H^vF>FEX(7YqOj5K(DC9O~c%Ntyr*#wY}kjAML?? zg*_Rk)y=(4e%=?Zs`1Xc$UXoT`iB_jN?&{Jxs2n>Pr&H$HA6-R!Hq^42a*D1+)2k3 zA=58KhKX57dh069tw^dYG%6L#P`+JJ{#0Y)6w1XP!3z4EUEGJf0^MQ;Uo`hcwk&+* zINLkmMtj#rumu}1Zu#&+|M#&|G~aFH zfnrP)k!5iHA;J+C#=J)jV=UwU{K-9(OTn1<4zR4f z=wI;jmmib_rk>4C!drWR=9F9K(c0@3;yO65%{wz(d!>*p6wqgLuTJ?BmmDdT`*$I= z*Nf{rIH&)sy^uh{5Byx(k0fGuF$Y?Eg1vqg(<>fpe)^B4ql-_FG?lH$5%qU73~X zw52KX1Z%tl(q|!it{0DAc>nms+JoCAU%h_2{m?AJt1w@){m;0jlfS(S^6kpndV^zS zibBu;HHgY4-8u5RxoxI1zQhQMDbPKpTDE}?@L(zdD5gO3Is4lO)mQye@5}mzoG?+B=ms70o<_AKD3@H z2dR^xBO{PEo<$V&6;$CE1;9Ttf?oOna_Og$LtAnqF_)&7@slBN87E2Gh^8or?2Kz^ zBEOS1HQaKrTzcS6wsXSU>gb)TF|POH?agt`>9>S9}Xh@-B~bT zssjfHT5S{|u7h(M!NFmmwA^rQJj!iKASh3e{lf}9UXTc6&dNX~sany-*wUOY;ebG`@oMa}X{Fv>q)Ay7Y)kllSZ3&;N z3DXT6udwk14-TkwOQpm*6#C1JZk{x`>}8x7iBhEhRc4o%|}d-F)lg_ zR_eH{^nJ&O)kcJN9D@$A^p7)C4psbv82{)obyX3Lsjvw}T8T0uO{XfpujW1|DHE!C z$!?A}zd*G@=7KV(m^F3g6v}Xp>*H$3sGOhYlMDI=H4;A|Kz>9=lhbo5}vs4 zc2GG*d2*L<><&j>S;hUJV^?1{OrWf-wNOCtgI6`E95bKroe#i#Gmi?{6F^I0<4Oy3-~|4 zA(WQx5J6E1ML_9L@hSoaV33j`VF5~)t%QM!5{hCWU(OcA0BXm^;s8|O~M(xDc>ovd&bu_?H- z{d@ZzJ%l^@+~zfJ{&EK~B>aMM2RI*e;%WS>fIR9zz4-((Zo(y_D5fGY-b{(9MC8}h zUT}$aWSYak#u|@rmWSb7S2EYSjLZ_!LW1MiW=$=EsoyV(dGQHPh(lEM3v3w+o>3V6 zC^7XY2ac&j2&T>oaMZ`Y?C91X$sTRbr#Y^*W^|9RrpoSI`*7yg%qVsCt2?xg8mnv! z{8BZ%#%xrZu**Iu-INkjkH#?-5y+2E=vPq^Xv9AW7aGMB7!)EDQ#B8c$X)K4)uL+S z@hU%bM95;p{vTa;8;@;adgNZJh@ARI|7;zNTNcYf(wme>XkO)wh&CM;(NC-R^ppLh40 zcjy8eFG5Jr9zObN(gGWqkpBm3@PKHKR6n7}JA<(je&m54P6}7x)=4oLkj)haYTa#X zGSF=!ve7OkKA5y^1Y7ExzLGuYQXkOF*7zNonH3|tfcEu(SL6FYgb3gDXnlg&zNrYQ zQ*6n#79nI6_qr9&{}K{0B>aLB5{iUyWFSmvRndK*XpA>gBJb`c>c~6j$dvevfi*z; zHYe?!r^~epMPipSt^C&bj{U>>^`iG4S+>V9Iw#IIXum0!dv^uf{)m|wZ58YR-up*Q z`=nd21fvK;GeV-+v#|fPRde`_Vvr7NZ{}@?P4)k#|I>a9sos zr$F)&#uFdhifTq}FFYTR>!>PsOm30)k4Rsq?(mJ_A&bykQtTf8eHOiH+ZsmPtrq&Z zd%}@7YGsd4=ztm@ROoYXp;6?4K_SABccI1;L7uf%PUJl?w~kYbmuim&t2L*TUmVQr z>kH}}m0;c9=-SF2VW&@X_gi4U2T`{z`EW*#Rlt$j(Ms?s@qdw5>$@&6sOKp@q1kl& zP@&Jmg+`HAQG@s_1bHj@*6xj!Xk=1n)oNk7#amw;QF%S9ekt9tn|-ZCH`AXzzc^}N z*sO*kPpa(q`#^fj?@bUIMB7Ob7r4iAJ!5o5kPqPwXg^qIfNMpN|IcP-Ks5cCCU`S5 z&^;sf4T`B4%y9gVm;$%{%o_}Os)ojX(EY}iU^F`_K?VV|jjBz@vm*~qYjm3@P`75P zRk?!oe53Gdlt}b;<1vt(~eBL`FK--NyX}t-(WAkBfwNoSuKL8 z-!C9)EM=b%hp6gT(m%F|ei5S|C8jEnU&CHuIw54V>kua=dxpf^d5+bj+ya6ZX}3_r zy;)DBB7`m}?5GSVIrnxoW^m;5AYK@k4ElsQXI0+g#%MN zBVSIiG&Gw6LOcJ+z?)5h?xS>>Xg0-JFaZ%?6gHgC{cl^Z>yQmqr~4MZVK96`n2Om zi7RA193$#K-cuD$c*-4UkV6V5vKOAOpZB+L0x=}~f)W+#&5_7J%&j_zJ_BFEcr#@f zZ@dH#<55CJQQpj)VeeSY@|aDOuhd?gt!l`0j_Dm^j6UOwED_HPF4pc!vkwmKJF0Z< zkY-9OSaM2!X>fYQE6OnbZ@I!MM;zZx07CEb%14Dp%H1Yhzl27^cu5_#FkYh^wIHGQ zW}C+*yT_S#NS>-|6)Iw$sN7km>Lvd<(duAXn1(U0BV+S-Qi)H$0djz5yz5n)oZ^sB zW464>Vf^v4iKnnrwe;nIm#EN*aG}vK-Y<^m0}11LJnwp9#hMzPzFaR3ZDwenruf;)zg1$0C7~BI*fAhoq26pIIfSMU zWN)5xviqH{NX7~cE>Vb!&EX$A2rLQX&6HuhPZu7>&j=Pqc`$Mg z)eiVuHs#nHid&koJHMCZ!Hlh(LtS0$?<8`5C^C11Dm={mz^P^QlE~{J;@mDhly%}R zWf=drc$k6NX+m(JD)jX?V=G}*tMZ&nv$LFoL^sJP#7oJw@^Pb1` zQ0D2U*$-t79UfKl*zXk3%$1aqVp$vI)BdVm%GxklLiguLkXi2|_eoFV;vzV9q=2P! zImcGIuET|1p$$%%&O(M262`T-3wdJ2zMt}+lM&Ep>GJgKvrUa<4tlt(0v8_*gGzQNYn>y$33=_qCRO=bXi_kEB12Y`>!#Ffr0If|oa-)yeWGfJyIVOEJWw>hf0dUF!T-rHBK7_JR)*<4@y7O3LWdxV9iK%M>5>8O%0!Sc*K2 z_b7f~o&EP|95E#Pf&vQa%_ot8VENpMZs*^`cr)dqQKuyJMI$R`=EUWtL;hN=ZD$Pm zmJTw1j=ncgS{M*F&p-KCe-Y!n6Nq_U;mM|;5AGVjjE~lA3${ZW=QK(I( zF1uNqF$_Jg5?M5&q3^9lzS8#lMWc%W?nx32lLadF6-tYUUdS&}QbGr(+6aE6%DcYC$}!xbP;@T;i*u`nzJ6 zG6R_J@jcnE(EDh~VM3b&>C{_NUd?VFo%VcEjN<6qnuVF#xypy zE-@{!8^MdIh}Ze}gjO_DN4N7+Fhc*=?R+qmM0T3vMIPw>Gh`E*=m;Um1MOBFV_eA7 z|Fa8FK(zkvxhV2dF;>ElJSZ5zt;Lz?(afa`aSYHtsX!uvhYIk;x1#2Hv@A^fves0a z-p{|t<*i0Gy;tSAXXO9g^hS&azo1|MBRc7c3=D*^I6WjCV4&8$c?L6@=#ahn=GE3o z-b}8upsav@Ic4!RGa10(qHaS+43nO+{sY$cJQF%(Tc|Qi($bhtQb1=X~o0)GYJ)) zbn1(>kWfpmV8eqv>(Z#-$26edi9KTrY3P|h#>z*49Vd8*zrf&UNOr1tVwNve*Nkpvl0OQRO$ltvu9MKa;A|j_Pfk1O$1(*>8aY7)$uQcJZ z%oUm0NtZn%N;J1rtGw^6>@=!tU#FkAD*A+&u%7Pa(*g4A-Ca$jm(E%uPl*GnTv{A1 z%wkfje6v%Dr#(!AZBAweL>P{A${<0pkJL}Fax*n_J@Ij45BP6DFP$HB6Cw|Eegcgu zO2)R8A7V731e3HR98B?oMxACuvqaZx8Ym?3i7qPQB1rG)Xj-N==w+b#qNsM4j@RlA z?Um2FJZAb5=I!fbH7i{e(Xl<`ksldMZ!OmHYBqESj_JU|+KW%F=1#aEv}z_ji0E3YeD#$Ijm9r?p-Dn2EBpP$HkCWV*BnqS`6;rw02acD^(d-8!E z4&fzQKiS0c_6`NQ{=6^$3rxsaZwQ!dd`$@aJ$vv8omjURJqPD8Mrb=k`Cn&8mng?0 zjY;R=uzX#;skSPg-Q|0N_M6owY`YJNYk{mdR5)l#^^~^eTRQ7Sf*26Zut#@J^ z*!nJP?tAw0$7_GA*R&_)`QN%UbALZX)=iqAJCpGX-NlHbF2t7chh zhEK>r^Yz0H6`ONnz=>>2rud3MV&=pTL_iY_;1Bt)`61yQ%-czG7j&b}koMNHxREv^V{qIV3 zBfC{CjR_&wi~m9sxIgTILKEuE(a1p9{^&wUl!ftTN)jntqE4b8%Qjm+eAE7An6B4! zdG`@BTai4WV*{rCH~7w_(OGAle5d#UDFQE7eCLl16&7@69|=xQ0bB z;-4m(*sKHAUdW20*C=6=rf0n!TfOp~uXEygKLYy#=3Xlj_va52)p0gbqT?`0J~mB^zsy23Vcr|MdYK@>f3# z@;0zKA2jfMzMer1)Q2M&s;(oGCN2*2~i!iWhAA>lR0szdbElnODlG&DvFnNS$UcWm+n?Ohl~BKqM$le@pI`c zG7#Ig_|YaJ_j-=;W=bw;>r&^^pz0<|uiT$s2$clp%%}btE7MNbE8e?)Qs>S4a5~{E z+B4P+ZbL=a+Zt9zv&UB_g`{XSU9B*@Q?;*oKwj^w8zU0d(X~1U?zi@CpAE^2T$8m^ zUM}$a;0ZLEwec~^rF@J=cE~-E=XO27rHu$LAY8gnH`~8hfvreYZHH$sV*vY1esPI_ z{U_SA+R{jA_N~jU9`uWctO{n)CTLa&31s>)_ayvZY#V$jbN-=9b@Y3G?^6Ro$&KCyE8f~@ za?;WhnD&~TxMI2D?ZCm4--^lgd>c_N*&w?{DT6EU32hMi80FGSjL?)^vJXV;4CRu( z5|8tNmzqC6YWRK%Tk-VAfc!EA$Mx)KF&X(g2J`=6bxOKfZsRXW`(Ew))(lc6@-^y{ z=6JcJenJbM3d0EZfNNH;s0C%*YmAlfGme4Ut$zqEMsqK+(L9tz|8LB_fYBZ+EykOB zf$l{-N@(u&8Bray9a81SbFVN3Jz_g2lU#!+VRtJq#)#>T2RpIzXc&s z8-c&X&$x5QK)A~`pt;u@j5kv<&Zv$m<05I5%&Sy#e|)CTnNha%*4|ZetWKZGQyCna z2;8nx75%&m#W%dB^O(uDjmh|w|0{Z*4QbU|N%1D}!gCopqlEgx|7F}8*C`qI&q?3k zM=_w$y_yXLkw8xaV||qme4R*+>UQ*#x?MzSA{1<{jR2=@w~yfS-1- zP(-SSwAgw{ztxw!O*5PQb+)byGffz0T&*2jgZZhmBKV{X${C?Kh)DSsBPAu{fZd4@ z#;IMl>^(1gfn!8UuVZ#o>H@-!D>=-Mh1paMmJNH0FE_H0JK}#gW>}kJ=G{QUWcr7x zeA-TK-x6Mwg&b{RtN(Wciky4hPq~gyXeBuwlyUDcLQ^tsuQ;L=jB#)Dy5~3_crn`X za`vW6U20c5N@u=XsT9lMs#SY$m`+KcPbK=rT4xAXd>c<_vJ8peL9i!igbGh_C|ui$ZSk9s-KnCjvm5>X z?oJ_wgkR8L&w%uWj-!1+Gny2Woj)?>J=LU`a&6c9&(=wtM||eU#}<@JMHp|UD_;hme6CyTm&^W`-AQJdsH^|Op8X{oW;E2y+b=qMpZ9U*ru}{C zteNV&hs)K-Tr#b-lWQ=)a+?dElvX!qp}AKHMoLO90lO2ST&gO&BpWllcSulwPq>U) z#xa3icW0jxp6mD7>QQ-TUqFLwNMkxD$H8~0Ru&fqTxWmdX)@B+S!L)Kb+mURoV*9_qB zbl(-`G1nw6^0}N;%W72@tEH+ypX@R$b%6BZKjmJ#MsU=202L1H@YBWnx4$n{*sfnYfQ{j(Pas%0#L@3q>e2ipL-WVf4TmjpAjPc}f|@L+YSWyn#`| zpHox)LS%-l`i>JmBJ&QXFfA3*l@}F0-*4B^uU=hOM(8RHdAIM&rRZZfYDHK=Hzkpy zxaT$y#f1VUKIKxCa?|<VsFq@;}E?vhlZcw*IqOS0!1%ZvM>W0vHvzLk7YdfCxq zs{H4Dz6_+qUyF@t9T1jisAG>xs&d^oGJ9e7&$;rkHLo8so;xBYaBw_|Bjs&q6kpiJ zIAQWr6@2=EaRm(;zkqivT-c9yX9slWZ=*pY^K2wC zgZ7u8u1PyPuw>*9e}5Z@Rwpuc-1SGy;Na&I_~E1|4sOjPdl-^kK%+S5F7JF8eTXF+ zt-~V(*F)^|75hS?I1n{<#(8vSrwUOWw7CmUjVD4y{Y>aG5S$b>SQpKQmr$@N78iS>o*T% zcAXpA-#2%2;_Zy#i|s){tHb(IzMU+;pqQD*8^G?%@h0BFWOSsq(Y+97Z`EGiP7|-os5HLM1B$RwN_~uU#b?^6Btw)^ zl^CHZDU}Gk6h^66xqA$q53IBjjb`Z<*4ppE;33j@$#0k3aLN9z0cFVY5N2fK(L(@aDm;cl{cf1{# znPxew6r)Xovcn$+Jli{j7d_8ePpkG1E7^P}3`q?42BUd9_mV{DjWQMrkP96GnQemdAUpNI8 zYtBEr{%lqDUF~nWbVq}^e@1M)P<6_4(1b`FJS#U-ym|-SHnCrnQa0TNDE0N+SA0?m z6|X@lRg005l2Tw4h)_yZnO>4TH~N^As<|MzVtdvPyU4>%zSjL+>A61c!=iyRUWJ4a z*u#tk_h=tWB}SCZN-S*GvU;oPlk}`@o!75i3i)LAFG_8^eFmSyO1;cg+K5m}Y~F!g@1>FpEm$`VTu#{iI&14?nq4P}?Y^Q%o0jm9igyn5eeSnj z`-@UNICCj)=^lJ4R5YzYs}brkDojz00L{yQr&%fPgfG%ztniabHW?v{)4x~j#x{nj~XcV4>$yj28`!r zXDS-*TNzW)?$=*8W|;jxrUCtSq!-YE+JtWy{V0idCkIA6JyvEhKShJ>5pC8j+5M-F z&Nm#s+j}B=p;y-G1RC)lbZgt$MJ4R{!%iL?P_!ETI+}fKfAje2p=muJp6R2L_@pc+ zzNI^eNZE*yl9G5}6o@e5U9D2SA$#`6+_H<2C+CIlD`cuz)BdU3_4`~4*Lsi*J z`=2y_-Abym4Y=UazpA?SP+QSOtF@|4;!jb#SUd&_J=oWfM}aEbs<27 zQVMkUZ+nYU>N{p|@KXx>5EyR4>&D>L*{rsBj|u?Y)8D(GM+J~={6%2vq@w~NVkHlP z!}ut-z>iH^8V98m5Y>*x6+NAz7Ev3tn**H3Pp633lO%-ZWy-Vqrl^_wygjh%+kx85 zB){3>ozMT~Wz^;>eoDn51JR;9jMi>7W4!rSeAEL~)H$~B_e}Hojf>j~xkqLU-TG%+ z#KSr9>uYW;eY|@ex3^4guR?V>NAT98{hw35TD1#t5?ns_KJsepGG3>it#*avhrVaJ z^;UzMvUacOFI9}b$WI6^&?yLnHZ)&`rZFvWq4lkigC~+N!GpR=k|i$^)Oju~jBB7P ziap{tH=XIx#vP|VI*xRWw3`j->{A=*j2TdiyRlj0dM zTS8HRV*1(mgl<2&3{7KNF+x)Y^^zLI6tJNFBIm^l@3*>mLFfbG)7+0@Ey^n z?ZcaZ^^3!b**kR(?Xa^s(KuX@&%C_qKL+(>suP!Dp{WP3J5xd^BvL{d0J?WP2tyel zgD?QJ4QvA@F@U@!M9BbyXQTyD8PS7zeqsg(e^6&YMs8A22U>6F9eaC+Y=tCA z{z<6^`P{K}PoHFvU&Y-zZ3!6A5h`j$j5yxZ187#TWdWLc)FJl+Z57M8;7ju z8MO(7$2m$?wd_j`G12tVoB(QKd8qe1F?%(i*5htJ4V0pe20o$v>c@ocfD4U6 z3k(Vol8LA!RhHe5jSiYiysaAO_4wTTRhjt#MPF~dN{TJlI>f+FP`nW`{{fp=zk?f1 z`niCdC8X^#kx?1zLI++fZ7kx$LW>|iV-7x{ySW#l(CWkpO$n_!U-sZJZ?pqU8J?65#Fx7d~ZMKbKp46rS*|P!+b5?)W zc6VXX{^u=?ZzzX6iA5=WmE6n+XN6Nd&c^7;GP49`On{bo!+o3Itd~8|c9;-{ptBX| z*ZN%$g;oznKT2rTUV=mG9RjWL=RD30r7MH^y`KijDKjydKP>vn;d!XkN%qv`WM zGp<=6DpAzpm7BJgnfA+4(FkR6!kZw*?N`nR~h!Kg{~=x70Jl%X(!MWQGL( z!)6<}i#O=QX}b46IOzU;wP$R@D2m|uKo+0SP2WUOX!T))ri50fIO4J}XyxnfIgUU} zY?DE!xr2MiobPId?K^eTK#|wTTyM%|CnhhURggu1t^7p zvq;u%MN_AF2!%jL(5h|*4S3cfM3uyN0NClc{eMRmE z+9HDQ$1^RD$7?b`rlrl^v{Ih>E2l?fMy1vv(0cC{5dQctv=BqWFZ|HDgbc(X))o|6 z0~l|ngw~f}>d?w`;ge_(dAd)%)zQ_>`oh7~GMfuOGKVhr6}{bRv;Ug`BYWAg&9P`^8+)t3+713GFfO+LaWTY@?Pz4p!H6+bV3}0&Q_q` z+A>}={tsgGql8w6E*x4Ve8rrF**rCdIkIAL%>kb!)a(3TE{ouQ%^3A2{gzw%kPdg( z=s}es)|!oaK^v~nFIXUv8#mX__tb4Nw36*9Gp(VH^Z113KFo&-Jp>mTg%%hTA_Q7X zu3MZzpv5n{=v8%0zSiT+!$~@9uTFiJm9M#*6K@-kaC!AeTxMUp{LdhG&4 z%@C3NqxkH;epC>xau~s=Fh!LE6o9}S7ysFf0#FtKAZQ<1wS5u*$!Qqn+96@u)()Wn z1fq%D^~Vc9(4F<$12k(J#aIbH0KpF@Q3c#OKI9ADtPONe4*r5>ZDboWe1AD9Ya{cL zGHX+xHeN!rHX!QyS6|SqtpQOTw9TJZjAv~eB?<@kGa>-Yx<1wC181;qU%rvKHZ$+b zyfykf&06F@Kj!54|M~yQ?|wh$FCzm%v-u^ue?(xIXh8}?UR>T&{j4pc&z!Vz%}ghPU@O6L_b))$ z6*M>~|GN}N(#7%FQ6O~e;Tb3(Xy8JlfM}7V28c?}J9AHmJe%=lvAC<}t7q3YlOE`^ zvI$xQZ6O+P+_&B;9{h0yOPSc6ov|%9_Kya&Y`M00mUcy`*H7k}9qg1#aZ74_qXJj@ zP5gQ^En>$>lN$)VxtkdU1T98rNr+%KEKXp3!_b}@6aM=H3%T69-&P++KxU4@i~j-LTfbi(P31;ANrs{ z7+7aXnhzuhL-7K--;gKHSYS&$+7bRbt znaT2ve#7YBwKd9lM)_wlg(gfq=z{?|Fg}HlbQ2nC5|ME$NEku08X{(%Qf4(nIn=Y7 zBo}^(F#5aR>aNB)Vo4-^BR|oGITG!0wbfT@FUSmJW|Q<8#NQ8)w7${jzFNZWNtAgn z)*PXge{Q&uLr{*B<(W3S5rLuKM4Y-DI52jD6HyA-#VfaAP7Fer-!WKf|Q;6kI&0)s+?Wi>Y{mG8_w)n;AO zcA${^)yI%m85tvaVUjb?OY$$z%;?kNS>Vkzul8Alr=`U3X8#Pcq0NW5!n8LV2MsAP z#iw8@*`fmT&$ZyQ^m_vt6k3cJp(&vy0sI;Ut-S2+`v|nG4v2gqke=)G`VEg5YqnR0 z2ZvP6?X@MwOu%=*=Kwuadk%q2B(RlXs-lOP)=Kw}BaHJopx0vhmvM;k;1`r}P;b71 z421DX0$Oi91LMsU(E7k^OC4HqE&>vEJt^MmhDNS6hBXfU7_eUz}PNx9EUcb%!SJ9R~WRE|vOhTi9{D+m&!1$5S_ z%&!7IJR)jd<7^HLe-^BHwCk?>=b8vYnN}|&=_0y+WcL=S)cZHniYP3@Cp2kvOlW4f z&;VNKpb#O@T71)j7lD=#qpJL;lyvhSK|gfE=nef=4YuVjK3L(pE2vOl#CU!09r0zm zgIMka2bIipa_^R^{sIm~Z?7k3TIfgfkk?|P0we8O@d>Tut%5>}1tWA2;->jepu!+o(>1(wq4x zv{*5NgCAPphm$A-wANFkLF=i>R(QX?V^XH25&t3t>>p8PS`pJmAt+ygsGGLZKq|$d z&;s3cDrnI1>n22X&`#VvGM<=OJG-}o#4Jl>>s0$kNlvL2CBrqDFJ*XNTZ{f(K0_=C zz3@ZpDl!m;T>T0K|Wzcr?eYMRT%|u@k z8iQ8lz3Sg=3|RKkPl!X%*^277KLLdnJ4QcBXvycmp;g0YvNI`-cfVp!U(~jzDL#d7 z1LAkm+oWsodOvL6P0Hf(&FLCCpw~82Y(PBVt6*zOV_tquceLJf-f?Il`$zv~T0ARp z6g>c;JMWAM%>fr0g%%hTA{1I*dhg6V$#9+BSFXa&XegEW>OX5f?+t8TXSemWv9L(s z;pf5OEzGIWqgTX5_AwYJ2n4f#_?l@|a6q(1Kwh4lX<C-;#|Aw2utJXKB-%1Qc4F z7@;YlrLB#)EEHNgr{21R=2geji50{rziDA%UXtgw%`;`F$ExiMp>vq;aFGHn%^h7v z2KUw)UQ+q5ObZ{h?A3%Or~qA*19sQ$V?ik-hfoN#Ge0p-q7ba;Va>Er2+CI=+Urp5t<{ zeC_59X%CC#Pg}OE9J*}O5R-D&I3urNXjUGF;n^*gnZ?9_dEoU`@I@eHa_UwG59^pO6Ji8`?-{yT5o^8 zL4S@dw_^!ZFdrN#m zN2`RRTj+ckq5ta^Ixs++qX8OD<&kg-+Q$_2CWTYj7WyB41rvxiv&0Z@I0fA$JjMjB(9dPRqztiXz#t+1oK|AhU#Q2jkhoeLrcv24h9=2kNpkrcm>~=Zwq}=Kj zyVv6HlQM`e&92xT{E)tpK%DrmCH@viXt>n zT|-1oRnkWnt^_dNOql~L)1{sR1h@!^e=SY$QcITKNvluXaIE`Wnc&UOJ`amGwMfv) zv0KLa4P8v1UBYqPBt-fEOY<5LU-oQgk|=TJ*@HQ5#_MhL`UKeT4U%(!5p?0I^tT5E zY`MlJjaVNn({6KjbJL)zAMw&16c>UR{U~u^g!~$Uiz@>BgFD@JU!-Z6J=5vzQ^gYt zI5tW=%C*bb;69+&!Y!$~ZhQ8b+5?;K5uYAf$W{C3BEyUC#m=1KdO~gM*(^;D?i<8_?S3p*&i-MYaNm$^1!`TdqFTwsygbF@^TRt&wGB+^XYMEk>D2k#B^it@P0v4mSpqi zqq(Y2RvVl#X&+r6XB6>pWyZ+IVT0(wHiZW_bGldSE?*8!B?t&@S3C@yv!%^Ttn7n0sl*7!qaSTQU-~= zTjJXzm5bJ2o`yKwr7^w0u-aNFG^_Z=aI_C;XG)P>fYJ)`JEcfX0L4nxm2@1hX9A%! z-Nu9#hYOA7IABnSP-uPed?Fn=BZJ3U)@jQ**(ev91iJkftJRfOdPnrjbjCjx3tshe z=GpjvZqXRV(44bm$j|b;mGJE+as8QDw9&tyHMYG{jeOxChOG;qr44_+L!l*s5tf{f10D{}hJ$Rhg>{Z9n+q z#MLoqeZ+e<_G&xg1Qnpr0(PgP5y1;B&|UPh2nsDl1X`eNV$O#PT7P(Z1r;?g9bKuC z#0<{=2rY2yGy`q)k(_LWlq@-_kK~Q}BgYHbzWiBe0Z}bYwIRs{6k4GBsl_^I(%6DH zEojewpgx{7X6@P536e(bo??S3l133Jvk^=2+Dsj5v%N!%f1!m~5_;i>)(vDJIxCq_ zXwAlWGbOY(1yhHXql<8S;`DKp9kq#sSMwJsw7#!}Y z{%o(!qb+A(PS&ajvAShE`YM5D*EQa^?!8e#t&WT*){>zGwpYr`OF#T>d!^u%&4f5a zi{)>S4s=2euPZ=zW5_mM8=W_4H-?;;Q8I3L+876g z7SOEn{wH*Mr4`WywB4-#-Cjv880bQawL5-JwY|b`ak|P1ffjR8!neV{&_WCezwkrr zCNdE3IKHAa0dq0lObIP#N$SwDautcUOFr$X#>%(PSI65}O~O-QvFYl(b7BeUtjjBM z}Gz7h}0KnwC7o_d$7)@gF|o-J2J|5<@#dUjG0?!JmT83(Mj&- z{m-{%aTl#P?YBm-t&cIQpqjDHS8QH~+F`;zy!v{qN82q@pVabl(L&=DhE$FLeph4;RN zD&5IR@7dzqB?lC28YClB4Fg+u_6PdZ-ESmkTI0J(EDJL534QsM5t?bu#|TXctrOOW z&qAT4eV+$`R_3?oB1TVs-Yat7PG?dd(_46JuVq@L&$7zzT3XLs;>DKxTMvwsjAvSX z55fI@d$Gh$OctTg0?uMQYblyKEk~E@7T&<7_`LyEVO`V{2fanNg>r~kk^ol zW?Bm{gM%Mh;D?ha1SGj2?Sp1oWTPe3@4=O6P2by1P`(0Dm+1N8&9p%GMD;Uhru7q1 z9kdC`k>i<`LXiiWX$8(JohsA1XZvND1p=+YgzTu{ztEyORq;dX7BUdcs>3L>7Gk`a z5?bCl)S+eODk`pjC35eRRyLUnE$6NtIewa+lRf|DiAMIHch#LT=Zwzi&{uossHKt^ ze9U5Ius7G&%(J|j-Z}kS9h*;(&PS1&YDvD97--$AHlV5>`Y(3;V=s;TVd~{UpxG79*FR;ycvmOv8qo#Q9@t?+MBK_ zPl64s0Ihr4p5CGK0HXCB@~awr zSlIRV{Zwk<6+bEnBsB@@aFqxTQ*8{A#` z^OlZxsGDEfCf%N=!acnVX?h<;+H3!iM!NeboVGmMKhM zIW4~17jv~-Y)gHjrL=4GeB01g_s=V}c08rOmpJ=d+ZD;tgX(a4+-}72!VwVqZ1kAW zOW;DI^Zm+PoJ$LGh7#Sh`-hOtZ_q(J150_L#lXOW<%vN8S+!2L`y*p|fC1}jfa|*cq zyH00LZ5B}e{j6<<(nx#!Q3DDspu+lY1H8}z-D7B1q0m}_Knt|(jrAu%3zi}M;TKMU zXc{G-QD~`P1_wX1prIYL!U0V*F=$6j4B9NtmFdM^!58~S{9S` zh4k^bejk=_tp8el{5-ABycruG7OoBmvG<=tV%N6Q-rA-)W8|w+pR?z`&_YhxHZT2c z%Ck~#XhIyKsvoVR915+a82u=rbt?xBtx=v5MYH4lZXMM@q1Co*uUSU+4Ts3l4@iGm zUH-Af$i7p3UTe?K!irn$%jx~NQjo_nR=)W2UxO|6e?e<&@`QCdwyz|7LJJ7Wqe3r( z3ynex3bkUH-T!^ALZr}$F!}5LU9a4J+e>$ zE#vDzbOG&%RkOy+_&5SuhsItwO|zLI(@HAy)g3|J-MRL%W5?>h@9t3bb106X-kgFA zgvF^lD6}*%-b@Ltq)XJHrQ#|cm**F<=gFGh#g-j@X{zr!B4rn;=z9okwoD4-+C9s@ zvg369&y2Rmw4=h^{dr#(3MlN;%O2S?^FhFm+6eaU;rgvz4${A%MMIBNMD&`!Ew%d1 zI$f|u=I>`K(C;XB6B_?DG5S$LE8P|fElygVb&4ww^Ebp6)XY{dx4AEQIc&9Gz~H&B zuKD#ZR>ui>(1=&CuEI@W`1N%C+EyQ)F{#YjPvy=2Fp#YVtj&)npL3qJ{H3_m^+ee36d02o#y__$IAG0FoT01 zTHuG1C_l_{ZQH!Z<4K{oOurOZ z?<0hW(QGmuGm!N@O{sQ)zw3R79jRPU3PB%yz(6comkUV>q28Q|bl)&*6y03X z#dtF%v>xhGhn9@1gjnLSSA(knf>AJ`p-&z7~~Rp_F31G^y>GsJ6ys*>$l5;{OO-3#38Esm0FIK@#$go zql8ug@@vTVj5vur>rAx5_%Cc+wmSQZA%DEb>i7#@AKJ7f*>t&;o@5oz*Ay@yUb5!e zZ!N%{wXL%IDyce+tf6eR{E6=~+oA6}Nwu{!R8 zTZUin{=u}CICAaz*eTnfjBlm%Vtkf%tzL>kYdJ<}N@#rvM0^|ut@O+Y9`tW?FYX4UdDk+w_UJ$JK){)Gx#IaJmURxVl1#ZxZU3Mdkvjr z3!1AH+5&NRhiU68nVmEzLhJX-gK9H#Cd47C`bk=heb3GSqaP)-K3{@Ei;25rr%I3j z7rRWJbwEzzF_EsPt-BU%`(B;<%t=zt`lLYqA2M4|N$ zMrcZCwTL603Wb)|X?xcYU2%mlkHU*j7UiC|lq=hll3yfzh_Fl6pOLXa-r9L#f{zvB z+WUPaqGQmiKu-R~wWoE=H6EV|D|lw1MIEa#D*V@?4luDCO*TMMAt*0FcT>?VC@+^H zyaesIpw+l|Id|Gt&8SYP8jqVO0M}rw^gjX+XdULnhPRF#be|!~j@E#XjkY0+X;KZy zwVL)|a1aJ;CaJu62VP4b3I-DXoS>QEb%j|6WF zuOLOf{rko+CGsZz89xB;AOo==rV0h1A;z020azAH9e{kUl5utxUb`RPD$|V^O?Z7! zhD|TG@|u`Q-<8-amvdflAOF@#tIi%-q`o%j?n*|{`S~xzwup{|HjRk1{m94)tJL|r zj&VD7sqwe_V-LeF{ywGw{TPeJzKypQqaP&zn|0v;WZ^D}l<^Umk;|h)+O6v8bu#9l z`mrGXIFVpgWFse4{T+chyK76pJInZ1CB1vDGfX)axyA2Q3VIPX4nXAPD~<82Ema`^ zpU_nY#L)FoBe>8g0KuRTApo3z_cJd7K&}h(9!Mw|RAfs;=QbG3j4n~2>7i>mWbB!} zMULi99+O&xf`oSc9=e+I?DY|rN;F~2B*o<;`^qs(jZuL)rGof`o)aj60?-&EG$jCs zvJhW|0q|+&MpyR`UBXJApHK4@4rbe&V7_?t4Y&Izt^g;C1O8vw6;i@FwQYF3->TJy zTK=20;Q}Dz-c1u!fKmuJi*e6ubi->ILLt!pr!aTYh8MX?ZFoTwOd#s(i0YudA*OsBTExM{n14{-eP98fejz%JlG~=Z53~S3$5+8;uGQ! zbhZNh(zVCV?pu%1&knI2`Ru;II%GahI=fGQ`8>|PooAT>7hKtWRAlBHZg1sB+kbv~ zn`rfYEh%~~k5`ddw)XAn;3w&f4jd}kRTYCit2QEsG3^^An8}&eZY#1kmevWzC#A28 z4oayF7%6|D6{Qpy1tN@6_cr&wmO4V(N3&~hUPoVX_~Vu7><=Smitjy>qF_ZLWeJ!t z3@s9|(CS_n*S(bSeuSL%EYS#K^f4sZ# z<+sN^JKKwN0PPK%O8(tHo2Rw~U0Xc1#9|6caU@CX5pAoFa*B&&fW zHOeqTG??>*aH>tmgPVafDBRN$uVv}a``JM!CUo-ck}ovN_$LapiHgS#5*_02LrSIJoq6}EPh$`DHR z6Hnq^OuS;~JV6C0rGR&YaPCA`^wbebfp+J+9g|k{V7Zj~pQRKK?aA8RkhMyx9@Shm zwg9*VGdTDu1r7Z`lHI(@cz3`+cUeq#ZCa)F>AKiPL6dD5Zd?!|uxH z4p<-Jw4nXoL2i5ptS0H64Ek2r$EK-rsYQ-WGq{jk>h`)WZrQ)N6qHclFY#0AJ~9v| zWl~T|ZN+#q1*J;qZK+eL&vlOI*Co!o9cK6-k>YZ8wfd^^AK5ctsV6`V9dTvmpAmhE4z74dzAtZ-4Bf|zXdg7c0Bgd_J ze;~;gay1c5e!a#O6WNh#TQ#P%>sJYQiGJ(Oi{>8qkCeJ;<2gYEXo3Q~WBY^EkkAi> z8t6XX+zy4B1_CwE9yZpQ1T}Ik;`!~u@#N3E?wPiLOV#hMzH&oX{H!s9gP&61hrkl< z9vZZ)$p7zgcD2_4T4&I>pt%&;3Zp^#xN@nAX)C;d0u_jArRjnoAYMDMmQkCK75Bz{ifyW!4_mI~BimF~va{9-{5{wfF(mxLPpJpUK$N_w zKq+N|@n%X&@gQ@7%4=NNjjnTI6BpX=Qf*0N2+2QaPS0~hOkL!ZkL@VQC2nz9dgDuG&Re`pZSR~l+GP4pTav~;!St&l9yxC5H@TE7 zY2Ji51f8uwzxrE5G?%i)=toJZc{xx@?UiTmO}lDz>uJRa#r08KQqH%#H_lCP=a==} zbf*0$W8Ss)GebFBqI%~j@$j{quuxE{;a4uzId2s{DFdD{pt;mGjFgm=0;52LQR=~F z&l0IabXqewcAskfNWV_B-XQ;Ve_&~yJ`L@8y55t@=x%d`y2l%jorq*k;ao9j-2IavO{MiMt}oMdU62+@iLbY{e&^uo4H)ta3##sS1|+i=7+%Lt_q>=fH`8k znUZ)?m#7o3+Fd#}RK#YN>aCPVYxa9*DMfu36*OnRCZ779Wkz1~T_cB0Bhg7O^hrVj zBW$7FN7_YHYGnBcS@nJ?`2Va@bomBo>atX7F<1FGRcaKf9qRX;7@G{me@Y!2 zSh2LTDQAFh&WobankR}kNeShO6jQ3tI@hWaWXJQHVRL=h z<%GmG`3cT>5*QtS=2WQ}QK*%8snn&cDcAjCY@^nVs+iU+y!!Pf^`SxvsvqN>*Um9BYMz}e5*cse zVt1EGzHU;KU&dCcAAH7T2@PboBJflxx_9gu9?!2z(aDE@*uhdMTclF-asOrOdsHdT zcf0?+C9xjIbkRaY$(EljJ?t8dobSI(>Y5ck!>VFFoJBf%SzEdE0*q`{dO#HR&&m9O%8HN`uN0=RlJr%E|g^sG`h{Q4^oXU+&PAG=jy(>sM7fvNKcKm92{iEX)H zxG?ql?TDSnh0a&L`4ky^(yHWs{SKQKIUap}1Yh0{Pie!v#=P%Q@loePi6~vWkPL*1qih3zel^P~{ z7pau(E(gg`sd1qncZBuPGul5yRU~h}z7qLr=8uG&3CTGZZ^X{6V}736rBZyxQvJ7A zbSnZq+5R@+(!Jwt#B`SI*df`Wk1e*0|4nwfc4BbK&hWn$9Pt!XI%_4eb9o4AkNsZt zKmjY33g&7Y{#A-+#6jnTDdEYAINDIH(#I{K<9|gQ`x7VOR}~%{<@*mSL5)(olPt5! zvg0DGGYm)kyQ>b>B+nnuxtwlMp1nD4q$ewC?dr{pIGf+1*4}FUetYO`@?VQud0sxy zeWq-VCo3L@ZsU|^Z@!9>U+9PT3ohg8&73MVDW+$Y%J%EOkI_CQz+89By(LM>9kDtB zLrlj-k7Q(PSt(8*H!?x|RmX78Pmb;fRie&~V#qg{OQ?v=v~qsY-1kwQUS!{p7{)ij zkDPb>ocm1wb1daFTbIA`S%^xrR4Rn4e4HxfW6x8iP8o{@Z%r4d(XE;<(MvEw!uZRj z7W1VZkK8}*j@;2-X=GCf>9J~djO6W3+e;lCgnvME+TiXgrF%`1zg#{ZEW=W%Z|K*KlLj5yrXt>URvHY=J}*ICQj5*v5@=vDN}ogOB>f-{y5RX zZqaKvzgt8&R8m#{#xU^t26n_bY+eVOQlwDWtvl^mgk@$#Y?3! zuK##~REjubZMIX->KC-;2^R(%Zoo~isxrHuKy z<9EWlSGOYYR4Kam&r==CQYm|+QuMLgddD7BigTf9^8Z$)=&ae*jpbLR=;UvePO(%f zl&f+0S1Fz|41MZ;gO7|It{yV?Topf@Y+D2CB)m6vFMD1^(7q7X~|F6xRJoT5( z>Vee+YYD9#m4!Z@I=KBmvDAdKthYkhM(8a52mKuwDw=rxL|OJBN_2nh_=cnKr)` z<(W9|a*9~U5liu< zQtywtcAJNKET3$YDodrpxyr|>QXv&QRqDE_=#L56L7lxkmDU6|y!iAc=SFIVSKAf$ zX}u$yTSRq=jiMrFbVkgd(x5Z{;jWdQzRiIO94dwOwreGChT+i5CX7*;lO|4o72cm1TbGz*WUO(=;Rk=MtM^q446JH3y8PWXj2 z+no9xR~>mrZP3CN*SKnxzQ3uI?X(E~GJT**owbc2k}K2yu#JJIT+rRnZN~tXTsR=P zppQ=|tM(`toI#E5|5h&OtnG9j$gf<`$v*~PV#!4mS1s``7d$6b`qVi)tyn=#2bu@y z;}=8Ce+4zf2X`FQY@PQ{LCv6xeZA~ZP}6=}sK4I7K@HCdl{WI6RC($+-4M?Q+wzDM z=nn9kj?>9^{lp(>?&|ytw-4Ye1`7IXI?i)C%|uz9GwL*J8$&c#Z|0PXrFlKeMVjA0 zrTjO}{zFc#FfkB(`^w&Y)s#Brko1~0LS*#&K$}~gPQ4c95K3=5B660^hk-L2ZV@ep zt4NC)WiPqaHP>3Aqznk9ZU3!YoH;y48`b_lFA&k?dt!W#u^gq0;VK`eT*Srjl#9DV zM1!TWD&JJ?dv?8g?vR4C?ej-1pI(!?ugJ1h_;9OK_P74Z27{V%7@y?*j(Frun?>3` zLvk^cy+D-ye!TN&KAtC{bEzD5j}eD*8OxPRPPw34f#M|>S7+?3P+ui7T7I&B@yBf6 zm0v7Ee;BX#k@axO+-ab{R`uC3f%IZUW5Ji%Z#UJq1fH$dFYe!7mTpKz=}Bj(u;qgF z8bDT1Lwq~NU)4tKy~jvEnO?<}Y0jW#7y5UeLCr<$gdYLX*-^QAb5&j2ErhDW2Pa1~ zsh2q(DDSr~Y+QZ)lg48Q$^)$9euy%>ihgZl;4i2Viu~TK2s~AauJ^0}eU?fMM=C`h zpLEdfQKkML1~qinO6`pJRVh08-1GfeU-Mnf)j0gC6wk?!KJ}fDM1F%BI{DM`L{?DK zRmV<6t9}b=x*md3QqcbO8o=RyZ8GGkzjRi2TJB;k5OtzXOCN7Pvg21!6Q$|H(%+nd zt26%LQQbibh0oXCD$ADK=sIJ+^}j(4J^p$9cdl-g6*ISI zl}hm&q;&AfNdF-lLq6`$nd5x6*fXpowO*oXK&a@DaDmpZA?rE?JVZ{H>E>p-`qW;H zd^Fakrak-hB9Wt(`7h3F-q9)F&}Mt?zg4Ma8_sr{hk7g@DU!kpYT~%c$Ei|@>O58I z%}|k`vqh7~-sqQbae>4El~q2rwVRzYH|#T0UVclfTHx)mHZ_5|RSL?=az3GM9b=uE z`^0rusk%68{&G2JSPCnsS<976PL-ltf#Ri7*JdoMP>*e#UAv*Me!Fr+yZB1?wl*jd z=>1vZ)%>?j*-7SYjBB+kpQQ{cO*z~#U7sqJz{T9^J#8Uiay2vYq zb?a>%SEe~tDs4aNqP%XsUAR7_9jTN@N&j@2$}kC)fJ))_i6Q})nHH)t^|AMDm72EP zuqvB*wmFjdb>!*)Ql-2c!@3oL=c)_c`xk}H<1zK~6g4{ercd)&qBa7F8hxBe&goIq zI76>he=YRtvERo%2w}xi>$w_-f0d#g>6+;F5ENbNLwk!@>kwVF05RD;UW0SKP{QZ0 z$qr9JrL+3@Iewu;_q&8VRfZX2QnGcHx$Z*2PI{?W2O{9DXFQwneU~#=B7E&7#g==-tQMWfEcMLR25* zP^psl<2UMs^OwtOqZ?Q%wTUa2oGL}P0>x9MUYH@3^7Or#sW99+AYQ|8u0ugynDo!H zn-W#sX|nymLkj5_{;RO z*$pg}+RT+{PL;Z3i~1-}m9jPyLn@{ERy|87ta+8e3dfK7=}kFj@(Vj9G{c?y2+5|s zD|+iF7$#WUSKH;+0ug^(!Z#~!?p6ezDn<7W-JB*K?-JsvQgrhCMWQT~azwKjef;%h zO^K`su&(o2b#rC!D{) zdOoJB(%Pzz2+{XI=FSQ9cK2PdWD(-71tLySyZqNgji*Y{S#7Ag&RQTM+3kC2!DS)3 z?<}6(MWiuvLl;{D%din~B1U?U70y(9qV9`&Xq<QW!JcYmd4f)$Oa3&tER*5EM(LwsGZ>Q>Exupm?a1M&^t@73wh( z5o!(oK2=BePndi}dt+R^PnCs%oVSo|VYFhPK$?-FXt7TA{@exum2YxoX?Lz=+vvnJ z3W;uw<5H=KQQNI-`O9?4a*Cx=+qp8$sZvE{sFU(isq>jXl>(x(%DySKwjElw!j3pv z7<;=|A~9xCRxc0g){r{=qu^3%5Zo5;L^3k|OOt2Di#RmiC@4ajJI6xuLkPoVKll07+H01~a}DL>11l*pA!PL-ltf#Ri7*QfuiR*!By-Qrxh zXWNi-HX$#?Or|Wl{H*HXmg73L9x0`-T3grE&X)M1?*1seMNsQwMC56S(t^+k)%G7l zyFLVrUMRter6xu_NI%10)fzb9&33!EGR>({Poq!==A}~SGM{P%L=W5FET>kN+<0%= zS~2yn?MJ%E(p7=uj9{{YCGWO7Zulk3Jvv@>hgMpJiA(F_O57 z@ZavlpxZF-`2Z_~ zB>1bP!FpLN#oNtQOZ+Py&)JI3iF`r^zi}Cz{OJ7*R$SIq$8*Q`9GCq+6i7O&m!@7~ z#btCw)5k?W&i{(bY|Yk>d?a8?L=Eu{R_ggnAL37?tiINlW_PMq3^IZ*HeEu{qf+$n z5C4Dh`*Uy3MGet#P=>X}wTG)Wb1Gg=MbC;C;-{y?I6P!Y#z~h7<#s2Q6BXRme_{+hLA z+Y+sf7AZg5I`3|O|LuFyUSe)PGA{pnf5BUChiMuvU#9izg%XvXHQwVrZq@OZ%VUFA zu;Q}4T)E^_Jh~Mqo{IPUX|;M()#HcB3WRl^{Ofy^YQuHB1d2BXOMXu~AX}Pxj+`(| z;KG^oEgkR8B5F*V)Ka!A?HJ7z9J^Af%I3GYOst@ozf5l!v4Rzs?c>Tcr{aB3?Md;h zcA6r^n?v-@{xl+#u}C6{nA17_v|wj#O6P;sw?H7=$+|>UFtjsmm&}Ww|E1zZ6B*r# zK!+Fff8CaA=-z+s_&b(LjY88Bef;u#*>5V<6=-!Ho_efHrDXov?LeL?MQ2TH#|M^5 zk=;rLJ(bLL=P{N_rEoP4|0+d0@~pJ6Ti3ASvaU)yHaY6IxUB1TAm`SKRevom<0+_g zrOi-W$K&#!=iNg(`N6S?tT2v_)#>ANxm$mQap{pQTlNW{yN5qx0{-FdVQky*Yj1YF z&SnN!Z@sktUzI|gi07|Mh5y`}^H4*?G}N);vi)4WnNy`oVtQ7o`2l*#`3c5L%r@FB z4fmAW5qYB1V8Fha?=EMZSG(Qzsrb}Z84+zpVU3OKjmU>Wox5su?G5sh9MoU_nEqOc z(Ruu5%;>$n7-n5}4`o?#+1JN!*{{muSwdt?EfeCed|Lh0SSppuRX#6t685J(=D)Y+ zsZv@tLV*F#R;LW|9yhvd>D~8_63$yRDSGsOJO5sL{0EuZs3}J}hw8Ld*1xlSmSSkD z5E3=X=07UsCFwcdri718@N_GT)YD+8)B&zsa;g;F3KS2OQp=jYvsyJuMs#4C|E#JP zPXe#oy<{{b&lzgpx4_h6Vgc#px8{hPq7V`EF8Al!=;x0G3;O%#+TFK_4(TVbP2!hI zEt^KKaZQX8ddNq}OJ_RzsRm1>4svChQ>A|7pnB#Nmz~XAHUg=XT3_)~Z{M_-hxhsR z=CRz6_Gwu~`&xB6Mk&?oCuYVEtJfusB{~Wm|4Ws+I4?#U{oP-?NqJ8FbP*=)$lwv6 z&~x)|^D~{?+x-$t)JCIOj6N=uJ^P!eIXo)fOMd|H+{x-aU7|KT;BSRqJXMO$n$W4^ zER{OM)j0gC6wj%jKJ{>;i~N2ykxp*-^&;zgRb7=f(K-FM?^W>5>J;zcuYqsxFiZ07GI4>aSy71qf7-%T* z{PC|+`KTcRhCOH9T|2_nn>lCkhKla9IQ#CJshwcGRB5BQc&c=ar-x%sK|rgpqo|+n znTQkX$<835Z#F_c;cDZ=m%Ytf)+w--{kql{GdIP1vNcHO;f`*z3C}S{=Tc}9{`5r} zS1vhc@iO%uX7TLl%c@l)qs$rZg*UdXt+FdSTsWp=N9KU$t7k$#G7hNfRJOJ+sP%Rl zIyF^j60=}g-=)P94WcKvwjNsWeWc27v-rmLB>pm;d4Dmi=PYhz z<{S`pyFAyA!n{wES%v9p%XGbaJbKBRmdBuBOCNjge)8M2ydlYWD?KgOFP@jwH7$GnwP~5B;Luq+ zCjE-vSc6V3U$TNViyz}^od4r2PM`YG-o31av+noi=)d!u^wG&{-lnh?&bsQ@_57~i zHX%Ou+7!zD?b~qQza@Pl82A1a3D zEIz_PpzcAbPiEcg=R-e<P|2}I z(lv{(+rr1LI69Y+^;1}vwI{i9$(|60dk~UOzUL8yD95$9d&lJ?$4?$lK4z1r(7xJF zUGUq9#hYJ+2^fpUJbk?R)Ro^P;EwB= z&D>Y!e@_B>90f1V9A?ewr@0!3|2ds@{B4q^OTAoV1?y6~s}>$E4EpU-drCz@B&!x) zw*7iE$M>&YYV!;t=&a82U&kYe;5nz$$;(HrXI+`o4=T~eYSF8HU77nYj(m26p3|3_ zoQ~*uPQST(_?ju>DzkMjho;P5_37U^9rYq!KmM=G3sFOSxKqixe{_bcH*?-U>YUs2 z{iBfq2FaemT8mA;4A@z^$4zkaD#aIto%a&=8Vc985hvP*x7dj4Bz}t3Kh&EM7b(6Z zzxc3U+K!bOH-!Ypt5Wgmo!S0c?MX|z?jI?!*Y*Fj%Hi2m*lix-Ia=xRdDr1ry`^)N zk5jW`)VpgIdzHg^guv3R;ys&T)z) z!kPWz73(^f8yj1be%(JR_mZp}AJ$^XUoO*h@mtf*a^;d!v*=dfsadwUZOmAtSs%95 z_c;>h(s!>=`N%?%3x0k%r*nO6g`*xdtS&Ihl6-d5VQR3r)b{e;Zxilh-TT?ZN?u_O~`gq_S>EABT54<8b z(KElJZ*Fo|2;}qELZBXN$$S@n-@*m1TH;^-c!~;LYU%#|tVN0LmBv$4=;RxS0M?=e zy)%G5KDT}GuSE$1nV)_ek*GxFnkoK6ppfFAUSu6yncd&` zdNYUqJ(WP-Jv}326JVIE;jOmV^rWh@zU#iYVcOwxnq{NqcG#|*bw$U%IkfI9`F!M$ zL%_sN4;p$0ck)2y=?esTNXELQMraRv3 z*flTI<8zC%PHnbLXLw|#^Dq6YMEW=VY41pF)JJ|r3q7{NvX3J;CY=yV<`71)?Y+QuPyo7Pi+ ztZqf%DMEB#shicv3c_5_d`chZU#j~p2wO30@&?wh6`lG^gf#vC);%Jg@=9lIM`|07 z@UqAL71|5OY*)A%hkwnY9ee6$}_B65ceyifU zZ1sd*!SR(J|BcyDFXHv%DX%Y!08C1e}oI~gYGx( z)h~{S*f{8CuU~ty*_!pokNc_kKI=9Q^;kZQTqBldUE?Yrr)C+?<*8YVM^bfCW!YO( zZ3jfH+c{b{EjwiWissn&Q3rHmLiLC#@r)sA{!y!Q-M`0X7bVVVej0b@(yv#_u{0~d zE0(`pPGlOhG%J%UmzdF>5p zr@Hqxl~2AW3)rShGD5y+tujd40$Bkf1NA?5q-6BT(|^>fpVTkSl6ltcKI4=6bpDD^ z|G<=0gzLPEz|t&-GSp#tY1V1WovQ<*tjP&&_1EQA+qH-L8#Zn;*r*!nm;34I@)Bvr z>d4x4(L4N`+SW>@%w}s=dn11_8*|62ZbhJ{Ui!ao`_$;MAbbgb$!Hv!r0C<@TU~w& zoW{L5vW|7jjAKdP!0&&mS#;L4C(q^)681O>P6+R0-Dk|=YMlS0X3;qjT)&)kecn}R zi_L<5yFRZ@^^Kw{?bVFqzclN@U(+m}B1C6(TgiG>j3>-)-zwSFzhb zPqgn@gf44s(j9T(UAE_hSDqv1{VPIf81ek^6d|4$6LiBwp%@Rf3D=X-JbQC7N>7 zX4Vde9IoPp2Oo`8*SxB2l8?PBn7ZSPNl1g=t0-9p zF(UeV;M?qURi{&=df#`~8}~$YHk^#SEW*|-v#E3F`;109^8Dp8AgzU@UlEtfl}k>| zqFaHdW;x`()(Z1GAXucApMK-YX4AbR1uLY-3@-I-e-L&tLMKePuD7JsqVXLw8E(qr zx(0%mJq+A4GfL`3vR_|QS<%bJNw__%k4@KoMlQ|jOL_BGgz2aIurw==cM%+I(c0Lg zC{#vXnsw^h)P%q&t6Y!f@=Iz0A^jLXCC(pDd`%3^@u=!cPWbUP!&c&`ZH#B=#!oH# z@ZRb`SAN&Vre5oQ--@SZ(PLr8^U8x6Z zJ>mB@CY?ND=M$F1iLkrFnJp)ONt~_0>*=i9m?rCX4D4CrPHz_;SJL}^wyVwBJ>Po& zD{*M}^ZfCYIG%I*P1G>K7WFL6D&XqPoSJ1_(X(c$1Q_qhA0g{&s!If^C2I(6l9d0E z`yjJwMb@*8b$cjrUB%w;LsNdselPp{fyJV&_Y3^St&+~_pZ{}`#(nSX#T^dEEySar z{ztRsjQ#oi&mWjjn#;%5F}i#&g)LZdRv}mUI5lhH0iK$*X%wS2;QL|s#c5Z!oUEN* zad|;mY4rC;53|=gbVzSg-c=LskfJuQZIy68(%*kWZjEW&ks(;Krs%OWYx;$W_q0(P z{qwy+I+wZ{maGZCh%1+znnkw)Pt6*h``UB`(yT1$t=2Jjn!YSOSFQB){K-omCrgv= z)UL05S!?%NR;U+w$ZX>jRV|0m(fGv6KjZ})R7c~HS$5dyyEvZ9eW1BPt8_Rlq;Y4`9+^SuvwMH8dSCce^r&P&>(^YB{IfY%7BKcsyJPHb zKQ`)kAiN}ojWy^Tn;ZH@z;Sd}H++M-VekvL#;#k`>s9*kSA;Lar?A#H?{XF4AJ#W} zY_v@WUbFhcJ+5={-ycv{;N2gNjhB}DjQ)|H8t9szbwo^#PAsH={zreQx2NAyFL%Qs z24*Hh42(^Tj1A09hMJq0o73BUXU_Ky8Xr8@cR`?!x7TDp{{^0bZmN!R7WjI5sRsF} zdipI1n&TOyx_C}tkoQv6IYFv3ef>P=`1<$-23Z&y8XFqA>8Ywt^bT6OV!@L6dU|@+ z^q&k`?5{s}!IC*kS9p2P)nDotICzPlchCaQ!3aJ5{RbQB8Jg)C=m#$K)c0L57a8^O z`Ss90N}v~qzSd663Qis5*70L?oS3a*k!K|lfO1R3$-hb_oL-h6u zs6ZP-Ibx`gmbpu%`>9JUl$>0mBcbB+@X>=Sld4r-mdAUlz&?TH*2MDdciSb*G$V4g zI#!%-d0Mj|j69qIE|Oy5stpJ83x{<^bT;3wA!prvKhMlYz!WtK{i_h;&eqw#l7~X=*7E79ltWOqDdP|a#0 z;=XnQ!$(zox`AFm`t1>UhX)B4eAJh-&5Ov|QPpz#*StW>(-e+*!EDB0{_0(<5EtPg z?;^0|DaHd;3NLv&arNo-z{pH;UH$gsySHS7_e&cvp=r_G@yjQY<+EE}Z~x-%v2bnl zIPu8gSDLP^?79ue=QsPduQ%tf2peRsvZkX)Tt(o2I^sFg-9Zg}@eUq!kGW3GIqH1# zdK`5*odISF4oZTHOg4PnoBu9uPn%R@{o8>Citb6q&ZZs3_O+|g+LY}u_3=M9XMGNB z?MNxVyMLipZmwLfe)W$wqYj3Cnse0Ajly%(jkL?065y{^we@;H z*>I^9s`dROb2K^!ESs^7Ss=Ihtny^>du1xtN~1grcCmj?c|Z+`xENUMX7u& zS)I)|6CxpZE|TL$M)0%$sPXY;8UBjEl(k?L;Ti8DutwcBdMe^M>MV8^21j1Y-Bm}X z9h4Q>H`h_nMM^^Hxj9i*Cd2Uf)@RSSwRNh)PDVY?t+>tJFy-3bWJ-_XKmHVjj__B6 zlg-;$qwYCZ5&n;(?k?(;Pu%cZ@m_G9nsd~J&+U2CH3pjPVaPHTUKmp~RrhGKrbt%b zal`yhT$|QE&8Bh0?ChZ(rp@24-CAU$lb-i%uE=RkJL>zKr1pIk)J2`n^Qu>_*nA({ znQ)q8)D8bR{Y}H4_9gz5E&p@X(dFBv$6$@Rmt5uJ9CcgOd5=1OH0t^aswB=d_&~(J zS&$q4W5&lJodNx=7KTh7B&T|OW8}9RVrQI&oYJ>H7a<(pn0BM0lBk=MpGkIZVrkuE zdeqtd(z;v4oA}Fgbq0Rc=oMF{IY%AcC_G1+D2frmtygh^Ph4w#OVE>Iv>hI|9 zljuK=OKN>|`0M5ZVpf?v{eqVSSqq^fcJV2imw7L>roBDTe?=!UyvHAA$n{5e_h{bT zM^MfjRW!}jj)HQN(4V0B>mot9xyYI3^>Ko7;plJC>?7MtE)G>69k|ee%(hPgteT{2 zR_uoW(Jfo_AEm1rXNx}Kl`TVw5JWq=F=EF(>^bp`6W5PZ_Y&4%IRcR3i212X0qh)!dG7oI7~_PwJ+=VEM`5;o`jFZ?8r2-~ zO~}Y27dc}-JeGhlE|^zsBY+r(nRu!X`D}g^m_kjQKoGZ)@0HwAQgt%s73-Mrgv|nz z1Yy?{Z0}>Wh02;r5O+}eA<2Vb;|$Cuwu`{d74zU_i-4Mm`L<>uwS5*r+(qf7XHzi8 z9dlCEP_XmBoTYC8)EvwPt`=~E%^QbiP*Xh#;vVwdUoHTe^DyViECFgh<}0FpaD&ZG zdVa9p2is3b^kT{{B#8USH+qXDRQY0-a1DVQi!t}x83N%;FzcKM0UJNeF`6}0HhN;N z1o_TYXTT;TEi~t^bc4k~nB&j6fqF3J6IpKXp3P38=P8j8f_Q+^o33pmU#!4v{!J6E zgkrAO)`IXb%x8?Xz$P5?ma&c0y$FJMh@NO@J+!gJ)?$A)D9uYr)!hY|r1dlIn{tww@s0l}!!g`yH4QO_iW90kd(4GOXQ+ zStn5$#w216J*o`EF3i2lN-6Cmf_RF~qg+=@HtfNy^xg;_Cu6RbHHO5!m`$~e!4+N7 z(CI^}Zc(372;v#yNdgSGbO3Xe`xr<(h*>Xw47eV`Y<6G_C>_SU?oBIo@(4jZN4`rx zwUD95Fi(@#1J~o28^`K_(h1DvGxea1&2|#4l;0_Wc!7L(F-uA7bj+m{9i-A(%wOa? z$uc%;YIl+c&tdz)rQ0Z@3k2~J`L+*iAc;$upFOdHGBzKcW&;N=V|!qL4a~cOS#p&P zC|t#?C2^XvyM~_bMCZwlttKV1Fdw@x1irKR?dKtIE*smue-44*9L$1!&rnnzLA*x3 zvc=EHhXt4yib=q^Ld*fy5)fR3`TQseFu#G>dE*Z1MlnIWLB7*kZ;%PMG4EmwfZ#ir zqfG~Z`CZJLp3SCi+#`rml-?-#8JX|^^M-RI1V6+KjU<>q!kjM!(8}gBQO~ILPYB{I z^4(|9K~8^;`Pd*!5PN~S%F_}`*sOib5>~&&_R~H^)QMLFQHFdiN;}CFrI_;{Tf&UD zn9HQBK)eidV1Fxkz-B|2gVclyf+$D6YiHMyD(^9;y3Am7vU%*@8H~dpuzmIE84RB) z%v-~+Qy)K~yCKN8HB1sRK4Y%DbrEKL!CWU<0xDlIuL<5prFo0-Z)USPvevInD0`<%wQT`vKV( zkfo#{TDzlJ#MKBI*i12u;S$>*SPvy|O+s+R;wd_?J``;hQV3iHI(&QK(cIV#&3!g^zVHflT!lfk@?xJ})a zC5cbS_mFrMxknE3n|xzfFOONW));L1VQ!Ez0Z?G4S2Tf3Y|gj4PrXqjiE4D7mmx}U zLItzOY&Y1gin%%74aTWqhP`eet&aJf8iSdtK@y*l?@6d6{j@Qk`l14}bucR{s)Aa7 z%*De8!#g%_CbFo&fh6$-`HqdKA?*fZ{yJ|U80cYsnK2OR**rkX12Xin{mt$#)Cfb8 z_=r`KJlK6&vB@}Lw?=3J7P!xs7 zmY6qG>q4Rx=GXCq!4+K((&>jQ+kwO|%<4v|l&K9#d`EW2PsJnyeHEH!FV|0G9h+yb z{X|}L!1mQ6tI6fVF;BQUj}mkwi5irC=)TF+8T5uQnqBk5seS0hDl}&aL{M|aV7_xG zg3?4!fztNZdrPPnh-(p>KWQS<(5@|-KhCy>ooF?j=F`20!Hfx*1@;XC#fg}ISU#an zOd^Rol*UV;fm|^avlM86&os;@-3(#Sbj-zT4B;D_<0ete2v?G*N4^`**uWS!%zB0X zU@;rBz_I{nXR{Hp6ms0L{lo615bS~ZbXf(pXAVg;pz}OBbAqwJ3-jS@CCX?X=Dkfy z)DJeRX(>~e=VLq64`eR(A&DQzH(E&qEEi$k6KxCweKE&eG={Hi-gCnk(ida9hBw8u z@gs?!$k#BvofKP&*<_I%w6S^rB{|3r#P+%XIi^c6Ni?GLUOS{faXIDf zGvOYapN*akTSKwk=*UM(HJl`xknaPXX7bxg%;k$M;cO)4!zU~uCG#fFoZHTe-}1_gKMyT=A3rQXe~*!p!7eaYe`}Q=BujaP{!s> znnU5>Mr?mFawyE(gjs6nZR*Enl4wP~Ew=ObeD_7F!}7zJYgdhd=|?bo<%|J^ zG|Z;&#y}~XTV!*n#m7jZ6Zu}e(o9;O#5^8d%PE|~Y;;-~O4&RsPZ?5AW4i`}WE!NC z1c4$sg-2p~6&8>L1D!`_p%iStf%)Mg3kWR6%-C%KW;Zd{EUHE(YA~07 z9}OmGsM2Xfy~aQzn;l;Qb4@)-2qXK^j}%P)iJ9dkwN4|Gf>%bjJ%(pbM;iMGi2@7+ko#On7o2L(vp&K#Z z9$G}XZvsLMok#Ge8w`%eJVxUJRIqvd%nNXME4DYTN}x=(1EG%6_tShw!cNSeK^AJ+ zTzOL#jwE9H>e+o@!7j`F&- z%&;Mw%G?ixCi2ZcO2GPqn9V=T0GmUYZKPcR4r3lMCXw2D1PCpZ{+jY5()}1_-^6Cp z$_bcZo_F}o~#Mg`mh zVh}pd{+MRc<^krX=afM2A!dztN>IyY=PLu@!Xs?I_$-}rcmjkj^4;@N1f-s0o_xv( zh!>bok2Z#UHY*96z?zrXUb?M>l6?)tVC4Hk381nRGb7U;^50^gAO2!kgr17&w0H@~ zMV}$3IXv72QqkucXx?_l1-yD;cDcKYsuyMuCdhv2Q#+X}hIw~De@GR_T#z~dyd*Hk zs|^GlNz99O4}{ljzV@hz0%-2i&(AkXPzzOz6WRvBO(oi7R_4KY{vih-~Z+dj}2 zve^t1-cs+48H5Eo&pW{i^1K=5aW3MpdI;u?lO@5)9P{2GRvxgY-Swm@1Uq659G*l`P7J~t`G#*(gomRsM=zQP=f+?@bbKPrAB%Ze?nD^qjM*(U zo4Vq{Aci5|iJ!$FZUW}Sajr0bBIYp(uAn#xbL$CLc+2Lgn-{>2$=L3g6vezcg+bV$ z^Ju5Phqco&m%Nf>_{_jOSW=pyMbBXnb|_87h)!~x7iO2TS)lKY*>IB^ykm2K&}>MXhwb+d+@s8V7=%6YT@<^5 zK`p|3=2{iw9h*s!kBlT=Z0|VtkuhsAW|90f>hlr?;edSY;X8RH0JEjZ07zPjIU{ip zxCCOZx-tl)f-qkY3WXvzANRRPeFQH8qw&I3i!; z&|{#v26MPhU&aeIzgQ{HNRGqypJX;QXdQzXiPEPJ>L8mpV6NG$11~mW*0b&pn>S%T zQPdwMY{pDA+@s32FbF5)TRG|pd3qaW`7~kJydCrDBPQUm1M`gmrXY}j*(}WzuCaO3 z&68C5P6jawokt>?31^ZpAB`FcTXtiPzBCku@4+lAVFiN8n4g)ZQ#Dg^x^Jj4hZw{d~0(hRmY@<98w9_#U_eXn& z&N2vRH(l9g+bC9JI$me737vKc< z*t|d53APqs`{&bDlxh)!7>|78#*y%?81rm27Vh1|Tw6T|VsBv=A>Qhcs~JF$MX~ zn$k+9eZu_YohrmtV?Had23DUjn`x^-C!432-J&*sWe`)5ufz-?m{o)M;x1jVs>N*o zP8Vv~JP!uLg*t2xXgp6jG%$#1$aj<6J5s6<^Q57YP|N0ZuO#6}6SkikD+LRhF^|41 z1F7MAXTFir9hh6L_kub$4{PoPX`R@vvsxGyqMapl`l#i%sb-QQ zW+0xCFAg^-%oT$fj5H=@tAP|_mH_6vwiH7}5VM|D5p`OKB3zO0s88R>a1qS5JLO@P zDCXlA6hKN0bA`JqJZ7`Ts0Wm%1Vzk5zIM^iNkeJO@gtca)f=;glmHa5IZs^x*2!Rd z?vj<1Tpx;Sc~q%Xk!l5b%1ak z%%$!QVAvnCpyNttVDrx{52)CI6yc71Pd*le34<}4y#L5B)Wdwux`y$A%?3U-jAQ!P z{&-<;rnw1374>xpMa)6I%Zoe6 zYZjP8zZ=3HOU%hCMljwA^Q3h~(AygGx1^y^%;t$_?o!`|QN&zy9)*?$GSd$8IXweN zw#V#HW&jf$*lBEyL1sASO+#N&2S-qZC-U9b{)P;2!d&@@f(fHA7sv^Mz-Y|n@q%!Z z&Gjj%)bz0w;e~u>3YL@F<1mj}rwsz*F(1g&hMR1@Ib{m9asowoqx5o~?WFb;%zIi) z;1!!kr<%gPso4H1Wf*htbc&dV(yL_Ekgcwma|WrvtC^U!gH<4Y7G|?|YB0$SbDo$w z2+hX)blhpCo;yX%N9UPm^O|g%gSkDpg?v31^JKGDGR710mW8e42rtaKlaEu6y(z*6 z`MxrHO&;*UJgT5KI4!{JjBeI)abFn27Fg9YK3v&!W_Hv%)MI|s_yoM3)~5{#q>Uv!>hL%)+xqA{xp%frPO z%!e&3VPP!ht78} zALa{}GbyhWiU>fyny+4wMh7rYiEbjL4`NO#Y9ep2+25v_Tz?4LN5?E>${(SKrO3Bq zhcHwf#k}!{5!^V2x$~(BgdfLDE;I$36PUGg-%$5XQbZu~RWW%@qK^@<*mtKeOi0K4 zHcuF2&SD-?m_QvoM-f3Nec++K5O5LmxQI#Jn=u6q?w4b(I;Mx{mGr4w?a4PGY4uIco-5*_gK)BIrh%;r1cXx*m(+s_|12G2sw^T$|Gazzvof_wuqTgjZ8m>0cQhyAxO`)g>x zl-uky6E#5i4rZ-ESE;1C6tNunw&sMXW#^J@o}C{tWY)-HqfoHv3yOkq4h+`-iDb~7+fgB_M(D=)XQ>;2t((IAM%7e`VMnkJK9J0 z9`kc8ZLt1;+4YPz)UjFT-3}`HBSnNG-)B0{$#I`CZ$C$Y{uj(1)fBvA^RZS6lD=ZQ zk763t|2suQAm4$?-^ib}n4N<);AtJ^#bTPUp&oPM8cncmz+4!9mb(9gB32^bN7I|h zy-k=6c1(bXX3Qqm6Tzqj^TZ1ip^D98F7Kq4wNXSQ@^wA;njGGV`A&%rXcA18U0SR^ zykPVGjVPWXv3>p1iPTHPQONg{k~G9HF(0e5gSi5jC3NheuOMa*Q+v3>X3t;==FDD9 zA{zOga~TQ)L@_5QZ-ZW9m}ix4gDf_SE#3}+;@Iw|dzu1CCJ}>t`$~TyOQbQ|I!Zu# zZ_Ml0N`SWv=AtKd&`%aKQ(l63tPhijMZS#o2V{sm=9MP(N%jO4;f^Y@|efY!-4UW0p>0 z5*v|k=2}4*IvI1;93%L~=J_dTkIEEmw{Cbr8BJpnn^5}5OK6eW6*Dtd1nF`hsGsIhb#5L$TFd%<{d|p_a{6Th-x+ zC$?WUe@Wf(ViH@>c_x54?4FNVP%0EceK1Fsg@OJ8%;I(t@Q%${`M0USMNA?d`9?n}gzD)LP8N z>Fc0_%}Nv2!})dCF5{=jwBNua5|FQ#c_S&g8T0dpMo_`#t2fa;=PlTNsKOY=#$)#Q zevi7dl}YSGzA3w#$=y3JPo3ogz6qEGMvn)zotQs791l;~yyEF&YU(Z~k%)ZfR@al7 zdoW)>?}+%x=D3-bkerO|X%&_*VK3%@H8s??eN18(^7Wh<0hy_orKenh%?B_)4ZZ>< z2QfRXx&oirY_atUoIHf>cZ|MJ4-YemBy=9Xf$z!vM=@X7&`vHrhB-Q~os>I{*+fqk z?y}inO%Ap2B$L>UeAmDDLYkk!e9Tl5gwrv*pOA)hHrp0UgZEi%U%ND%>VJ+&>_NWc zG8RC|Ma(lM#lx};%(f}! z8_J`IN$f-CVY(E6-c8KDnRgiCw=h4W?lLa0IoA9xW5I1~U-|tmC2*HXq#)lfW^Lr- z`Bf%yjJ@#DF3OOpKy}ibxhz5ClX#2nZ?|5C>*}Ax8ysRxsy`Ig5&z zv&S3`M$C$s6?4wM>e(CRoO|y5?p@#e-dpc2S-RMCcU4!X>iX^f-qoE#TFJA{p#5T8 zE4k}gv>kgX;MzIRTg10dVa2l5g=p8vt0K?3i1wg6RphRh(0*gwT>hHucV~~X$yY#c zF)#b*u5iHR7>u91MR4l_2jR~et5s0eAP`{e%hjddEElNCA{p|ice%!@1X5> zvAX;<*~VEl08%Xy;^=lOHF$ z$D8umwj`#O_vY@YI z_g^fx`-b+bO^f9Z$?o?1V)=saxcsTnBGyVK>aFBubC0;o^%>fADov8FCwo@QN%9E5 z<@J+puq;v3TgA8EXzwWxE{8T-vPs_30PWgG^5r+k4tkw0pIRQ5UthbCwWuiSt>$GT zzM9Lw8lt_Xd4c>!WwiGt6v(GmLA%ql0=c&l+VzI+W_PQKdTV&uS2y(Ko2sLIFVa~) zwFcR~t>oS{(H_&cmHZ3YOOBL>IkiN+wY+RW<-4+|x@fz1s3G^Rhql*%8uBk>FFo!r z&ojm4hwdF@e)UDYb-Zke(-&EthG?7BvRU5viTGF{Ow?CmV?-WF}^iO%v$ZfM6BIm-`{9k=-- zA0vr+dA#hTMh|3GUT8N6ts<|~9&NW3RpbZB?tHk4Ji{B8x36X-Z`}dyTS;q~%16}O z#Otw~cT?8F7j4CvdUC@~Xx|)dDnCSaqoMWXnSQw3{=f`YJwVjU=VhmTxGlRIi1uzb zcln_pvh{?dm(&?rx&qW?r^OY>_OxJKC3b$mKCT(BAS%F0b4Z z?Mamd`C+n4ZmwnFp`zXvX}{(hWY&GqeiU>|R#}C1Sh-@^RLDxf@yg3{h_fFZ(6;xy*O~+I4o;lD{GQ$=6!)xdUNiv^0J$270GrEL%UT4Bl+CnXkTb$&Nd>j&)xl>h0%cYo9zUGhdB% zqtPWY#TvAI{GZBl$+q?|mWQv!<>yO`<&D;%-P~#~`z=@0eRBNGQe(NvMzmMH$&h~} zdv)_n`NBM0KF%vs-enWo(s$lp=8L)y!Af7r-nSKP>5I|}x1lY4x7lku+S2EcKankc zeRtMQQTO>l>D#56J!nf`2ra)CZRx|C`^lER^BB5c)P34Y`bu82gJ?_NqB~2r^ij9z zhj6*{4KbS|qVB6D(ifP%^J%pX^K&JAt*GcY+R`_HMx8)g`nXPqlW0p{sVRR7ZRuMq zHh+k^+4`j~S$yYH10CV@NMC&@I)k?KZG};1(Uv|^(BT}~(wzAB&WpNPlcgEO^Dm+; z&AvVD657%n*Up#GmL`k-LbfzF@tmuoZuUxPX4j}2XiKx8df!A_nrrka+0vw)8;fwc zG)H8oVo~onZ>KcFUhTVROS92^CR>{GZSg%^E=``MzK^yv3z_)?QI}mUO&d0xY-w^X zi-)*en&qm5Y-yINmXAcelYC#&>`7}MqbcE?+;#9n#p1o+0v{J{a)d6X)*=N z*Jw+#59TW{I93DJR^W(*sjAlpgfcfuPd(JI9TOjy$Ir5et-ep$$pVOn6%dY&v!foXoe zO;g#`N|?6y@op{lny2vT()Qj}WI`QGrR&>LpB-(0X>tda7Ym-08YipuOo+9n^fnJy z=F$?=&-~9CF|jSClT{5cV$HoUJ#1i1iBQ-9)4iIc-4)ITVk%b}o5+@S#}uppVLUs~ z2UDshclaU*k(eU&$Hj6?lYr?@=jMMBOw%!)DXVuRYd-+flcG%$S^6+cJ!57+zvzD>+ZhCns&qx_W0mrRuq6C=`+iH z5gv8JaPvwtOqfq^3>i0g632qn81}t0bpZPwiy_@5Ra&zP$rz$tWAl6ADG%4K$B+Ha zf*veyFosYszMy64qc9A5hGQ5T#Y3X&^KJY#w`4#e&eb|LVaF+Cx!6izw|O{nL%vPB zoX^6xRCSxWB7+&c)>ZV{(1w$miJ#c3wH8AAy!>D>pQwKvB zJwLm!u8lAx@r=(SVT&b(7;e;S&$ilOnBeEDJOZs1hW)inY{JsrFpRHe>k^@GLyLv}pZoM!(?phqMm3=V8t>~0H>lK9|Yp(b7S(O9~LF;G{ z%Zw=O?8c|HViYGy4{ma*^8k4NrXz*s=vA%Hx))*S%&a_LwuVAy5}oR> z%}+42<=~^n%$P!3GIE~^)hIk=aL9YXREFUr%a6ShF7QwiDSRY0xihmj#PAMdSylF^ z8is59-YJniu7e>Li;A4tn+6z8;n$)n>(UfMBFdZSvEJ4g;;=a@g6(aIAq(m;-ptq) z!wx1sPGV_Z7*b&R!kG>J4Z{Wc_Q+yaI-~r5>A^c8C>SOD7w4lG|G?FykC-zrjbw~;wK6498C24bRunF78WlKqVya4(v@;pk3Yj_`0k)*^qy93x-k^@h+ zjbwo&?=62Ju#eABe!J-&!xoYJHln>3oBtUlw9P|%GY2_JU$ZNRv0?fsU!5~HWfq1g zM}2nXeQ!0CnryC(W_|0RM3mp67So!cylxJpzMYFArD8sx- zsm&7FpnMW@z?FUVKsjVc>nQfv2c?c??OzE~0#LfRW^cqox}g+NW0yZ$6N+*|nrngZ zDgvbfPlL|PljH&IKU+y2&BzzzB4m%dV%es^mJ) zs?(1xn1XKZBPVLIfwRz6-R$u-!I)gt%IUS((&gw%7W%}p*5p1ub7?lS%12jk^X4Cf zXWP(?JKZgmwI$bWm`yqxeHh)T-Cf(UOQ+CPs=s(7laZ^m>NHoj^)k9Lm)y{@>g39- zo6o02A$O!h-T*eAT#Y9lg|TVZ8{S_vM>o@#me++k z>YuxAIN=4zvBf)(MV^kMDb!-#*{ zSVuG5w02`5`8WfOxBp&YCgFsg>Wa{maN<+j<3b&pA!a$pi8?g@1(DMVg&O5n#wx5Kdg#C1aHcC#t_K5#A6^JY9Q6SWP(L*6W^dgwJ9k%}VmY_NK6% zaN=S{2R4szV*Su))`D#Lz(-m<-FT#lRBAobm_?f_b5GQ(v@^IH6#EDN6CbM&d6VF~uW-kaQ zrtkP9glG^aS{WFy>4X!DLO%&n8uaAM60Cl*0C!EOUCJKoCys`iG8N&(Ne@0Ln20#he|j(0kZ_{wgof-vRm6!jTNPVV z3vr_O8-Gm_P7F@-U;`}>CpP7qu>``2v;~pO%n@6*}@ywoZqR8WmP%{{DV!^zcEGHCk;?VL`Hk@$6&^3c~A)NUBuoY`UI8kTz2cZsS ze4bep#%>W#93K|LmJv>js?djRCY)%p&5`w(iW!her%YKr!ikVi0;{qJabm@l&%%Ae ziCgnsn38a!5yy%8`G^xWGODpco;7H}ySbBdGucJLi80w3Eb|Cv<5gWbjLqVidCmAX z-Ku*t?FG!1TYXQ>VhAU^W+$?5JQL1}Z&PTtfc-`|F{NS{`%XB~CrZwyy~B*Lrwa$L zQG^pkO^sLw!in!E^jQbOiTh8AgkZvnYPH+3S~U?T3W7Y?>3WD0H=66Y8+0Y#HIi)xJ-J?t~L{znZb4PKXo!d5xH1SHy|S4?YPq zdLm9(Csk$_2`3ya+p%$k6Q8GAGdIGCq=B)_op55w*c$8|;Y3K1B^x*#abnk`J3?o| ziG5pIGY7&6qigQWk#M4YZ*R7HKH@~VVP}LzgcDU3e-IiIPW-kdm>J|DPS|A*W97Fa zPApsK#r6|UghYK2+7nJpd)JILJ%uCur*Oq~%L7ebDDrXZ2C)VES&8iYk+*=mNxCciU z^}JUW&yIT{PGr|<#c~NJir-~3{XoPCpO2q~;e-?Z7o*t-!U^w+Jy{bC;zVF!EPGBk zaoyOOZ6Tbv*wvd^5>Bii@5Fi$PV`x4$xI0+sz0@07qSs2RvQ?ztSK0I`g+V~A)as| zWp6c>or5?rb8=0VKsd4HP!(pj4sjw|aYy(-I5DxseD;ZOV$`dOEO{SBO76OSfGr`M zSUT5)JtLg3eNlnUAe=bq7RK5TP6QVFGd;qIc}|gRqI!wcczlbZ4f6GHPNzrgcE~?nz03h6Hk4eSRmm4-StHSU$LpKu~zkP#b0 zIH8&0&%XCUoM<(wK&Y=qobd7pU~^(Hdgz<5Pe>%3c=y?nDF`Rp8^*D(gcHw3bZ0LJ zCn^+bS#&l=);#v=%U(@JoG3n7i>)P`*cDVHco0tP$~R^ocqEGyy^=H_hD~0J(Isp8 z&SE1rAx_kME@O2ma-_(y2b;bZqd6*1PG?&PC$@EUXO@H$FSm|j)`SyEeRp<`aH2tR zFBV&b(G3earm{F5=^#ZntT7+UP7+SMup zj~zSP3UQ)mUSBra9hKp3^?I_cgcAX8PY7)YCuU5!A>8hYI5DD1G8@|qaiUp>nuQWh zEDEU0{0S%e#09apgcF|kr?Yv46GekUm=WPbq|%pdABk#V==3J+K{hIZdy`wR2~$wb zoA4--g%eI3jIF@*NuAqRp#xh_IPv?{DMIo(#EDI7?g}*sCp2N(geQa(ea>GMM(jf+ z>g*HxHmyW;!g2l_IFXAuvD^9yIBh|ksJDLt+}eZc#M8C&!QddO z6Y<)S(BL?#6Sd9LPPnE`fX}2(?34B3sYs|!6RqH8GO81u zH|0XReuxvlFRKRw2BJFQ9diu2jzD!{iu*)x%0hLb@uI!(Xfmo3!p#Y=cP8S5U$R`h zI3Lvs=a+@xvm9|Ea`9|9wig0}qJ10?{FyF8o+Fn3)V(yNcpuK|XM9OasU~(}|JT>do5D*`rI&r>cG#r15>cr5A zN1>P|gPOK4SzJbwI}Mw80gU9RPCWR$i@EC~PORI$6uMVJbwco}33aL=PS|#z4Mn6* zyc-b!oy<_3NYbjrJW?mRv_1=_HmFWyRq6~)S|U!|K2TN6az%Aw=Z@--?}6&Xl9nf+ zlP}_gWnvY`4MKIIT*gH>NH}4#zJz6vIx%~E9@s^qI$_~D4YJ};o!GbK9`sH@bz-b& z3y}j*op@7t5bh5_b>iEMcr!cCJ;0iCrC*ioDDq2 zOs7uFn|BIktweR=RSO0pi4{ z+kRqpV^k;d&I{s*7N|~)x$y)_>`|SFtujS?OX|eYCbQs~2dWcuUbsM52UI6=`DY6E z`6Eu`)Xf&FbwivedGQv~LlGywUbq2c!cm0 zw?8oyzYIimVyrq6-i|_?Xcp!UC&r;VQN6}1XgwJd#v2u%h6l4yotS)XHM}HsV)F0B zuwogi6Ekle192Uy6W4^c;;v1I6JKT?fhs!?ClUi!!I6EaPNeuwg2qQtorszoDqi~o zaiV@-WAP%X6H&KB@!=IzC-Mx>gL5&e6Xi=ZVg@DiCU;#9k0?>sdsh!Jht!Ef(bu5k z7sLtc9=oB79My?lig>YHImC%ZN&cd?GU7zqkT|GU1J#L2(`t*`O;MeAGsI2g0+B8u z)W*VfLSGK)fJph9(`@%!JQMI_?H%9#G*+gMa5?%s%pUCF3U%-`7oR2isF z6prZ(6^Eia@y;p)x{N|~Vnd?=uz5Vdi%UqQlG+AW$;eyGn;) z`zll?_FuaIO>z+@tSg4W>U>lu7J7FRm+!ztt@2HSVFjrZ^R7IAPDe43s%y7scteR) z_q_OhScvKbTblyyu3@s$`&(at&pWD1R=N@R9&yomf)0otV=Q z)rr~Lhr$y}R40-KE)er=5hu2vk;64VE7&O zc8bd@5GO7NU4g^v5GO3QEl`A#I-%b*3(R()Ix(;QJVn}m#EAxaIS_di)rkWY!$lPZ zdtaWXgyQp{3*^4GV~?Wb3aS&@*GJ&DV#J9>Bd#kJlRB|!z-ib?!Ot!HyXrT4gX%>0 zUNUjqXH+L_c_fva9C2d6#yK!ZA93Qgw}--^5~>ruySc$*V^k-6Mw}GikUHVuF%Y^m zK%DTjI4MrGKy^aMaRmo!R40xLqN1|{2DRD-9)ymrh!Y#UM=ExaIx#A`lDOLk)rk`m zPKtFpqdGC^)+P8o7;z%Cx}#zbsS{zYu~12a>csFxpA=nVQJwgd+YUY_qdF0M|B%8r z6V-`Zst6c24AqHR#qShPMNiXAJE;>FE??sc-@{bp1rJZ-m#uXdt3ael!9wtYY1M6aYQvE@-zCyXqT zVEiAbP7G8W5lb$hIw5^I+=&9lq)+7r-Tn(si2o5z+&sS(CORNan07k^DTEU-+eX5N zj))V39ZVs&GvdV9_jAC6aKioALfG0H)ro*JMR1F7V&2_)Vp!3Z^{- z1>rj!p#&ySu#<8344A(QapGL(EC{3d?yqhd37rWi9L+3x@=As4Rgoz$kS4|No>K_h*FJ_1n_Kn9u0R=mq zHnV|f!ijTwd!QBJMEA*tu#a$J@|gt?-X3uR9Ad$qaAL{GLgqj?p>J6Li+UkWoVvdp z_C_F1gkJRmRSe?9rQ6NHj&P!mT^=M8PPq1T1dqXp6a8);g*C$wCyMWnhY4d5C)W4d z4>hMCPMF>q4^;^#Vvfp1Sb#V&b@F7$S%x@q#%?K0UW+)v&Tocs6znw6>^@ZAfjALR zDGILaL!1ao*a8m;C-R?;0Hafg6CTS}Ktd@_7#6{7!ihu4rf}~T;)LwdKNENOr^@wQT;nAr@~3A2C#NV7(qxVc{iFP#u4c>aZW(*<#&J9C5qUWgNI zE?aviuoaNjMR_fwxVAIMJ>7bl6Tfu~&8{NHyOE?i+*$RdePOL2$4K)ZSUXR!V zPgWpKEK8UMU2_p9sx7k%g*ZH_o` zX6X}fBb>MzFhy+G8r6yKduDQo2u7S}cJdXRpkSxx z({8|Z72-tP*gs%D;Y5+k5vY`mII--&7%gI5Bd+8<-MK z-0%MpVhAVt@7)J&2q$jeSp&)(R40};ehCRH5GM@7w?oghh!bxVu3}}vi5{Bc;6^wx zbm}U2OE{sPI1vUML7dp#AXKy_oKWAg5OWGqolss=h&8SvPRzC14HIu6PJBNaEZRRn zbz*PNEs*ydgPk_s=pp_=IMHX(Rai*DPFLpdh7p7lw>$@mXAKZ1^hP{^y%g+}yDSz` zt0PW)+bk2)>LN}&4RaSm2q$K~Z6Q`{hU&zfwe7{8c8C+7ay~!@!ig7$k|2X{B5>6n z;KHMBj+N6nnczz}QCzv6c$;uy@ymx$pKxNusVmT#aAHpS6?h+oIN>yOoY*i9abnV% zkuV|^aiYRJADB!yu{S3G!iOMEO#BuCBM2vkoaqPU2qzYAybr0*heZ?}uiLeHrp)eDJow{{3RtSU> zCFRP)Uc!mzX;(xI;e_+xnecoX;>1b^b4A{4#EDZebs>Xr;&ClIMRURl%_%&FiY)gK)ybMkd~(yvN-k$HW;j#EEwab739f zglbz0MRrBRiM$SO(3WuGOW-NdtTw6>^BWI<)%6i4rgu9bJ}^g|h+N+m)PxhaEJejo z!igoH*Mrywabn)ck&4HJ6XhBhiY0^-deJAvh;#ECK95pZiL;)MCj_li2B5hrHz2@CE{K%BT( zJWKJCaAMhlBM?0YaiVknW_=gJiP3qPuz3aI#H!O<^$T-Ro$#G98hi;SY@Tir>+eFG zcu};FJs_OOD4!{gKY}sII%1IFx2%zb)tCJG+6o@;)I7;Ll{HqL~g%z@S1R9T;wRI8;UrQ8uk>%sWI4T zP^z`)N9u%giJ|C2>O{>N5#W}A>O{im4j>Fcbs}rhLpVY>QH!e+H%Xn?;e8N%r=dEb zemnzuk~-n~M+LFoVpJ#g)_x4dgcFa(tc8!HPWZ{5Lev&iCwfJU1Ml646OFbn0B2Gs zKJS?WUkE3D->-!)gcIt~1+b5Bq86X?z=70>n>&7o;5(>J*yJyS6jCRi_V9q^&rzM2 z`sOandWY&n)b!cV^b6v|s*+@QO6tVj#=BtvsS^z*c!AmwaUx`vqbRR|>O{vW9UzL- zi327vaFNuBvuTTA6{!=`YFNP{8&oH*M(=_YCsZe#CYnNw8wNXV>y-m-NS%nia1bi_ zp*oS6b%Wg^oLE%;1SonUPHdUE02+lOPRMS0!m((?i3P#OWw z4XG0aKF47ssT0}XmO&J$6IIjKLBJ%$iMsVx!BfHs)rsojM#72Ok=X!C5hr}|7QuT` zCydS<1Wg{Q6CHQmf>O|y%(=3V9 z3Fk#yz|IiWi9W+j;2q&a=ff*t2jPU?x&RnR>clB^f_RJ6iLf!n;7962$yybJv_zc9 z>049WPU?h7kvZHZb)rYxqp+EBC+?3f58BSCPSkr)0t-Sg*y*w6Im->hU?=&}?GR4t z#ML<~AtD~tiMsXPf>SE06VqqgK!Z%giBH9cpxRKxiO_FHq3amLiO=(Th>u8}(BB*l z+UXeV)F{jan$JUZ!eGc&cu6>6zv~Y;vJ!D3+iNMbAa$a(vz=(Z8P$orw##4(;Y9V$ z1H`Qd5GUr(Jt&SPb>f1-NpZz#40e)T?+88@5hv<6c|gY-s7`!790_)JP@PD>WC3Rg zC(cfP3Ppqy+hW|rC8SR5S@!@mq)zDXxCIs>suS~OzXOX3s7|aoG7&Cu zzK9b;S63FB4MLo-AEts{BM~Q>c(jJmS*T8Qy2!+8Q&63lY;gvv&qj4ZQ?w6G5>A+W zdIp80PBc4m92`iU$S&{`9}`aOu6hMZ2q(-t?14r5QJrwVI|;gyIx)RVoM=#h>O_HA zV-YSOP6Vu|F8)q9v5~72(WFiUcxc2T!ilKRO;Gb0;>639v0}zs40gKZ^%ycqotW_G z58$agx~V#cg$xj#NS(Oo79h?hb>dN2GISz!B6YoyXj%_(A~VZf6dIvAp;WdJw-Zi8 zz4j1)Cv{?$Njb5BGpZBkdJh3JH&iF;7N3L5q)t?`&4hekR3~=KZ6;0#!eFOIpB}*2 z5L727cDw*tVW>_Nce?;Q{Y*EVVMxw+@igJYvdshGF{u;77j=WynW#>j-QF46kU9}L zuRB|2O9;o15Cn65;exaNKvg!QOS zxbF^w%lU{Cbsnh2Av;l>*w8Wxh91CRr%IJSfcY_0Cp2yYp@49rM))a6Cv~D?-YBSZ z9o320%U{9p4yqGl`J5J82`7}s+aQ$Gi34ZGLw8aqYSed!wS*Je@e|+?;l$~d*|4=7 zsuMOB!XdOW;Y8jX1*?JT#FBNzFsdHnM2G5~#f(O%PE0BCg`j4L6V3+~inj?Tj+q<4 zVNxf?j9nlOaz&iz+NlqyyilFEwSKAik#OSd#kx>E5OJc9Ac#>Rs7^d^EQF#`oCqio zWs#^(tm!!%zQ!U>{86N@@J~f`B4Jqxw8=o6uuC#maBqliiX&xtfOjKNo$#M`Ok6My z)d@CmB6ORII8p1Vqhj0~#EAumnu6IPR42|8zjQ;kDV{~6*$-ECd<;Vr5Y?w4iaPEsdcCR`B{<%km_2F-xGBB~SFL7oaV zsT0%f+`*(8suMT1TonrlCziFD2_qUHPS^()iU&xYm^a!L=3AjUk=@)x(aaImi5ZrA zA&+ok|L&QJ86Fty)YHgRoY@i83D=|>;%}r*Tv~q_UJ_1xNpw`q>W#rpCkMuY3#k)B zR)PLVQYSjh@qj|Yi4K(u6az?|0K*9I9)dWrWR#x%(b0$#`)9U+I^z*1X11QKm_H4J zow6gZLOW6?-aOc&|B!Iv)AIfhwi0n-TeofcL)RltEcKlQjklmWk?MO;JViKh{LMNx z^#H09#Xi|$B&ieQ^pauMABYpDGcJg=3Q?VydL|ndUPGK1G_w$fb9JIICx~+Vzb+2nT1o$nY4xlXy*Pswzqorq)Di6SQgC!~_>JGXpa-Wsrt{&Rm<8sf^=B zoRT*=HBFU}mde)|glDE{l%8Jgx_Z0VD9zNG@boC$4A=PhwfA?iRVvM*Gzpp%Rb1J1 z!R~>6F7|v~KUGQsZ*bXlUV(xBfi6x;GfhfLVoKRH?cKY$`?}aTD$OEQX{xvu5xiwu zer{+RY;k+7CQhwuUk8m!tw~Wv>waOIbVp&3nwBE%GeH%vQJUee#7JdYv__ebsMeIO zj!%u^Yw64=OP5A!HHh>%XCv~j8V_XX0m32An=$~bL; zRI~2a-*1TR?is4o-6W}eW1XQpf>hpqRoah(P$s2^$7xfeHK_&$F8`Z<3_1mQw^F92 zsl)gwN^OCs*?`|G{LWCTQq=sMq^Z>W3MnJ`^^4XLp7u8eqV;Cs8TWwq*8dO24?N0bIdp5rF2)6|5l}@CPrxa1yC#dY15*AJPp#8 zVOq7pKlE|crB09E2`;?Li%L-?MdMxNn;7+{ev%{VKkp~`z(!BWH~864hO1Jgn?-s6 z!=jQ>!_+DKa;N;MkF@#KN17S@>LY*kk-z%LzjnR<@AQ$<=A?8HYtw2NPZnqPyOvSX$H z%Ma{d9=4xD8vf+raHbOi7FU9PKCb+c{4u0y^z(6z(?q6~8MCxVNsA4O)MkW9!BYB2^k&Cr1az2y0ullSUKi=;-Ve?r7bIx8`pi zW@&Kjr1bL-3+(cn(nV>FPo=WGufKa2A3tweY(tCrMq%xJLimQZ27g`@)}?!Z*S~$l z)udA+4G`n`z*LhWosS^hsj%ZKe)N5O1o-1*){;sfNB{NY@Do|Yr#1U;pFvx-oysXf zZSU+H?i_BfvbME%jtqBpid0)W@RMrq8I+ejna)vl|g)z92cjRSVfzm zVM)LjgY>jZP1XIa>2~(wp_2r*hj)>D(avwq(f4lu?-p?9- zTn($g47>RzKiaIj7w}Q%Y{~&TUe`neen0U01#f%_zj#+5T~uGbHZ5JPQT9}8qEa-P z)KDvBKw>KQi}055Q2^#74!?lu+DcZ^)GA>b-=4h{!4oIp@ zS8-xO)W58m-ymsH#Yrg=5XMXHDXl8u&-M4?3|6N4(ki9;-Q!9d9{-mntHSwXt~;f4 zKmRyyyqHCDx>l3MpAJ8zHj*EmQdL@y#GZXs+Bj(}$8Vw1V=7%Q?GsN7zupmvI^dRY zyk~rG+fb#>@5S%evKLjQC9z-gcvTGdlyKLD zCIxT)b{g&&)3oBd;fGbmsgbJd0J!fVS{ttAJCGiI$??Ijw}$Ty{Xa>Gagtx9^c3`T z4po-6Fzn}%ODDt!yNq!BTQ|#VNIEQfsAyi_`FaE7OWf^J=^U zg7}%kC#=+cfKTg>(=6SAD(?3v)2~=^AC<1byhG!ck>4D;v&!$HsKf+SoUZ=F1pcUc zYU8-K1|QP^z8`I>^heq2ZOLokx0;qeD;j=|m8vA}#K^!u%N(5Kp5yya@q3b9NFM&( z+yi~w{XD#M@1(GErJ0@8|9t zR$8C!KiOmHdb`pl{mrEclU{$nhP=OqywXiy>K^>RJmmeK@+y}Z9h80L@dx~GM?_Lb zMQ=;V+5B$~pm=S;(Hj2N=8oM&C4UJ@J(1LJq(@039{wVr4p7azx|DeCnAdqj!xQa%Bvo44 zjr^$~m6FO>|EXZv;1D1E(hU{ zna(?`jN)(81ohulfUWpB&}k>V?ogiMFN%JAtQGxl43+r{GK#-T@Z9s$t+dk2&$;_dG1sN`KUh4iDbr>JOXg=`Gq<#K` zONk|=$VX`-^dRune9-%k;jGRMcHu3Zog%Cw9c=6)Bdr~+RStG4J01iZp>edecMRlp z|E=<-i!v;Y+T+ghgdZ+rwd5_)phu#Na#PmvjxN^*JPY_a((xm5|I1m> z(YJIV{&cx4b$TFQtfuw4)B&AOyv)hAQu1raFXm65bZ4$bYk1(lr0M;)Ri8?kK0xEPAhXh z|J-=#FDV9q&bt&@z#pzsnf&jMsB~>VgMa=mx8yHsU3ktv-ba6a8wO$QzM5}uB)$8i z(-UH)(SS>}%KT6MBSmLNa6KT+MW0($_G%Hv3UN*M diff --git a/src/preload/execution b/src/preload/execution index 55223bccd..c1e64bf8e 100644 --- a/src/preload/execution +++ b/src/preload/execution @@ -5,6 +5,7 @@ #ifdef __APPLE__ #include + namespace std { enum class execution { From 1a7f82337dc0c4e594cb15d8e6eb8fc451484c70 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Mon, 4 Mar 2024 16:33:21 -0500 Subject: [PATCH 085/145] add transition back to waypoint if we leave long range, but need to test bearing measurements --- config/simulator/simulator.yaml | 2 +- scripts/debug_course_publisher.py | 20 ++++++++++---------- src/navigation/approach_object.py | 1 + src/navigation/approach_target_base.py | 5 ++++- src/navigation/context.py | 1 + src/navigation/navigation.py | 2 +- src/navigation/waypoint.py | 3 ++- 7 files changed, 20 insertions(+), 14 deletions(-) diff --git a/config/simulator/simulator.yaml b/config/simulator/simulator.yaml index 5df26b848..553a13972 100644 --- a/config/simulator/simulator.yaml +++ b/config/simulator/simulator.yaml @@ -13,7 +13,7 @@ objects: - type: urdf name: tag_1 uri: package://mrover/urdf/world/tag_1.urdf.xacro - translation: [15, -14, 2.4] + translation: [10, -9, 1.4] - type: urdf name: hammer uri: package://mrover/urdf/world/hammer.urdf.xacro diff --git a/scripts/debug_course_publisher.py b/scripts/debug_course_publisher.py index ff5b34384..db34d42bf 100755 --- a/scripts/debug_course_publisher.py +++ b/scripts/debug_course_publisher.py @@ -20,18 +20,18 @@ [ 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=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])), - ), + SE3(position=np.array([9, -7, 0])), + ) ] ] ) diff --git a/src/navigation/approach_object.py b/src/navigation/approach_object.py index 694840cdf..b4ab42742 100644 --- a/src/navigation/approach_object.py +++ b/src/navigation/approach_object.py @@ -25,6 +25,7 @@ def on_exit(self, context): pass def get_target_pos(self, context) -> Optional[np.ndarray]: + # either position or None object_pos = context.env.current_target_pos() return object_pos diff --git a/src/navigation/approach_target_base.py b/src/navigation/approach_target_base.py index 5c763149b..74959d765 100644 --- a/src/navigation/approach_target_base.py +++ b/src/navigation/approach_target_base.py @@ -6,11 +6,12 @@ from typing import Optional import numpy as np -from navigation import search +from navigation import search, waypoint class ApproachTargetBaseState(State): STOP_THRESH = get_rosparam("single_fiducial/stop_thresh", 0.7) + STOP_THRESH_WAYPOINT = get_rosparam("waypoint/stop_thresh", 0.5) 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 @@ -37,6 +38,8 @@ def on_loop(self, context) -> State: """ target_pos = self.get_target_pos(context) if target_pos is None: + if self.__class__.__name__ == "LongRangeState" and not context.env.arrived_at_waypoint: + return waypoint.WaypointState() return search.SearchState() try: diff --git a/src/navigation/context.py b/src/navigation/context.py index 3e450d3c8..5495efa5d 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -79,6 +79,7 @@ class Environment: long_range_tags: LongRangeTagStore NO_FIDUCIAL: ClassVar[int] = -1 arrived_at_target: bool = False + arrived_at_waypoint: bool = False last_target_location: Optional[np.ndarray] = None def get_target_pos(self, id: str, in_odom_frame: bool = True) -> Optional[np.ndarray]: diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py index 5cb574e71..c13662768 100755 --- a/src/navigation/navigation.py +++ b/src/navigation/navigation.py @@ -60,7 +60,7 @@ def __init__(self, context: Context): ], ) self.state_machine.add_transitions(ApproachObjectState(), [DoneState(), SearchState(), RecoveryState()]) - self.state_machine.add_transitions(LongRangeState(), [ApproachPostState(), SearchState(), RecoveryState()]) + self.state_machine.add_transitions(LongRangeState(), [ApproachPostState(), SearchState(), WaypointState(), RecoveryState()]) 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/waypoint.py b/src/navigation/waypoint.py index 16c0453e4..493c23d7a 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -12,7 +12,7 @@ class WaypointState(State): NO_FIDUCIAL = get_rosparam("waypoint/no_fiducial", -1) def on_enter(self, context) -> None: - pass + context.env.arrived_at_waypoint = False def on_exit(self, context) -> None: pass @@ -49,6 +49,7 @@ def on_loop(self, context) -> State: self.DRIVE_FWD_THRESH, ) if arrived: + context.env.arrived_at_waypoint = True if not context.course.look_for_post() and not context.course.look_for_object(): # We finished a regular waypoint, go onto the next one context.course.increment_waypoint() From dc456ea47c1ef135add422eb7db86dc9e44cb0e0 Mon Sep 17 00:00:00 2001 From: dllliu Date: Thu, 21 Mar 2024 20:04:59 -0400 Subject: [PATCH 086/145] start on one or two gps handling error --- config/localization.yaml | 1 + launch/rtk_rover.launch | 3 +- src/localization/gps_linearization.py | 110 +++++++++---- src/localization/one_gps_gps_linearization.py | 149 ++++++++++++++++++ 4 files changed, 232 insertions(+), 31 deletions(-) create mode 100644 src/localization/one_gps_gps_linearization.py diff --git a/config/localization.yaml b/config/localization.yaml index 000c48ad8..55dc1b47a 100644 --- a/config/localization.yaml +++ b/config/localization.yaml @@ -8,3 +8,4 @@ gps_linearization: reference_point_longitude: -83.7096706 reference_point_altitude: 234.1 reference_heading: 90.0 + use_both_gps: true diff --git a/launch/rtk_rover.launch b/launch/rtk_rover.launch index b1420a310..07012b4ae 100644 --- a/launch/rtk_rover.launch +++ b/launch/rtk_rover.launch @@ -1,13 +1,14 @@ + - + diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 0d5364981..03bc7344b 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -31,6 +31,7 @@ class GPSLinearization: ref_alt: float world_frame: str + both_gps: bool # covariance config use_dop_covariance: bool @@ -39,8 +40,9 @@ class GPSLinearization: def __init__(self): # read required parameters, if they don't exist an error will be thrown self.ref_lat = rospy.get_param("gps_linearization/reference_point_latitude") - self.ref_lon = rospy.get_param("gps_linearization/reference_point_longitude") + self.ref_lon = rospy.get_param(param_name="gps_linearization/reference_point_longitude") self.ref_alt = rospy.get_param("gps_linearization/reference_point_altitude") + self.both_gps = False#rospy.get_param("gps_linearization/use_both_gps") self.world_frame = rospy.get_param("world_frame") self.use_dop_covariance = rospy.get_param("global_ekf/use_gps_dop_covariance") @@ -51,16 +53,42 @@ def __init__(self): self.last_gps_msg = None self.last_imu_msg = None - right_gps_sub = message_filters.Subscriber("right_gps_driver/fix", NavSatFix) - left_gps_sub = message_filters.Subscriber("left_gps_driver/fix", NavSatFix) - - sync_gps_sub = message_filters.ApproximateTimeSynchronizer([right_gps_sub, left_gps_sub], 10, 0.5) - sync_gps_sub.registerCallback(self.gps_callback) + if self.both_gps: + right_gps_sub = message_filters.Subscriber("right_gps_driver/fix", NavSatFix) + left_gps_sub = message_filters.Subscriber("left_gps_driver/fix", NavSatFix) + sync_gps_sub = message_filters.ApproximateTimeSynchronizer([right_gps_sub, left_gps_sub], 10, 0.5) + sync_gps_sub.registerCallback(self.duo_gps_callback) + else: + rospy.Subscriber("left_gps_driver/fix", NavSatFix, self.single_gps_callback) rospy.Subscriber("imu/data", ImuAndMag, self.imu_callback) self.pose_publisher = rospy.Publisher("linearized_pose", PoseWithCovarianceStamped, queue_size=1) - def gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): + + def single_gps_callback(self, msg: NavSatFix): + """ + Callback function that receives GPS messages, assigns their covariance matrix, + and then publishes the linearized pose. + + :param msg: The NavSatFix message containing GPS data that was just received + """ + if np.any(np.isnan([msg.latitude, msg.longitude, msg.altitude])): + return + + if not self.use_dop_covariance: + msg.position_covariance = self.config_gps_covariance + + ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) + + self.last_gps_msg = msg + print("using single callback") + + if self.last_imu_msg is not None: + self.publish_pose() + self.last_gps_msg = self.get_linearized_pose_in_world(msg, self.last_imu_msg, ref_coord) + + + def duo_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): """ Callback function that receives GPS messages, assigns their covariance matrix, and then publishes the linearized pose. @@ -82,25 +110,10 @@ def gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): geodetic2enu(left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude, *ref_coord, deg=True) ) - pose = GPSLinearization.compute_gps_pose(right_cartesian=right_cartesian, left_cartesian=left_cartesian) + self.last_gps_msg = GPSLinearization.compute_gps_pose(right_cartesian=right_cartesian, left_cartesian=left_cartesian) - covariance_matrix = np.zeros((6, 6)) - covariance_matrix[:3, :3] = self.config_gps_covariance.reshape(3, 3) - covariance_matrix[3:, 3:] = self.config_imu_covariance.reshape(3, 3) - - # TODO: publish to ekf - pose_msg = PoseWithCovarianceStamped( - header=Header(stamp=rospy.Time.now(), frame_id=self.world_frame), - pose=PoseWithCovariance( - pose=Pose( - position=Point(*pose.position), - orientation=Quaternion(*pose.rotation.quaternion), - ), - covariance=covariance_matrix.flatten().tolist(), - ), - ) - - self.pose_publisher.publish(pose_msg) + if self.last_gps_msg is not None: + self.publish_pose() def imu_callback(self, msg: ImuAndMag): """ @@ -118,17 +131,18 @@ def publish_pose(self): Publishes the linearized pose of the rover relative to the map frame, as a PoseWithCovarianceStamped message. """ - ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) - linearized_pose, covariance = self.get_linearized_pose_in_world(self.last_gps_msg, self.last_imu_msg, ref_coord) + covariance_matrix = np.zeros((6, 6)) + covariance_matrix[:3, :3] = self.config_gps_covariance.reshape(3, 3) + covariance_matrix[3:, 3:] = self.config_imu_covariance.reshape(3, 3) pose_msg = PoseWithCovarianceStamped( header=Header(stamp=rospy.Time.now(), frame_id=self.world_frame), pose=PoseWithCovariance( pose=Pose( - position=Point(*linearized_pose.position), - orientation=Quaternion(*linearized_pose.rotation.quaternion), + position=Point(*self.last_gps_msg.position), + orientation=Quaternion(*self.last_gps_msg.rotation.quaternion), ), - covariance=covariance.flatten().tolist(), + covariance=covariance_matrix.flatten().tolist(), ), ) self.pose_publisher.publish(pose_msg) @@ -158,6 +172,42 @@ def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: pose = SE3(rover_position, SO3.from_matrix(rotation_matrix=rotation_matrix)) return pose + + @staticmethod + def get_linearized_pose_in_world( + gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray + ) -> Tuple[SE3, np.ndarray]: + """ + Linearizes the GPS geodetic coordinates into ENU cartesian coordinates, + then combines them with the IMU orientation into a pose estimate relative + to the world frame, with corresponding covariance matrix. + + :param gps_msg: Message containing the rover's GPS coordinates and their corresponding + covariance matrix. + :param imu_msg: Message containing the rover's orientation from IMU, with + corresponding covariance matrix. + :param ref_coord: numpy array containing the geodetic coordinate which will be the origin + of the tangent plane, [latitude, longitude, altitude] + :returns: The pose consisting of linearized GPS and IMU orientation, and the corresponding + covariance matrix as a 6x6 numpy array where each row is [x, y, z, roll, pitch, yaw] + """ + # linearize GPS coordinates into cartesian + cartesian = np.array(geodetic2enu(gps_msg.latitude, gps_msg.longitude, gps_msg.altitude, *ref_coord, deg=True)) + + # ignore Z + cartesian[2] = 0 + + imu_quat = numpify(imu_msg.imu.orientation) + + # normalize to avoid rounding errors + imu_quat = imu_quat / np.linalg.norm(imu_quat) + pose = SE3.from_pos_quat(position=cartesian, quaternion=imu_quat) + + covariance = np.zeros((6, 6)) + covariance[:3, :3] = np.array([gps_msg.position_covariance]).reshape(3, 3) + covariance[3:, 3:] = np.array([imu_msg.imu.orientation_covariance]).reshape(3, 3) + + return pose def main(): diff --git a/src/localization/one_gps_gps_linearization.py b/src/localization/one_gps_gps_linearization.py new file mode 100644 index 000000000..9c1a36d04 --- /dev/null +++ b/src/localization/one_gps_gps_linearization.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 +from typing import Optional, Tuple + +import numpy as np +import rospy +from geometry_msgs.msg import PoseWithCovarianceStamped, PoseWithCovariance, Pose, Point, Quaternion +from mrover.msg import ImuAndMag +from pymap3d.enu import geodetic2enu +from sensor_msgs.msg import NavSatFix +from std_msgs.msg import Header +from util.SE3 import SE3 +from util.np_utils import numpify + + +class GPSLinearization: + """ + This node subscribes to GPS and IMU data, linearizes the GPS data + into ENU coordinates, then combines the linearized GPS position and the IMU + orientation into a pose estimate for the rover and publishes it. + """ + + last_gps_msg: Optional[NavSatFix] + last_imu_msg: Optional[ImuAndMag] + pose_publisher: rospy.Publisher + + # reference coordinates + ref_lat: float + ref_lon: float + ref_alt: float + + world_frame: str + + # covariance config + use_dop_covariance: bool + config_gps_covariance: np.ndarray + + def __init__(self): + # read required parameters, if they don't exist an error will be thrown + self.ref_lat = rospy.get_param("gps_linearization/reference_point_latitude") + self.ref_lon = rospy.get_param("gps_linearization/reference_point_longitude") + self.ref_alt = rospy.get_param("gps_linearization/reference_point_altitude") + self.world_frame = rospy.get_param("world_frame") + self.use_dop_covariance = rospy.get_param("global_ekf/use_gps_dop_covariance") + self.config_gps_covariance = np.array(rospy.get_param("global_ekf/gps_covariance", None)) + + self.last_gps_msg = None + self.last_imu_msg = None + + rospy.Subscriber("gps/fix", NavSatFix, self.gps_callback) + rospy.Subscriber("imu/data", ImuAndMag, self.imu_callback) + + + self.pose_publisher = rospy.Publisher("linearized_pose", PoseWithCovarianceStamped, queue_size=1) + + def gps_callback(self, msg: NavSatFix): + """ + Callback function that receives GPS messages, assigns their covariance matrix, + and then publishes the linearized pose. + + :param msg: The NavSatFix message containing GPS data that was just received + """ + if np.any(np.isnan([msg.latitude, msg.longitude, msg.altitude])): + return + + if not self.use_dop_covariance: + msg.position_covariance = self.config_gps_covariance + + self.last_gps_msg = msg + + if self.last_imu_msg is not None: + self.publish_pose() + + def imu_callback(self, msg: ImuAndMag): + """ + Callback function that receives IMU messages and publishes the linearized pose. + + :param msg: The Imu message containing IMU data that was just received + """ + self.last_imu_msg = msg + + if self.last_gps_msg is not None: + self.publish_pose() + + @staticmethod + def get_linearized_pose_in_world( + gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray + ) -> Tuple[SE3, np.ndarray]: + """ + Linearizes the GPS geodetic coordinates into ENU cartesian coordinates, + then combines them with the IMU orientation into a pose estimate relative + to the world frame, with corresponding covariance matrix. + + :param gps_msg: Message containing the rover's GPS coordinates and their corresponding + covariance matrix. + :param imu_msg: Message containing the rover's orientation from IMU, with + corresponding covariance matrix. + :param ref_coord: numpy array containing the geodetic coordinate which will be the origin + of the tangent plane, [latitude, longitude, altitude] + :returns: The pose consisting of linearized GPS and IMU orientation, and the corresponding + covariance matrix as a 6x6 numpy array where each row is [x, y, z, roll, pitch, yaw] + """ + # linearize GPS coordinates into cartesian + cartesian = np.array(geodetic2enu(gps_msg.latitude, gps_msg.longitude, gps_msg.altitude, *ref_coord, deg=True)) + + # ignore Z + cartesian[2] = 0 + + imu_quat = numpify(imu_msg.imu.orientation) + + # normalize to avoid rounding errors + imu_quat = imu_quat / np.linalg.norm(imu_quat) + pose = SE3.from_pos_quat(position=cartesian, quaternion=imu_quat) + + covariance = np.zeros((6, 6)) + covariance[:3, :3] = np.array([gps_msg.position_covariance]).reshape(3, 3) + covariance[3:, 3:] = np.array([imu_msg.imu.orientation_covariance]).reshape(3, 3) + + return pose, covariance + + def publish_pose(self): + """ + Publishes the linearized pose of the rover relative to the map frame, + as a PoseWithCovarianceStamped message. + """ + ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) + linearized_pose, covariance = self.get_linearized_pose_in_world(self.last_gps_msg, self.last_imu_msg, ref_coord) + + pose_msg = PoseWithCovarianceStamped( + header=Header(stamp=rospy.Time.now(), frame_id=self.world_frame), + pose=PoseWithCovariance( + pose=Pose( + position=Point(*linearized_pose.position), + orientation=Quaternion(*linearized_pose.rotation.quaternion), + ), + covariance=covariance.flatten().tolist(), + ), + ) + self.pose_publisher.publish(pose_msg) + + +def main(): + # start the node and spin to wait for messages to be received + rospy.init_node("gps_linearization") + GPSLinearization() + rospy.spin() + + +if __name__ == "__main__": + main() \ No newline at end of file From 3aadb737f88b88d0450ea45a20832c385f0c1248 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 23 Mar 2024 16:29:40 -0400 Subject: [PATCH 087/145] Format --- src/navigation/context.py | 2 +- src/navigation/navigation.py | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/navigation/context.py b/src/navigation/context.py index 5495efa5d..8a05e3219 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -150,7 +150,7 @@ def __init__(self, ctx: Context, min_hits: int = MIN_HITS, max_hits: int = MAX_H def push_frame(self, tags: List[LongRangeTag]) -> None: """ - Loops through our current list of our stored tags and checks if the new message includes each tag or doesn't. + Loops through our current list of our stored tags and checks if the new message includes each tag or doesn't. If it does include it, we will increment our hit count for that tag id and reset the time we saw it. If it does not include it, we will decrement our hit count for that tag id, and if the hit count becomes zero, then we remove it from our stored list. If there are tag ids in the new message that we don't have stored, we will add it to our stored list. diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py index c13662768..a97cb1ec5 100755 --- a/src/navigation/navigation.py +++ b/src/navigation/navigation.py @@ -60,7 +60,9 @@ def __init__(self, context: Context): ], ) self.state_machine.add_transitions(ApproachObjectState(), [DoneState(), SearchState(), RecoveryState()]) - self.state_machine.add_transitions(LongRangeState(), [ApproachPostState(), SearchState(), WaypointState(), RecoveryState()]) + self.state_machine.add_transitions( + LongRangeState(), [ApproachPostState(), SearchState(), WaypointState(), RecoveryState()] + ) 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 b0a12bc978f6905d2516f9011334ff854ae466c0 Mon Sep 17 00:00:00 2001 From: nehakankanala Date: Sun, 24 Mar 2024 13:04:59 -0400 Subject: [PATCH 088/145] created separate variable for last pose --- src/localization/gps_linearization.py | 29 ++++++++++++++------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 03bc7344b..452d69281 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -42,7 +42,8 @@ def __init__(self): self.ref_lat = rospy.get_param("gps_linearization/reference_point_latitude") self.ref_lon = rospy.get_param(param_name="gps_linearization/reference_point_longitude") self.ref_alt = rospy.get_param("gps_linearization/reference_point_altitude") - self.both_gps = False#rospy.get_param("gps_linearization/use_both_gps") + self.ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) + self.both_gps = True#rospy.get_param("gps_linearization/use_both_gps") self.world_frame = rospy.get_param("world_frame") self.use_dop_covariance = rospy.get_param("global_ekf/use_gps_dop_covariance") @@ -51,6 +52,7 @@ def __init__(self): self.config_imu_covariance = np.array(rospy.get_param("global_ekf/imu_orientation_covariance", None)) self.last_gps_msg = None + self.last_pose = None self.last_imu_msg = None if self.both_gps: @@ -78,17 +80,16 @@ def single_gps_callback(self, msg: NavSatFix): if not self.use_dop_covariance: msg.position_covariance = self.config_gps_covariance - ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) - self.last_gps_msg = msg print("using single callback") if self.last_imu_msg is not None: + self.last_pose = self.get_linearized_pose_in_world(msg, self.last_imu_msg, self.ref_coord) self.publish_pose() - self.last_gps_msg = self.get_linearized_pose_in_world(msg, self.last_imu_msg, ref_coord) def duo_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): + print("double") """ Callback function that receives GPS messages, assigns their covariance matrix, and then publishes the linearized pose. @@ -101,19 +102,16 @@ def duo_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): if np.any(np.isnan([left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude])): return - ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) - right_cartesian = np.array( - geodetic2enu(right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude, *ref_coord, deg=True) + geodetic2enu(right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude, *self.ref_coord, deg=True) ) left_cartesian = np.array( - geodetic2enu(left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude, *ref_coord, deg=True) + geodetic2enu(left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude, *self.ref_coord, deg=True) ) - self.last_gps_msg = GPSLinearization.compute_gps_pose(right_cartesian=right_cartesian, left_cartesian=left_cartesian) + self.last_pose = GPSLinearization.compute_gps_pose(right_cartesian=right_cartesian, left_cartesian=left_cartesian) - if self.last_gps_msg is not None: - self.publish_pose() + self.publish_pose() def imu_callback(self, msg: ImuAndMag): """ @@ -123,7 +121,10 @@ def imu_callback(self, msg: ImuAndMag): """ self.last_imu_msg = msg - if self.last_gps_msg is not None: + if self.last_gps_msg is not None and not self.both_gps: + self.last_pose = self.get_linearized_pose_in_world(self.last_gps_msg, self.last_imu_msg, self.ref_coord) + self.publish_pose() + elif self.last_pose is not None and self.both_gps: self.publish_pose() def publish_pose(self): @@ -139,8 +140,8 @@ def publish_pose(self): header=Header(stamp=rospy.Time.now(), frame_id=self.world_frame), pose=PoseWithCovariance( pose=Pose( - position=Point(*self.last_gps_msg.position), - orientation=Quaternion(*self.last_gps_msg.rotation.quaternion), + position=Point(*self.last_pose.position), + orientation=Quaternion(*self.last_pose.rotation.quaternion), ), covariance=covariance_matrix.flatten().tolist(), ), From 4443d5a9eafc5cc64aca3d4a9d414778297316ef Mon Sep 17 00:00:00 2001 From: dllliu Date: Sun, 24 Mar 2024 13:35:24 -0400 Subject: [PATCH 089/145] test single and duo callback with launch param --- src/localization/gps_linearization.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 452d69281..746340f20 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -43,7 +43,7 @@ def __init__(self): self.ref_lon = rospy.get_param(param_name="gps_linearization/reference_point_longitude") self.ref_alt = rospy.get_param("gps_linearization/reference_point_altitude") self.ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) - self.both_gps = True#rospy.get_param("gps_linearization/use_both_gps") + self.both_gps = rospy.get_param("gps_linearization/use_both_gps") self.world_frame = rospy.get_param("world_frame") self.use_dop_covariance = rospy.get_param("global_ekf/use_gps_dop_covariance") @@ -54,7 +54,7 @@ def __init__(self): self.last_gps_msg = None self.last_pose = None self.last_imu_msg = None - + if self.both_gps: right_gps_sub = message_filters.Subscriber("right_gps_driver/fix", NavSatFix) left_gps_sub = message_filters.Subscriber("left_gps_driver/fix", NavSatFix) @@ -81,15 +81,14 @@ def single_gps_callback(self, msg: NavSatFix): msg.position_covariance = self.config_gps_covariance self.last_gps_msg = msg - print("using single callback") if self.last_imu_msg is not None: self.last_pose = self.get_linearized_pose_in_world(msg, self.last_imu_msg, self.ref_coord) self.publish_pose() + #print("using single callback") def duo_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): - print("double") """ Callback function that receives GPS messages, assigns their covariance matrix, and then publishes the linearized pose. @@ -112,6 +111,7 @@ def duo_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): self.last_pose = GPSLinearization.compute_gps_pose(right_cartesian=right_cartesian, left_cartesian=left_cartesian) self.publish_pose() + #print("double") def imu_callback(self, msg: ImuAndMag): """ From 357fbfdba1db2fc3e89a88b7d300175d5b49ade0 Mon Sep 17 00:00:00 2001 From: dllliu Date: Sun, 24 Mar 2024 13:36:35 -0400 Subject: [PATCH 090/145] rm temp --- src/localization/one_gps_gps_linearization.py | 149 ------------------ 1 file changed, 149 deletions(-) delete mode 100644 src/localization/one_gps_gps_linearization.py diff --git a/src/localization/one_gps_gps_linearization.py b/src/localization/one_gps_gps_linearization.py deleted file mode 100644 index 9c1a36d04..000000000 --- a/src/localization/one_gps_gps_linearization.py +++ /dev/null @@ -1,149 +0,0 @@ -#!/usr/bin/env python3 -from typing import Optional, Tuple - -import numpy as np -import rospy -from geometry_msgs.msg import PoseWithCovarianceStamped, PoseWithCovariance, Pose, Point, Quaternion -from mrover.msg import ImuAndMag -from pymap3d.enu import geodetic2enu -from sensor_msgs.msg import NavSatFix -from std_msgs.msg import Header -from util.SE3 import SE3 -from util.np_utils import numpify - - -class GPSLinearization: - """ - This node subscribes to GPS and IMU data, linearizes the GPS data - into ENU coordinates, then combines the linearized GPS position and the IMU - orientation into a pose estimate for the rover and publishes it. - """ - - last_gps_msg: Optional[NavSatFix] - last_imu_msg: Optional[ImuAndMag] - pose_publisher: rospy.Publisher - - # reference coordinates - ref_lat: float - ref_lon: float - ref_alt: float - - world_frame: str - - # covariance config - use_dop_covariance: bool - config_gps_covariance: np.ndarray - - def __init__(self): - # read required parameters, if they don't exist an error will be thrown - self.ref_lat = rospy.get_param("gps_linearization/reference_point_latitude") - self.ref_lon = rospy.get_param("gps_linearization/reference_point_longitude") - self.ref_alt = rospy.get_param("gps_linearization/reference_point_altitude") - self.world_frame = rospy.get_param("world_frame") - self.use_dop_covariance = rospy.get_param("global_ekf/use_gps_dop_covariance") - self.config_gps_covariance = np.array(rospy.get_param("global_ekf/gps_covariance", None)) - - self.last_gps_msg = None - self.last_imu_msg = None - - rospy.Subscriber("gps/fix", NavSatFix, self.gps_callback) - rospy.Subscriber("imu/data", ImuAndMag, self.imu_callback) - - - self.pose_publisher = rospy.Publisher("linearized_pose", PoseWithCovarianceStamped, queue_size=1) - - def gps_callback(self, msg: NavSatFix): - """ - Callback function that receives GPS messages, assigns their covariance matrix, - and then publishes the linearized pose. - - :param msg: The NavSatFix message containing GPS data that was just received - """ - if np.any(np.isnan([msg.latitude, msg.longitude, msg.altitude])): - return - - if not self.use_dop_covariance: - msg.position_covariance = self.config_gps_covariance - - self.last_gps_msg = msg - - if self.last_imu_msg is not None: - self.publish_pose() - - def imu_callback(self, msg: ImuAndMag): - """ - Callback function that receives IMU messages and publishes the linearized pose. - - :param msg: The Imu message containing IMU data that was just received - """ - self.last_imu_msg = msg - - if self.last_gps_msg is not None: - self.publish_pose() - - @staticmethod - def get_linearized_pose_in_world( - gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray - ) -> Tuple[SE3, np.ndarray]: - """ - Linearizes the GPS geodetic coordinates into ENU cartesian coordinates, - then combines them with the IMU orientation into a pose estimate relative - to the world frame, with corresponding covariance matrix. - - :param gps_msg: Message containing the rover's GPS coordinates and their corresponding - covariance matrix. - :param imu_msg: Message containing the rover's orientation from IMU, with - corresponding covariance matrix. - :param ref_coord: numpy array containing the geodetic coordinate which will be the origin - of the tangent plane, [latitude, longitude, altitude] - :returns: The pose consisting of linearized GPS and IMU orientation, and the corresponding - covariance matrix as a 6x6 numpy array where each row is [x, y, z, roll, pitch, yaw] - """ - # linearize GPS coordinates into cartesian - cartesian = np.array(geodetic2enu(gps_msg.latitude, gps_msg.longitude, gps_msg.altitude, *ref_coord, deg=True)) - - # ignore Z - cartesian[2] = 0 - - imu_quat = numpify(imu_msg.imu.orientation) - - # normalize to avoid rounding errors - imu_quat = imu_quat / np.linalg.norm(imu_quat) - pose = SE3.from_pos_quat(position=cartesian, quaternion=imu_quat) - - covariance = np.zeros((6, 6)) - covariance[:3, :3] = np.array([gps_msg.position_covariance]).reshape(3, 3) - covariance[3:, 3:] = np.array([imu_msg.imu.orientation_covariance]).reshape(3, 3) - - return pose, covariance - - def publish_pose(self): - """ - Publishes the linearized pose of the rover relative to the map frame, - as a PoseWithCovarianceStamped message. - """ - ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) - linearized_pose, covariance = self.get_linearized_pose_in_world(self.last_gps_msg, self.last_imu_msg, ref_coord) - - pose_msg = PoseWithCovarianceStamped( - header=Header(stamp=rospy.Time.now(), frame_id=self.world_frame), - pose=PoseWithCovariance( - pose=Pose( - position=Point(*linearized_pose.position), - orientation=Quaternion(*linearized_pose.rotation.quaternion), - ), - covariance=covariance.flatten().tolist(), - ), - ) - self.pose_publisher.publish(pose_msg) - - -def main(): - # start the node and spin to wait for messages to be received - rospy.init_node("gps_linearization") - GPSLinearization() - rospy.spin() - - -if __name__ == "__main__": - main() \ No newline at end of file From c74a0737163f4ffe162e6ac9505a047c3c368204 Mon Sep 17 00:00:00 2001 From: Pearl Lin Date: Sun, 31 Mar 2024 13:11:55 -0400 Subject: [PATCH 091/145] fixed some bugs in single/duo gps mechanism --- src/localization/gps_linearization.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 03bc7344b..736a90679 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -21,7 +21,7 @@ class GPSLinearization: orientation into a pose estimate for the rover and publishes it. """ - last_gps_msg: Optional[NavSatFix] + last_gps_msg: Optional[np.ndarray] last_imu_msg: Optional[ImuAndMag] pose_publisher: rospy.Publisher @@ -42,7 +42,7 @@ def __init__(self): self.ref_lat = rospy.get_param("gps_linearization/reference_point_latitude") self.ref_lon = rospy.get_param(param_name="gps_linearization/reference_point_longitude") self.ref_alt = rospy.get_param("gps_linearization/reference_point_altitude") - self.both_gps = False#rospy.get_param("gps_linearization/use_both_gps") + self.both_gps = True self.world_frame = rospy.get_param("world_frame") self.use_dop_covariance = rospy.get_param("global_ekf/use_gps_dop_covariance") @@ -80,12 +80,11 @@ def single_gps_callback(self, msg: NavSatFix): ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) - self.last_gps_msg = msg print("using single callback") if self.last_imu_msg is not None: - self.publish_pose() self.last_gps_msg = self.get_linearized_pose_in_world(msg, self.last_imu_msg, ref_coord) + self.publish_pose() def duo_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): @@ -96,6 +95,8 @@ def duo_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): :param msg: The NavSatFix message containing GPS data that was just received TODO: Handle invalid PVTs """ + + print("using duo gps") if np.any(np.isnan([right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude])): return if np.any(np.isnan([left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude])): @@ -176,7 +177,7 @@ def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: @staticmethod def get_linearized_pose_in_world( gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray - ) -> Tuple[SE3, np.ndarray]: + ) -> np.ndarray: """ Linearizes the GPS geodetic coordinates into ENU cartesian coordinates, then combines them with the IMU orientation into a pose estimate relative From 172c590862cd402e636d620ff6c67e248a371f1c Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Sun, 31 Mar 2024 15:59:25 -0400 Subject: [PATCH 092/145] Testing changes --- config/esw.yaml | 2 +- launch/localization.launch | 10 ++++++++-- src/localization/gps_driver.py | 12 +++++++----- src/localization/gps_linearization.py | 7 ++++--- 4 files changed, 20 insertions(+), 11 deletions(-) diff --git a/config/esw.yaml b/config/esw.yaml index 511d9698b..beea9cbe4 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -1,5 +1,5 @@ right_gps_driver: - port: "/dev/ttyACM1" + port: "/dev/gps" baud: 38400 frame_id: "base_link" diff --git a/launch/localization.launch b/launch/localization.launch index f9491cafa..86cabfdae 100644 --- a/launch/localization.launch +++ b/launch/localization.launch @@ -2,6 +2,7 @@ + @@ -26,8 +27,13 @@ - - + + + + + + + \ No newline at end of file diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index 5ac0728f1..61b519132 100755 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -19,7 +19,7 @@ from std_msgs.msg import Header from sensor_msgs.msg import NavSatFix from rtcm_msgs.msg import Message -# from mrover.msg import rtkStatus +from mrover.msg import rtkStatus import datetime @@ -30,7 +30,7 @@ def __init__(self): self.baud = rospy.get_param("baud") self.base_station_sub = rospy.Subscriber("/rtcm", Message, self.process_rtcm) self.gps_pub = rospy.Publisher("fix", NavSatFix, queue_size=1) - # self.rtk_fix_pub = rospy.Publisher("rtk_fix_status", rtkStatus, queue_size=1) + self.rtk_fix_pub = rospy.Publisher("rtk_fix_status", rtkStatus, queue_size=1) self.lock = threading.Lock() self.valid_offset = False @@ -58,10 +58,12 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: # skip if message could not be parsed if not msg: return + + msg_used = None + if rover_gps_data.identity == "RXM-RTCM": rospy.loginfo("RXM") - msg_used = msg.msgUsed if msg_used == 0: rospy.logwarn("RTCM Usage unknown\n") @@ -97,7 +99,7 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: altitude=parsed_altitude, ) ) - # self.rtk_fix_pub.publish(rtkStatus(msg_used)) + self.rtk_fix_pub.publish(rtkStatus(msg_used)) if msg.difSoln == 1: rospy.loginfo_throttle(3, "Differential correction applied") @@ -126,7 +128,7 @@ def main(): # change values rtk_manager = GPS_Driver() rtk_manager.connect() - # rover_gps_thread = threading.Thread(rtk_manager.gps_data_thread) + # rover_gps_thread = threading.Thread(target=rtk_manager.gps_data_thread) # rover_gps_thread.start() rtk_manager.gps_data_thread() rtk_manager.exit() diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 3d76c5244..69926a3c0 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -42,9 +42,10 @@ def __init__(self): self.ref_lat = rospy.get_param("gps_linearization/reference_point_latitude") self.ref_lon = rospy.get_param(param_name="gps_linearization/reference_point_longitude") self.ref_alt = rospy.get_param("gps_linearization/reference_point_altitude") - self.both_gps = True + self.both_gps = rospy.get_param("gps_linearization/use_both_gps") self.world_frame = rospy.get_param("world_frame") self.use_dop_covariance = rospy.get_param("global_ekf/use_gps_dop_covariance") + self.ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) # config gps and imu convariance matrices self.config_gps_covariance = np.array(rospy.get_param("global_ekf/gps_covariance", None)) @@ -79,12 +80,12 @@ def single_gps_callback(self, msg: NavSatFix): if not self.use_dop_covariance: msg.position_covariance = self.config_gps_covariance - ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) + print("using single callback") if self.last_imu_msg is not None: - self.last_gps_msg = self.get_linearized_pose_in_world(msg, self.last_imu_msg, ref_coord) + self.last_gps_msg = self.get_linearized_pose_in_world(msg, self.last_imu_msg, self.ref_coord) self.publish_pose() From 6229e0c081ce7d92b1ea978d0759c6967a77e5ab Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Sun, 31 Mar 2024 16:09:11 -0400 Subject: [PATCH 093/145] added basesation driver back in --- src/localization/basestation_gps_driver.py | 47 ++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100755 src/localization/basestation_gps_driver.py diff --git a/src/localization/basestation_gps_driver.py b/src/localization/basestation_gps_driver.py new file mode 100755 index 000000000..c5414ff38 --- /dev/null +++ b/src/localization/basestation_gps_driver.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 +import rospy +from serial import Serial +from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol +from rtcm_msgs.msg import Message + +def main() -> None: + rtcm_pub = rospy.Publisher('rtcm', Message, queue_size=1) + rospy.init_node('basestation_driver') + port = rospy.get_param("basestation_gps_driver/port") + baud = rospy.get_param("basestation_gps_driver/baud") + svin_started = False + svin_complete = False + + # connect to GPS over serial, only read UBX and RTCM messages + with Serial(port, baud, timeout=1) as ser: + reader = UBXReader(ser, protfilter=(UBX_PROTOCOL | RTCM3_PROTOCOL)) + while not rospy.is_shutdown(): + if ser.in_waiting: + (raw_msg, msg) = reader.read() + + # skip if message could not be parsed + if not msg: + continue + + # publish RTCM messages + if protocol(raw_msg) == RTCM3_PROTOCOL: + rtcm_pub.publish(Message(message=raw_msg)) + + # print survey-in status + elif msg.identity == "NAV-SVIN": + if not svin_started and msg.active: + svin_started = True + rospy.loginfo("basestation survey-in started") + if not svin_complete and msg.valid: + svin_complete = True + rospy.loginfo(f"basestation survey-in complete, accuracy = {msg.meanAcc}") + if svin_started and not svin_complete: + print(f"current accuracy: {msg.meanAcc}") + + # fix quality information + elif msg.identity == "NAV-PVT": + print(f"{'valid' if msg.gnssFixOk else 'invalid'} fix, {msg.numSV} satellites used") + + +if __name__ == '__main__': + main() \ No newline at end of file From c9efc2f5e091105813715babb47fbccd4f6b5643 Mon Sep 17 00:00:00 2001 From: nehakankanala Date: Tue, 2 Apr 2024 19:57:12 -0400 Subject: [PATCH 094/145] fixed single gps callback -set last_pose instead of last msg --- src/localization/gps_linearization.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 69926a3c0..80283688f 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -80,12 +80,10 @@ def single_gps_callback(self, msg: NavSatFix): if not self.use_dop_covariance: msg.position_covariance = self.config_gps_covariance - - - print("using single callback") + # print("using single callback") if self.last_imu_msg is not None: - self.last_gps_msg = self.get_linearized_pose_in_world(msg, self.last_imu_msg, self.ref_coord) + self.last_pose = self.get_linearized_pose_in_world(msg, self.last_imu_msg, self.ref_coord) self.publish_pose() @@ -97,8 +95,7 @@ def duo_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): :param msg: The NavSatFix message containing GPS data that was just received TODO: Handle invalid PVTs """ - - print("using duo gps") + # print("using duo gps") if np.any(np.isnan([right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude])): return if np.any(np.isnan([left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude])): From 2a94ae5f74347e278d3a9505c130989e6788b91f Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 4 Apr 2024 01:05:15 -0400 Subject: [PATCH 095/145] Format --- scripts/gps_plotting.py | 2 +- scripts/rtk_animation.py | 21 +++++++++-------- src/localization/basestation_gps_driver.py | 13 ++++++----- src/localization/gps_driver.py | 5 ++--- src/localization/gps_linearization.py | 26 ++++++++++++---------- src/simulator/simulator.hpp | 2 +- src/simulator/simulator.sensors.cpp | 2 +- 7 files changed, 38 insertions(+), 33 deletions(-) diff --git a/scripts/gps_plotting.py b/scripts/gps_plotting.py index f1b241290..7b88af137 100644 --- a/scripts/gps_plotting.py +++ b/scripts/gps_plotting.py @@ -109,4 +109,4 @@ plt.title("RTK Stationary test") plt.legend() plt.show() -# plt.savefig("rtk_plot.png") \ No newline at end of file +# plt.savefig("rtk_plot.png") diff --git a/scripts/rtk_animation.py b/scripts/rtk_animation.py index e0e69f3c7..ea12cd559 100644 --- a/scripts/rtk_animation.py +++ b/scripts/rtk_animation.py @@ -9,9 +9,10 @@ def brownian(x0, N, sigma): result = np.zeros(N) result[0] = x0 for i in range(1, N): - result[i] = result[i-1] + np.random.normal(0, sigma) + result[i] = result[i - 1] + np.random.normal(0, sigma) return result + ref_lat = 42.293195 ref_lon = -83.7096706 ref_alt = 234.1 @@ -40,24 +41,25 @@ def brownian(x0, N, sigma): # plt.show() fig, ax = plt.subplots(1, 2) -scat1 = ax[0].scatter(rtk_xs[0], rtk_ys[0], color='red', label='RTK', s=3) -scat2 = ax[1].scatter(no_rtk_xs[0], no_rtk_ys[0], color='blue', label='No RTK', s=3) +scat1 = ax[0].scatter(rtk_xs[0], rtk_ys[0], color="red", label="RTK", s=3) +scat2 = ax[1].scatter(no_rtk_xs[0], no_rtk_ys[0], color="blue", label="No RTK", s=3) # ax[0].set(xlim=(-50, 10), ylim=(-5, 90), xlabel='x (m)', ylabel='y (m)', title='RTK vs No RTK') # ax[0].grid(True) -ax[0].set(xlim=(-50, 10), ylim=(-5, 90), xlabel='x (m)', ylabel='y (m)') -ax[0].set_title('RTK', fontsize=20) +ax[0].set(xlim=(-50, 10), ylim=(-5, 90), xlabel="x (m)", ylabel="y (m)") +ax[0].set_title("RTK", fontsize=20) ax[0].grid(True) -ax[1].set(xlim=(-50, 10), ylim=(-5, 90), xlabel='x (m)', ylabel='y (m)') -ax[1].set_title('No RTK', fontsize=20) +ax[1].set(xlim=(-50, 10), ylim=(-5, 90), xlabel="x (m)", ylabel="y (m)") +ax[1].set_title("No RTK", fontsize=20) ax[1].grid(True) # ax[2].legend() + def update(frame): rtk_x = rtk_xs[:frame] rtk_y = rtk_ys[:frame] data = np.stack((rtk_x, rtk_y)).T scat1.set_offsets(data) - + no_rtk_x = no_rtk_xs[:frame] no_rtk_y = no_rtk_ys[:frame] data = np.stack((no_rtk_x, no_rtk_y)).T @@ -65,6 +67,7 @@ def update(frame): return scat1, scat2 + ani = animation.FuncAnimation(fig, update, frames=range(len(rtk_xs)), blit=True, interval=10) plt.show() -ani.save('rtk_vs_no_rtk.gif', writer='imagemagick', fps=30) \ No newline at end of file +ani.save("rtk_vs_no_rtk.gif", writer="imagemagick", fps=30) diff --git a/src/localization/basestation_gps_driver.py b/src/localization/basestation_gps_driver.py index c5414ff38..062c0aa9c 100755 --- a/src/localization/basestation_gps_driver.py +++ b/src/localization/basestation_gps_driver.py @@ -4,9 +4,10 @@ from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol from rtcm_msgs.msg import Message + def main() -> None: - rtcm_pub = rospy.Publisher('rtcm', Message, queue_size=1) - rospy.init_node('basestation_driver') + rtcm_pub = rospy.Publisher("rtcm", Message, queue_size=1) + rospy.init_node("basestation_driver") port = rospy.get_param("basestation_gps_driver/port") baud = rospy.get_param("basestation_gps_driver/baud") svin_started = False @@ -18,7 +19,7 @@ def main() -> None: while not rospy.is_shutdown(): if ser.in_waiting: (raw_msg, msg) = reader.read() - + # skip if message could not be parsed if not msg: continue @@ -37,11 +38,11 @@ def main() -> None: rospy.loginfo(f"basestation survey-in complete, accuracy = {msg.meanAcc}") if svin_started and not svin_complete: print(f"current accuracy: {msg.meanAcc}") - + # fix quality information elif msg.identity == "NAV-PVT": print(f"{'valid' if msg.gnssFixOk else 'invalid'} fix, {msg.numSV} satellites used") -if __name__ == '__main__': - main() \ No newline at end of file +if __name__ == "__main__": + main() diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index 61b519132..643202263 100755 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -58,9 +58,8 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: # skip if message could not be parsed if not msg: return - - msg_used = None + msg_used = None if rover_gps_data.identity == "RXM-RTCM": rospy.loginfo("RXM") @@ -79,7 +78,7 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: parsed_latitude = msg.lat parsed_longitude = msg.lon parsed_altitude = msg.hMSL - time = datetime.datetime(year=msg.year, month=msg.month, day=msg.day, hour= msg.hour, second=msg.second) + time = datetime.datetime(year=msg.year, month=msg.month, day=msg.day, hour=msg.hour, second=msg.second) time = rospy.Time(secs=time.timestamp() + (msg.nano / 1e6)) if not self.valid_offset: self.time_offset = rospy.Time.now() - time diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 80283688f..2aa244a56 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -54,7 +54,7 @@ def __init__(self): self.last_gps_msg = None self.last_pose = None self.last_imu_msg = None - + if self.both_gps: right_gps_sub = message_filters.Subscriber("right_gps_driver/fix", NavSatFix) left_gps_sub = message_filters.Subscriber("left_gps_driver/fix", NavSatFix) @@ -66,7 +66,6 @@ def __init__(self): rospy.Subscriber("imu/data", ImuAndMag, self.imu_callback) self.pose_publisher = rospy.Publisher("linearized_pose", PoseWithCovarianceStamped, queue_size=1) - def single_gps_callback(self, msg: NavSatFix): """ Callback function that receives GPS messages, assigns their covariance matrix, @@ -81,11 +80,10 @@ def single_gps_callback(self, msg: NavSatFix): msg.position_covariance = self.config_gps_covariance # print("using single callback") - + if self.last_imu_msg is not None: self.last_pose = self.get_linearized_pose_in_world(msg, self.last_imu_msg, self.ref_coord) self.publish_pose() - def duo_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): """ @@ -102,16 +100,22 @@ def duo_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): return right_cartesian = np.array( - geodetic2enu(right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude, *self.ref_coord, deg=True) + geodetic2enu( + right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude, *self.ref_coord, deg=True + ) ) left_cartesian = np.array( - geodetic2enu(left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude, *self.ref_coord, deg=True) + geodetic2enu( + left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude, *self.ref_coord, deg=True + ) ) - self.last_pose = GPSLinearization.compute_gps_pose(right_cartesian=right_cartesian, left_cartesian=left_cartesian) + self.last_pose = GPSLinearization.compute_gps_pose( + right_cartesian=right_cartesian, left_cartesian=left_cartesian + ) self.publish_pose() - #print("double") + # print("double") def imu_callback(self, msg: ImuAndMag): """ @@ -173,11 +177,9 @@ def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: pose = SE3(rover_position, SO3.from_matrix(rotation_matrix=rotation_matrix)) return pose - + @staticmethod - def get_linearized_pose_in_world( - gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray - ) -> np.ndarray: + def get_linearized_pose_in_world(gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray) -> np.ndarray: """ Linearizes the GPS geodetic coordinates into ENU cartesian coordinates, then combines them with the IMU orientation into a pose estimate relative diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 2b89e1cb4..1287c74b7 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -237,7 +237,7 @@ namespace mrover { mRollDist{0, 0.01}, mPitchDist{0, 0.01}, mYawDist{0, 0.01}; - + // drift rate in rad/minute about each axis R3 mOrientationDriftRate{0.0, 0.0, 1.0}; R3 mOrientationDrift = R3::Zero(); diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index 5c14cb778..f71898d10 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -181,7 +181,7 @@ namespace mrover { R3 leftGpsNoise{mGPSDist(mRNG), mGPSDist(mRNG), mGPSDist(mRNG)}, rightGpsNoise{mGPSDist(mRNG), mGPSDist(mRNG), mGPSDist(mRNG)}; leftGpsInMap += leftGpsNoise; - rightGpsInMap += rightGpsNoise; + rightGpsInMap += rightGpsNoise; mLeftGpsPub.publish(computeNavSatFix(leftGpsInMap, mGpsLinearizationReferencePoint, mGpsLinerizationReferenceHeading)); mRightGpsPub.publish(computeNavSatFix(rightGpsInMap, mGpsLinearizationReferencePoint, mGpsLinerizationReferenceHeading)); From 902df9745ddcc36eae1375a606205bf76a4a7059 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Thu, 4 Apr 2024 01:54:59 -0400 Subject: [PATCH 096/145] fix overshooting in long range state, fix water bottle detection --- src/navigation/context.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/navigation/context.py b/src/navigation/context.py index 8a05e3219..fb1dbe2fb 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -120,7 +120,7 @@ def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray] 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) - elif current_waypoint.type == WaypointType.WATER_BOTTLE: + elif current_waypoint.type.val == WaypointType.WATER_BOTTLE: return self.get_target_pos("bottle", in_odom) else: return None @@ -151,26 +151,28 @@ def __init__(self, ctx: Context, min_hits: int = MIN_HITS, max_hits: int = MAX_H def push_frame(self, tags: List[LongRangeTag]) -> None: """ Loops through our current list of our stored tags and checks if the new message includes each tag or doesn't. - If it does include it, we will increment our hit count for that tag id and reset the time we saw it. + If it does include it, we will increment our hit count for that tag id, store the new tag information, and reset the time we saw it. If it does not include it, we will decrement our hit count for that tag id, and if the hit count becomes zero, then we remove it from our stored list. If there are tag ids in the new message that we don't have stored, we will add it to our stored list. :param tags: a list of LongRangeTags sent by perception, which includes an id and bearing for each tag in the list """ + tags_ids = [tag.id for tag in tags] for _, cur_tag in list(self.__data.items()): - tags_ids = [tag.id for tag in tags] if cur_tag.tag.id not in tags_ids: cur_tag.hit_count -= DECREMENT_WEIGHT if cur_tag.hit_count <= 0: del self.__data[cur_tag.tag.id] else: cur_tag.hit_count += INCREMENT_WEIGHT - cur_tag.time = rospy.get_time() if cur_tag.hit_count > self.max_hits: 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=INCREMENT_WEIGHT, tag=tag, time=rospy.get_time()) + else: + self.__data[tag.id].tag = tag + cur_tag.time = rospy.get_time() def get(self, tag_id: int) -> Optional[LongRangeTag]: """ From c7f59e24dc5dc91df803acafba6af63009d7be36 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Thu, 4 Apr 2024 02:03:35 -0400 Subject: [PATCH 097/145] oops accessed wrong variable --- src/navigation/context.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/navigation/context.py b/src/navigation/context.py index fb1dbe2fb..0b89f491d 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -172,7 +172,7 @@ def push_frame(self, tags: List[LongRangeTag]) -> None: self.__data[tag.id] = self.TagData(hit_count=INCREMENT_WEIGHT, tag=tag, time=rospy.get_time()) else: self.__data[tag.id].tag = tag - cur_tag.time = rospy.get_time() + self.__data[tag.id].time = rospy.get_time() def get(self, tag_id: int) -> Optional[LongRangeTag]: """ From a6b414f6a815b6c37254ad811aafd54b2913d8b6 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Thu, 4 Apr 2024 12:31:15 -0400 Subject: [PATCH 098/145] misc cleanup --- src/localization/basestation_gps_driver.py | 4 +- src/localization/gps_driver.py | 50 ++++++++++------------ src/localization/gps_linearization.py | 13 +++--- 3 files changed, 30 insertions(+), 37 deletions(-) diff --git a/src/localization/basestation_gps_driver.py b/src/localization/basestation_gps_driver.py index 062c0aa9c..ba9d61efe 100755 --- a/src/localization/basestation_gps_driver.py +++ b/src/localization/basestation_gps_driver.py @@ -37,11 +37,11 @@ def main() -> None: svin_complete = True rospy.loginfo(f"basestation survey-in complete, accuracy = {msg.meanAcc}") if svin_started and not svin_complete: - print(f"current accuracy: {msg.meanAcc}") + rospy.loginfo(f"current accuracy: {msg.meanAcc}") # fix quality information elif msg.identity == "NAV-PVT": - print(f"{'valid' if msg.gnssFixOk else 'invalid'} fix, {msg.numSV} satellites used") + rospy.loginfo(f"{'valid' if msg.gnssFixOk else 'invalid'} fix, {msg.numSV} satellites used") if __name__ == "__main__": diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index 643202263..472895bf0 100755 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -6,16 +6,10 @@ acquire an RTK fix. """ import serial -import asyncio import rospy import threading -import numpy as np -from os import getenv - import rospy - -# from rover_msgs import GPS, RTCM -from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol, UBXMessage +from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL from std_msgs.msg import Header from sensor_msgs.msg import NavSatFix from rtcm_msgs.msg import Message @@ -24,6 +18,17 @@ class GPS_Driver: + port: str + baud: int + base_station_sub: rospy.Subscriber + gps_pub: rospy.Publisher + rtk_fix_pub: rospy.Publisher + lock: threading.Lock + valid_offset: bool + time_offset: float + ser: serial.Serial + reader: UBXReader + def __init__(self): rospy.init_node("gps_driver") self.port = rospy.get_param("port") @@ -47,22 +52,20 @@ def exit(self) -> None: # close connection self.ser.close() + # rospy subscriber automatically runs this callback in separate thread def process_rtcm(self, data) -> None: print("processing RTCM") with self.lock: - # rtcm_data = RTCM.decode(data) self.ser.write(data.message) - def parse_rover_gps_data(self, rover_gps_data) -> None: - msg = rover_gps_data + def parse_ubx_message(self, msg) -> None: # skip if message could not be parsed if not msg: return - msg_used = None - - if rover_gps_data.identity == "RXM-RTCM": + if msg.identity == "RXM-RTCM": rospy.loginfo("RXM") + msg_used = msg.msgUsed if msg_used == 0: rospy.logwarn("RTCM Usage unknown\n") @@ -70,10 +73,9 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: rospy.logwarn("RTCM message not used\n") elif msg_used == 2: rospy.loginfo("RTCM message successfully used by receiver\n") - # rospy.loginfo(vars(rover_gps_data)) - if rover_gps_data.identity == "NAV-PVT": + elif msg.identity == "NAV-PVT": rospy.loginfo("PVT") parsed_latitude = msg.lat parsed_longitude = msg.lon @@ -85,9 +87,7 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: self.valid_offset = True time = time + self.time_offset - - rospy.loginfo_throttle(3, f"{time} {rospy.Time.now()} {time-rospy.Time.now()} {self.time_offset}") - + # rospy.loginfo_throttle(3, f"{time} {rospy.Time.now()} {time-rospy.Time.now()} {self.time_offset}") message_header = Header(stamp=time, frame_id="base_link") self.gps_pub.publish( @@ -98,12 +98,10 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: altitude=parsed_altitude, ) ) - self.rtk_fix_pub.publish(rtkStatus(msg_used)) + self.rtk_fix_pub.publish(rtkStatus(msg.carrSoln)) if msg.difSoln == 1: rospy.loginfo_throttle(3, "Differential correction applied") - - # publidh to navsatstatus in navsatfix if msg.carrSoln == 0: rospy.logwarn_throttle(3, "No RTK") elif msg.carrSoln == 1: @@ -111,24 +109,20 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: elif msg.carrSoln == 2: rospy.loginfo_throttle(3, "RTK FIX") - if rover_gps_data.identity == "NAV-STATUS": + elif msg.identity == "NAV-STATUS": pass def gps_data_thread(self) -> None: - # TODO: add more message checks if needed while not rospy.is_shutdown(): with self.lock: if self.ser.in_waiting: - raw, rover_gps_data = self.reader.read() - parsed_gps_data = self.parse_rover_gps_data(rover_gps_data) + raw, msg = self.reader.read() + self.parse_ubx_message(msg) def main(): - # change values rtk_manager = GPS_Driver() rtk_manager.connect() - # rover_gps_thread = threading.Thread(target=rtk_manager.gps_data_thread) - # rover_gps_thread.start() rtk_manager.gps_data_thread() rtk_manager.exit() diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 2aa244a56..7ee589f75 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -21,8 +21,9 @@ class GPSLinearization: orientation into a pose estimate for the rover and publishes it. """ - last_gps_msg: Optional[np.ndarray] + last_gps_msg: Optional[NavSatFix] last_imu_msg: Optional[ImuAndMag] + last_pose: Optional[SE3] pose_publisher: rospy.Publisher # reference coordinates @@ -59,7 +60,7 @@ def __init__(self): right_gps_sub = message_filters.Subscriber("right_gps_driver/fix", NavSatFix) left_gps_sub = message_filters.Subscriber("left_gps_driver/fix", NavSatFix) sync_gps_sub = message_filters.ApproximateTimeSynchronizer([right_gps_sub, left_gps_sub], 10, 0.5) - sync_gps_sub.registerCallback(self.duo_gps_callback) + sync_gps_sub.registerCallback(self.dual_gps_callback) else: rospy.Subscriber("left_gps_driver/fix", NavSatFix, self.single_gps_callback) @@ -85,7 +86,7 @@ def single_gps_callback(self, msg: NavSatFix): self.last_pose = self.get_linearized_pose_in_world(msg, self.last_imu_msg, self.ref_coord) self.publish_pose() - def duo_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): + def dual_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): """ Callback function that receives GPS messages, assigns their covariance matrix, and then publishes the linearized pose. @@ -153,10 +154,8 @@ def publish_pose(self): self.pose_publisher.publish(pose_msg) @staticmethod - def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: + def compute_gps_pose(right_cartesian, left_cartesian) -> SE3: # TODO: give simulated GPS non zero altitude so we can stop erasing the z component - # left_cartesian[2] = 0 - # right_cartesian[2] = 0 vector_connecting = left_cartesian - right_cartesian vector_connecting[2] = 0 magnitude = np.linalg.norm(vector_connecting) @@ -179,7 +178,7 @@ def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: return pose @staticmethod - def get_linearized_pose_in_world(gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray) -> np.ndarray: + def get_linearized_pose_in_world(gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray) -> SE3: """ Linearizes the GPS geodetic coordinates into ENU cartesian coordinates, then combines them with the IMU orientation into a pose estimate relative From 7542cee29614f2475d8e1273885c1d3382e460b3 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 4 Apr 2024 13:01:55 -0400 Subject: [PATCH 099/145] Remove updating deps in CI, lead should be contacted if you are adding deps anyways --- .github/workflows/ci.yml | 8 -------- 1 file changed, 8 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index fe8490c2e..496aa10ca 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -22,16 +22,8 @@ jobs: lfs: "true" # This makes sure that $GITHUB_WORKSPACE is the catkin workspace path path: "src/mrover" - - name: Ensure Python Requirements - run: . /home/mrover/catkin_ws/src/mrover/venv/bin/activate && pip install --no-cache-dir -e "$GITHUB_WORKSPACE/src/mrover[dev]" - name: Style Check run: . /home/mrover/catkin_ws/src/mrover/venv/bin/activate && cd $GITHUB_WORKSPACE/src/mrover/ && ./style.sh - - name: Update ROS APT - if: github.event.pull_request.draft == false - run: runuser -u mrover -- rosdep update - - name: Ensure ROS APT Requirements - if: github.event.pull_request.draft == false - run: runuser -u mrover -- rosdep install --from-paths "$GITHUB_WORKSPACE/src" --ignore-src -r -y --rosdistro noetic - name: Copy Catkin Profiles if: github.event.pull_request.draft == false run: rsync -r $GITHUB_WORKSPACE/src/mrover/ansible/roles/build/files/profiles $GITHUB_WORKSPACE/.catkin_tools From 3a56c662c0b82380e81c57bca03c78c92ce07566 Mon Sep 17 00:00:00 2001 From: Riley Bridges <32557768+rbridges12@users.noreply.github.com> Date: Thu, 4 Apr 2024 15:00:39 -0400 Subject: [PATCH 100/145] only launch gps drivers on hw --- launch/localization.launch | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/launch/localization.launch b/launch/localization.launch index 86cabfdae..33acfa8ff 100644 --- a/launch/localization.launch +++ b/launch/localization.launch @@ -26,13 +26,12 @@ - - - + + - + From 26bcbc49d8e0e91bc6388e092664936747774b51 Mon Sep 17 00:00:00 2001 From: Pearl Lin Date: Thu, 4 Apr 2024 18:31:37 -0400 Subject: [PATCH 101/145] minor fixes from rtk PR --- src/localization/gps_linearization.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 7ee589f75..84aac80a4 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -13,6 +13,8 @@ from util.np_utils import numpify import message_filters +QUEUE_SIZE = 10 +SLOP = 0.5 class GPSLinearization: """ @@ -41,7 +43,7 @@ class GPSLinearization: def __init__(self): # read required parameters, if they don't exist an error will be thrown self.ref_lat = rospy.get_param("gps_linearization/reference_point_latitude") - self.ref_lon = rospy.get_param(param_name="gps_linearization/reference_point_longitude") + self.ref_lon = rospy.get_param("gps_linearization/reference_point_longitude") self.ref_alt = rospy.get_param("gps_linearization/reference_point_altitude") self.both_gps = rospy.get_param("gps_linearization/use_both_gps") self.world_frame = rospy.get_param("world_frame") @@ -59,7 +61,7 @@ def __init__(self): if self.both_gps: right_gps_sub = message_filters.Subscriber("right_gps_driver/fix", NavSatFix) left_gps_sub = message_filters.Subscriber("left_gps_driver/fix", NavSatFix) - sync_gps_sub = message_filters.ApproximateTimeSynchronizer([right_gps_sub, left_gps_sub], 10, 0.5) + sync_gps_sub = message_filters.ApproximateTimeSynchronizer([right_gps_sub, left_gps_sub], QUEUE_SIZE, SLOP) sync_gps_sub.registerCallback(self.dual_gps_callback) else: rospy.Subscriber("left_gps_driver/fix", NavSatFix, self.single_gps_callback) @@ -74,14 +76,15 @@ def single_gps_callback(self, msg: NavSatFix): :param msg: The NavSatFix message containing GPS data that was just received """ + + rospy.logdebug("using single gps") + if np.any(np.isnan([msg.latitude, msg.longitude, msg.altitude])): return if not self.use_dop_covariance: msg.position_covariance = self.config_gps_covariance - # print("using single callback") - if self.last_imu_msg is not None: self.last_pose = self.get_linearized_pose_in_world(msg, self.last_imu_msg, self.ref_coord) self.publish_pose() @@ -94,7 +97,9 @@ def dual_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): :param msg: The NavSatFix message containing GPS data that was just received TODO: Handle invalid PVTs """ - # print("using duo gps") + + rospy.logdebug("using dual gps") + if np.any(np.isnan([right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude])): return if np.any(np.isnan([left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude])): @@ -116,7 +121,6 @@ def dual_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): ) self.publish_pose() - # print("double") def imu_callback(self, msg: ImuAndMag): """ From 4235b71c830cb76fe2361b083bdc6e6c1651d425 Mon Sep 17 00:00:00 2001 From: Riley Bridges <32557768+rbridges12@users.noreply.github.com> Date: Thu, 4 Apr 2024 18:41:05 -0400 Subject: [PATCH 102/145] changed print to ros log --- src/localization/gps_driver.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index 472895bf0..f3862ec4c 100755 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -54,7 +54,7 @@ def exit(self) -> None: # rospy subscriber automatically runs this callback in separate thread def process_rtcm(self, data) -> None: - print("processing RTCM") + rospy.loginfo("processing RTCM") with self.lock: self.ser.write(data.message) From 16837b7f6e96b3754c38371fb943dcbe4706e095 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Sat, 6 Apr 2024 02:31:05 -0400 Subject: [PATCH 103/145] follow long range tag until it expires --- src/navigation/context.py | 15 +++++++++++---- src/navigation/long_range.py | 4 ++++ 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/src/navigation/context.py b/src/navigation/context.py index 0b89f491d..68ca8ab20 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -156,17 +156,25 @@ def push_frame(self, tags: List[LongRangeTag]) -> None: If there are tag ids in the new message that we don't have stored, we will add it to our stored list. :param tags: a list of LongRangeTags sent by perception, which includes an id and bearing for each tag in the list """ + # Update our current tags tags_ids = [tag.id for tag in tags] for _, cur_tag in list(self.__data.items()): + # if we don't see one of our tags in the new message, decrement its hit count if cur_tag.tag.id not in tags_ids: cur_tag.hit_count -= DECREMENT_WEIGHT if cur_tag.hit_count <= 0: - del self.__data[cur_tag.tag.id] + cur_tag.hit_count = 0 + # if we haven't seen the tag in a while, remove it from our list + time_difference = rospy.get_time() - cur_tag.time + if time_difference > LONG_RANGE_TAG_EXPIRATION_TIME_SECONDS: + del self.__data[cur_tag.tag.id] + # if we do see one of our tags in the new message, increment its hit count else: cur_tag.hit_count += INCREMENT_WEIGHT if cur_tag.hit_count > self.max_hits: cur_tag.hit_count = self.max_hits + # Add newly seen tags to our data structure and update current tag's information for tag in tags: if tag.id not in self.__data: self.__data[tag.id] = self.TagData(hit_count=INCREMENT_WEIGHT, tag=tag, time=rospy.get_time()) @@ -186,9 +194,8 @@ def get(self, tag_id: int) -> Optional[LongRangeTag]: 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 <= LONG_RANGE_TAG_EXPIRATION_TIME_SECONDS + # self.__data[tag_id].hit_count >= self.min_hits and + time_difference <= LONG_RANGE_TAG_EXPIRATION_TIME_SECONDS ): return self.__data[tag_id].tag else: diff --git a/src/navigation/long_range.py b/src/navigation/long_range.py index 554c47490..1924c5284 100644 --- a/src/navigation/long_range.py +++ b/src/navigation/long_range.py @@ -42,6 +42,10 @@ def get_target_pos(self, context) -> Optional[np.ndarray]: rover_direction = pose.rotation.direction_vector() bearing_to_tag = np.radians(tag.bearing) + # If you have not seen the tag in a while but are waiting until the expiration time is up, + # keep going towards where it was last seen (the direction you are heading), don't use an old bearing value + if context.env.long_range_tags._LongRangeTagStore__data[tag_id].hit_count <= 0: + bearing_to_tag = 0 # rover_direction rotated by bearing to tag bearing_rotation_mat = np.array( From 9191b5d0ce19423e0e50a39d6c673daadb53a5a2 Mon Sep 17 00:00:00 2001 From: nehakankanala Date: Sat, 6 Apr 2024 11:41:20 -0400 Subject: [PATCH 104/145] set left gps to be the one with the udev rule --- config/esw.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config/esw.yaml b/config/esw.yaml index 30e77c82c..3ca715a73 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -1,10 +1,10 @@ right_gps_driver: - port: "/dev/gps" + port: "/dev/ttyACM0" baud: 38400 frame_id: "base_link" left_gps_driver: - port: "/dev/ttyACM0" + port: "/dev/gps" baud: 38400 frame_id: "base_link" From 62444fad0bc0b8dc1ec5f3b15e5410f166a21f12 Mon Sep 17 00:00:00 2001 From: neuenfeldttj Date: Sat, 6 Apr 2024 12:03:40 -0400 Subject: [PATCH 105/145] gps change --- src/teleoperation/backend/consumers.py | 30 +++++++++++++------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index c025a3fa1..8113168a6 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -65,6 +65,20 @@ class GUIConsumer(JsonWebsocketConsumer): def connect(self): self.accept() try: + # ROS Parameters + self.mappings = rospy.get_param("teleop/joystick_mappings") + self.drive_config = rospy.get_param("teleop/drive_controls") + self.max_wheel_speed = rospy.get_param("rover/max_speed") + self.wheel_radius = rospy.get_param("wheel/radius") + self.max_angular_speed = self.max_wheel_speed / self.wheel_radius + self.ra_config = rospy.get_param("teleop/ra_controls") + self.ik_names = rospy.get_param("teleop/ik_multipliers") + self.RA_NAMES = rospy.get_param("teleop/ra_names") + self.brushless_motors = rospy.get_param("brushless_motors/controllers") + self.brushed_motors = rospy.get_param("brushed_motors/controllers") + self.xbox_mappings = rospy.get_param("teleop/xbox_mappings") + self.sa_config = rospy.get_param("teleop/sa_controls") + # Publishers self.twist_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1) self.led_pub = rospy.Publisher("/auton_led_cmd", String, queue_size=1) @@ -88,7 +102,7 @@ def connect(self): self.drive_moteus_sub = rospy.Subscriber( "/drive_controller_data", ControllerState, self.drive_controller_callback ) - self.gps_fix = rospy.Subscriber("/gps/fix", NavSatFix, self.gps_fix_callback) + self.gps_fix = rospy.Subscriber("/left_gps_driver/fix", NavSatFix, self.gps_fix_callback) self.drive_status_sub = rospy.Subscriber("/drive_status", MotorsStatus, self.drive_status_callback) self.led_sub = rospy.Subscriber("/led", LED, self.led_callback) self.nav_state_sub = rospy.Subscriber("/nav_state", StateMachineStateUpdate, self.nav_state_callback) @@ -117,20 +131,6 @@ def connect(self): self.heater_auto_shutoff_srv = rospy.ServiceProxy("science_change_heater_auto_shutoff_state", SetBool) # self.capture_photo_srv = rospy.ServiceProxy("capture_photo", CapturePhoto) - # ROS Parameters - self.mappings = rospy.get_param("teleop/joystick_mappings") - self.drive_config = rospy.get_param("teleop/drive_controls") - self.max_wheel_speed = rospy.get_param("rover/max_speed") - self.wheel_radius = rospy.get_param("wheel/radius") - self.max_angular_speed = self.max_wheel_speed / self.wheel_radius - self.ra_config = rospy.get_param("teleop/ra_controls") - self.ik_names = rospy.get_param("teleop/ik_multipliers") - self.RA_NAMES = rospy.get_param("teleop/ra_names") - self.brushless_motors = rospy.get_param("brushless_motors/controllers") - self.brushed_motors = rospy.get_param("brushed_motors/controllers") - self.xbox_mappings = rospy.get_param("teleop/xbox_mappings") - self.sa_config = rospy.get_param("teleop/sa_controls") - self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) self.flight_thread = threading.Thread(target=self.flight_attitude_listener) From 49682dca358b97ef699b84a6a7ce7e4269661e29 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Sat, 6 Apr 2024 12:08:42 -0400 Subject: [PATCH 106/145] Comment out non-drive can busses --- launch/esw_base.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/esw_base.launch b/launch/esw_base.launch index ccccaf969..8e97e2ece 100644 --- a/launch/esw_base.launch +++ b/launch/esw_base.launch @@ -7,7 +7,7 @@ output="screen"> - @@ -21,7 +21,7 @@ respawn="true" respawn_delay="2" output="screen"> - + --> From ba87040bd7dd2c7972b844ac876d3954b01cc460 Mon Sep 17 00:00:00 2001 From: neuenfeldttj Date: Sat, 6 Apr 2024 12:17:55 -0400 Subject: [PATCH 107/145] Change stream number --- src/teleoperation/frontend/src/components/AutonTask.vue | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/teleoperation/frontend/src/components/AutonTask.vue b/src/teleoperation/frontend/src/components/AutonTask.vue index 0c36c1a89..35018caa2 100644 --- a/src/teleoperation/frontend/src/components/AutonTask.vue +++ b/src/teleoperation/frontend/src/components/AutonTask.vue @@ -21,7 +21,7 @@

- +
From aa21895ac5a735342cee046d682a66ed63dffe85 Mon Sep 17 00:00:00 2001 From: neuenfeldttj Date: Sat, 6 Apr 2024 12:24:37 -0400 Subject: [PATCH 108/145] stream --- src/teleoperation/frontend/src/components/CameraFeed.vue | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/teleoperation/frontend/src/components/CameraFeed.vue b/src/teleoperation/frontend/src/components/CameraFeed.vue index e5304206d..00afae0b9 100644 --- a/src/teleoperation/frontend/src/components/CameraFeed.vue +++ b/src/teleoperation/frontend/src/components/CameraFeed.vue @@ -65,7 +65,7 @@ export default defineComponent({ }, mounted: function () { - // this.startStream(this.id) + this.startStream(this.id) this.$nextTick(() => { const canvas: HTMLCanvasElement = document.getElementById( 'stream-' + this.id From 009a437cec1f51c90dc7104aff50293bd81c3303 Mon Sep 17 00:00:00 2001 From: Sarah Shapin Date: Tue, 9 Apr 2024 19:53:41 -0400 Subject: [PATCH 109/145] Waypoints now display individually --- deps/dawn | 2 +- deps/manif | 2 +- .../src/components/AutonWaypointEditor.vue | 169 ++++++++---------- .../src/components/AutonWaypointItem.vue | 10 +- .../frontend/src/components/CameraFeed.vue | 1 - 5 files changed, 82 insertions(+), 102 deletions(-) diff --git a/deps/dawn b/deps/dawn index e12af2be9..5096820bf 160000 --- a/deps/dawn +++ b/deps/dawn @@ -1 +1 @@ -Subproject commit e12af2be97514e32ccc0d2d1ec1995bec7201c97 +Subproject commit 5096820bfc553c002ab97c05002b3c22c00a7f8f diff --git a/deps/manif b/deps/manif index 03c497df3..c8a9fcbfe 160000 --- a/deps/manif +++ b/deps/manif @@ -1 +1 @@ -Subproject commit 03c497df3364e999cec46fe4f974eae1959a34e1 +Subproject commit c8a9fcbfe157b55a7a860577febdc5350043a722 diff --git a/src/teleoperation/frontend/src/components/AutonWaypointEditor.vue b/src/teleoperation/frontend/src/components/AutonWaypointEditor.vue index f40ba77a9..178a8160d 100644 --- a/src/teleoperation/frontend/src/components/AutonWaypointEditor.vue +++ b/src/teleoperation/frontend/src/components/AutonWaypointEditor.vue @@ -1,87 +1,51 @@ - diff --git a/src/teleoperation/frontend/src/components/SATask.vue b/src/teleoperation/frontend/src/components/SATask.vue index 7b0bc13d0..e9776bdba 100644 --- a/src/teleoperation/frontend/src/components/SATask.vue +++ b/src/teleoperation/frontend/src/components/SATask.vue @@ -28,7 +28,7 @@
- +
diff --git a/src/teleoperation/frontend/src/router/index.ts b/src/teleoperation/frontend/src/router/index.ts index f34e022f1..495e00bf9 100644 --- a/src/teleoperation/frontend/src/router/index.ts +++ b/src/teleoperation/frontend/src/router/index.ts @@ -4,6 +4,7 @@ import DMESTask from '../components/DMESTask.vue' import AutonTask from '../components/AutonTask.vue' import ISHTask from '../components/ISHTask.vue' import SATask from '../components/SATask.vue' +import Cameras from '../components/Cameras.vue' const routes = [ { @@ -41,7 +42,17 @@ const routes = [ path: '/ISHTask', name: 'ISHTask', component: ISHTask + }, + { + path: '/Cameras', + name: 'Cameras', + component: Cameras, + props: { + isSA: false, + mission: 'other' + } } + ] const router = createRouter({ history: createWebHistory(), From 1aebad6670c57e9a145b1c6787e0bfada38d15bb Mon Sep 17 00:00:00 2001 From: MRover Laptop Date: Sun, 28 Apr 2024 14:28:50 -0400 Subject: [PATCH 122/145] Changes --- ansible/roles/jetson_services/files/rules/99-usb-serial.rules | 3 +++ config/esw.yaml | 2 +- config/teleop.yaml | 2 ++ src/teleoperation/backend/consumers.py | 3 ++- 4 files changed, 8 insertions(+), 2 deletions(-) diff --git a/ansible/roles/jetson_services/files/rules/99-usb-serial.rules b/ansible/roles/jetson_services/files/rules/99-usb-serial.rules index 99fcb6258..8d0736fdd 100644 --- a/ansible/roles/jetson_services/files/rules/99-usb-serial.rules +++ b/ansible/roles/jetson_services/files/rules/99-usb-serial.rules @@ -1,3 +1,6 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="1546", ATTRS{idProduct}=="01a9", SYMLINK+="gps_%n" SUBSYSTEM=="tty", ATTRS{idVendor}=="2341", ATTRS{idProduct}=="0058", SYMLINK+="imu" SUBSYSTEM=="video4linux", KERNEL=="video*", ATTRS{idVendor}=="1bcf", ATTRS{idProduct}=="0b12", ATTR{index}=="0", GROUP="video", SYMLINK+="arducam" +SUBSYSTEM=="video4linux", KERNEL=="video*", ATTRS{idVendor}=="2b03", ATTRS{idProduct}=="f582", ATTR{index}=="0", GROUP="video", SYMLINK+="zed" +SUBSYSTEM=="video4linux", KERNEL=="video*", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9601", ATTR{index}=="0", GROUP="video", SYMLINK+="mobcam" +SUBSYSTEM=="video4linux", KERNEL=="video*", ATTRS{idVendor}=="32e4", ATTRS{idProduct}=="9601", ATTR{index}=="0", GROUP="video", SYMLINK+="staticcam" diff --git a/config/esw.yaml b/config/esw.yaml index 14bbfb5ad..036864196 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -9,7 +9,7 @@ left_gps_driver: frame_id: "base_link" basestation_gps_driver: - port: "/dev/ttyUSB1" + port: "/dev/gps" baud: 38400 frame_id: "base_link" diff --git a/config/teleop.yaml b/config/teleop.yaml index 1b8c68952..27158d2ee 100644 --- a/config/teleop.yaml +++ b/config/teleop.yaml @@ -60,6 +60,8 @@ teleop: multiplier: 0.6 tilt: multiplier: 1 + pan: + multiplier: 1 xbox_mappings: left_x: 0 diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 8599c34f5..eb9ab9941 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -436,7 +436,8 @@ def get_axes_input( # angular_from_lateral = get_axes_input("left_right", 0.4, True) angular = get_axes_input("twist", 0.03, True, self.max_angular_speed * dampen) - linear += get_axes_input("tilt", 0.5, scale=0.1) + linear -= get_axes_input("tilt", 0.5, scale=0.1) + angular -= get_axes_input("pan", 0.5, scale=0.1) self.twist_pub.publish( Twist( From 1f8363c5f981cd626bd37784d48ef6f8b7dc5566 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Wed, 24 Apr 2024 20:00:00 -0400 Subject: [PATCH 123/145] Update --- launch/esw_base.launch | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/launch/esw_base.launch b/launch/esw_base.launch index 623a5f7cf..aa74f4820 100644 --- a/launch/esw_base.launch +++ b/launch/esw_base.launch @@ -20,7 +20,7 @@ - + @@ -29,20 +29,20 @@ - + - - + --> Date: Wed, 24 Apr 2024 20:03:35 -0400 Subject: [PATCH 124/145] Update --- config/esw.yaml | 2 +- src/localization/gps_driver.py | 0 2 files changed, 1 insertion(+), 1 deletion(-) mode change 100644 => 100755 src/localization/gps_driver.py diff --git a/config/esw.yaml b/config/esw.yaml index 036864196..885440901 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -600,7 +600,7 @@ motors: default_network_iface: "enp0s31f6" auton_led_driver: - port: "/dev/ttyACM0" + port: "/dev/led" baud: 115200 science: diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py old mode 100644 new mode 100755 From 9f72b9af6ed21c6d3cbfd927356b86a240592e5e Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Wed, 24 Apr 2024 20:04:20 -0400 Subject: [PATCH 125/145] Rearange ports --- launch/esw_base.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/esw_base.launch b/launch/esw_base.launch index aa74f4820..29a320a22 100644 --- a/launch/esw_base.launch +++ b/launch/esw_base.launch @@ -21,7 +21,7 @@ args="load mrover/GstWebsocketStreamerNodelet perception_nodelet_manager" output="screen"> - + @@ -48,7 +48,7 @@ args="load mrover/GstWebsocketStreamerNodelet perception_nodelet_manager" output="screen"> - + From 3b1dab166bee63c524f5fe39cce752289fbbc887 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Wed, 24 Apr 2024 21:50:24 -0400 Subject: [PATCH 126/145] Scale down max turn --- config/esw.yaml | 2 +- config/localization.yaml | 4 ++-- launch/autonomy.launch | 2 +- src/esw/drive_bridge/main.cpp | 10 ++++++++-- src/navigation/drive.py | 4 ++++ 5 files changed, 16 insertions(+), 6 deletions(-) diff --git a/config/esw.yaml b/config/esw.yaml index 885440901..62402f471 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -4,7 +4,7 @@ right_gps_driver: frame_id: "base_link" left_gps_driver: - port: "/dev/gps_0" + port: "/dev/gps" baud: 38400 frame_id: "base_link" diff --git a/config/localization.yaml b/config/localization.yaml index b6ad42762..698a69056 100644 --- a/config/localization.yaml +++ b/config/localization.yaml @@ -4,8 +4,8 @@ rover_frame: "base_link" use_odom_frame: false gps_linearization: - reference_point_latitude: 42.30008806193693 - reference_point_longitude: -83.6931540297569 + reference_point_latitude: 42.293195 + reference_point_longitude: -83.7096706 reference_point_altitude: 234.1 reference_heading: 90.0 use_both_gps: false diff --git a/launch/autonomy.launch b/launch/autonomy.launch index 2312591c3..bbfc5b521 100644 --- a/launch/autonomy.launch +++ b/launch/autonomy.launch @@ -3,7 +3,7 @@ - + diff --git a/src/esw/drive_bridge/main.cpp b/src/esw/drive_bridge/main.cpp index afa2092ec..0cdf2f106 100644 --- a/src/esw/drive_bridge/main.cpp +++ b/src/esw/drive_bridge/main.cpp @@ -4,6 +4,7 @@ #include #include #include +#include /* * Converts from a Twist (linear and angular velocity) to the individual wheel velocities @@ -44,7 +45,7 @@ auto main(int argc, char** argv) -> int { void moveDrive(geometry_msgs::Twist::ConstPtr const& msg) { // See 13.3.1.4 in "Modern Robotics" for the math - // Link: https://hades.mech.northwestern.edu/images/7/7f/MR.pdf + // Link: https://hades.mech.northwestern.edu/images/7/7f/MR.pdf auto forward = MetersPerSecond{msg->linear.x}; auto turn = RadiansPerSecond{msg->angular.z}; // TODO(quintin) Don't ask me to explain perfectly why we need to cancel out a meters unit in the numerator @@ -52,7 +53,12 @@ void moveDrive(geometry_msgs::Twist::ConstPtr const& msg) { auto delta = turn / Radians{1} * WHEEL_DISTANCE_INNER; // should be in m/s RadiansPerSecond left = (forward - delta) * WHEEL_LINEAR_TO_ANGULAR; RadiansPerSecond right = (forward + delta) * WHEEL_LINEAR_TO_ANGULAR; - + RadiansPerSecond maximal = std::max(abs(right), abs(left)); + if (maximal > RadiansPerSecond{10 * 2 * M_PI}) { + Dimensionless changeRatio = RadiansPerSecond{10 * 2 * M_PI} / maximal; + left = left * changeRatio; + right = right * changeRatio; + } { Velocity leftVelocity; leftVelocity.names = {"front_left", "middle_left", "back_left"}; diff --git a/src/navigation/drive.py b/src/navigation/drive.py index 2ed873124..0cfe2e758 100644 --- a/src/navigation/drive.py +++ b/src/navigation/drive.py @@ -10,6 +10,8 @@ from util.np_utils import angle_to_rotate, normalized from util.ros_utils import get_rosparam +import rospy + default_constants = { "max_driving_effort": 1.0, "min_driving_effort": -1.0, @@ -212,6 +214,8 @@ def get_drive_command( if drive_back: output[0].linear.x *= -1 + rospy.logerr(f"output: {output}, target: {target_pos}, diff: {target_pos - rover_pos}") + self._last_angular_error = angular_error self._last_target = target_pos return output From 771a726123c8c8c41f7494fc28f307695eed8e14 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Wed, 24 Apr 2024 21:56:39 -0400 Subject: [PATCH 127/145] Push --- src/esw/drive_bridge/main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/esw/drive_bridge/main.cpp b/src/esw/drive_bridge/main.cpp index 0cdf2f106..ab5ce98f2 100644 --- a/src/esw/drive_bridge/main.cpp +++ b/src/esw/drive_bridge/main.cpp @@ -54,8 +54,8 @@ void moveDrive(geometry_msgs::Twist::ConstPtr const& msg) { RadiansPerSecond left = (forward - delta) * WHEEL_LINEAR_TO_ANGULAR; RadiansPerSecond right = (forward + delta) * WHEEL_LINEAR_TO_ANGULAR; RadiansPerSecond maximal = std::max(abs(right), abs(left)); - if (maximal > RadiansPerSecond{10 * 2 * M_PI}) { - Dimensionless changeRatio = RadiansPerSecond{10 * 2 * M_PI} / maximal; + if (maximal > RadiansPerSecond{15 * 2 * M_PI}) { + Dimensionless changeRatio = RadiansPerSecond{15 * 2 * M_PI} / maximal; left = left * changeRatio; right = right * changeRatio; } From 45a34794e5c9013e9f02396d606d13df3342512f Mon Sep 17 00:00:00 2001 From: tabiosg Date: Sun, 28 Apr 2024 16:44:20 -0400 Subject: [PATCH 128/145] Scale outer wheel speeds --- src/esw/drive_bridge/main.cpp | 32 ++++-- src/util/units.hpp | 185 +++++++++++++++++++++------------- 2 files changed, 137 insertions(+), 80 deletions(-) diff --git a/src/esw/drive_bridge/main.cpp b/src/esw/drive_bridge/main.cpp index ab5ce98f2..18391baf8 100644 --- a/src/esw/drive_bridge/main.cpp +++ b/src/esw/drive_bridge/main.cpp @@ -17,6 +17,7 @@ void moveDrive(geometry_msgs::Twist::ConstPtr const& msg); ros::Publisher leftVelocityPub, rightVelocityPub; Meters WHEEL_DISTANCE_INNER; +Meters WHEEL_DISTANCE_OUTER; compound_unit> WHEEL_LINEAR_TO_ANGULAR; auto main(int argc, char** argv) -> int { @@ -24,7 +25,9 @@ auto main(int argc, char** argv) -> int { ros::NodeHandle nh; auto roverWidth = requireParamAsUnit(nh, "rover/width"); + auto roverLength = requireParamAsUnit(nh, "rover/length"); WHEEL_DISTANCE_INNER = roverWidth / 2; + WHEEL_DISTANCE_OUTER = sqrt((roverWidth / 2) * (roverWidth / 2) + (roverLength / 2) * (roverLength / 2)); auto ratioMotorToWheel = requireParamAsUnit(nh, "wheel/gear_ratio"); auto wheelRadius = requireParamAsUnit(nh, "wheel/radius"); @@ -50,26 +53,33 @@ void moveDrive(geometry_msgs::Twist::ConstPtr const& msg) { auto turn = RadiansPerSecond{msg->angular.z}; // TODO(quintin) Don't ask me to explain perfectly why we need to cancel out a meters unit in the numerator // I think it comes from the fact that there is a unit vector in the denominator of the equation - auto delta = turn / Radians{1} * WHEEL_DISTANCE_INNER; // should be in m/s - RadiansPerSecond left = (forward - delta) * WHEEL_LINEAR_TO_ANGULAR; - RadiansPerSecond right = (forward + delta) * WHEEL_LINEAR_TO_ANGULAR; - RadiansPerSecond maximal = std::max(abs(right), abs(left)); - if (maximal > RadiansPerSecond{15 * 2 * M_PI}) { - Dimensionless changeRatio = RadiansPerSecond{15 * 2 * M_PI} / maximal; - left = left * changeRatio; - right = right * changeRatio; + auto delta_inner = turn / Radians{1} * WHEEL_DISTANCE_INNER; // should be in m/s + auto delta_outer = turn / Radians{1} * WHEEL_DISTANCE_OUTER; + RadiansPerSecond left_inner = (forward - delta_inner) * WHEEL_LINEAR_TO_ANGULAR; + RadiansPerSecond right_inner = (forward + delta_inner) * WHEEL_LINEAR_TO_ANGULAR; + RadiansPerSecond left_outer = (forward - delta_outer) * WHEEL_LINEAR_TO_ANGULAR; + RadiansPerSecond right_outer = (forward + delta_outer) * WHEEL_LINEAR_TO_ANGULAR; + + constexpr auto MAX_SPEED = RevolutionsPerSecond{15}; + RadiansPerSecond maximal = std::max(abs(left_outer), abs(right_outer)); + if (maximal > MAX_SPEED) { + Dimensionless changeRatio = MAX_SPEED / maximal; + left_inner = left_inner * changeRatio; + right_inner = right_inner * changeRatio; + left_outer = left_outer * changeRatio; + right_outer = right_outer * changeRatio; } + { Velocity leftVelocity; leftVelocity.names = {"front_left", "middle_left", "back_left"}; - leftVelocity.velocities.resize(leftVelocity.names.size(), left.get()); - leftVelocity.velocities.resize(leftVelocity.names.size(), left.get()); + leftVelocity.velocities = {left_outer.get(), left_inner.get(), left_outer.get()}; leftVelocityPub.publish(leftVelocity); } { Velocity rightVelocity; rightVelocity.names = {"front_right", "middle_right", "back_right"}; - rightVelocity.velocities.resize(rightVelocity.names.size(), right.get()); + rightVelocity.velocities = {right_outer.get(), right_inner.get(), right_outer.get()}; rightVelocityPub.publish(rightVelocity); } } diff --git a/src/util/units.hpp b/src/util/units.hpp index 28cece93a..29128c3ac 100644 --- a/src/util/units.hpp +++ b/src/util/units.hpp @@ -32,6 +32,7 @@ namespace mrover { typename T::ampere_exp_t; typename T::kelvin_exp_t; typename T::byte_exp_t; + typename T::tick_exp_t; requires IsRatio; requires IsRatio; requires IsRatio; @@ -40,6 +41,7 @@ namespace mrover { requires IsRatio; requires IsRatio; requires IsRatio; + requires IsRatio; { t.get() } -> std::convertible_to; }; @@ -52,7 +54,8 @@ namespace mrover { std::is_same_v && std::is_same_v && std::is_same_v && - std::is_same_v; + std::is_same_v && + std::is_same_v; // // Structs @@ -75,7 +78,8 @@ namespace mrover { IsRatio RadianExp = zero_exp_t, IsRatio AmpereExp = zero_exp_t, IsRatio KelvinExp = zero_exp_t, - IsRatio ByteExp = zero_exp_t> + IsRatio ByteExp = zero_exp_t, + IsRatio TickExp = zero_exp_t> requires(Conversion::num != 0 && Conversion::den != 0) struct Unit { using rep_t = Rep; @@ -87,6 +91,7 @@ namespace mrover { using ampere_exp_t = AmpereExp; using kelvin_exp_t = KelvinExp; using byte_exp_t = ByteExp; + using tick_exp_t = TickExp; constexpr static Rep CONVERSION = static_cast(conversion_t::num) / conversion_t::den; @@ -104,14 +109,14 @@ namespace mrover { constexpr Unit(U const& other) requires AreExponentsSame { - rep = other.get(); + rep = other.rep; } template - [[nodiscard]] constexpr auto operator=(U const& rhs) -> Unit& + constexpr auto operator=(U const& rhs) -> Unit& requires AreExponentsSame { - rep = rhs.get(); + rep = rhs.rep; return *this; } }; @@ -127,6 +132,7 @@ namespace mrover { using ampere_exp_t = zero_exp_t; using kelvin_exp_t = zero_exp_t; using byte_exp_t = zero_exp_t; + using tick_exp_t = zero_exp_t; constexpr static float CONVERSION = 1.0f; @@ -165,6 +171,13 @@ namespace mrover { // Static Operations // + template + static auto constexpr make_no_conversion(IsArithmetic auto const& other) -> U { + U result; + result.rep = static_cast(other); + return result; + } + namespace detail { template @@ -177,7 +190,8 @@ namespace mrover { std::ratio_multiply, std::ratio_multiply, std::ratio_multiply, - std::ratio_multiply>; + std::ratio_multiply, + std::ratio_multiply>; }; template @@ -190,7 +204,8 @@ namespace mrover { std::ratio_add, std::ratio_add, std::ratio_add, - std::ratio_add>; + std::ratio_add, + std::ratio_add>; }; template @@ -206,6 +221,20 @@ namespace mrover { using type = typename compound::type, Us...>::type; }; + template + struct strip_conversion { + using type = Unit, + typename U::meter_exp_t, + typename U::kilogram_exp_t, + typename U::second_exp_t, + typename U::radian_exp_t, + typename U::ampere_exp_t, + typename U::kelvin_exp_t, + typename U::byte_exp_t, + typename U::tick_exp_t>; + }; + } // namespace detail template @@ -225,82 +254,94 @@ namespace mrover { // template - inline constexpr auto operator*(IsArithmetic auto const& n, U const& u) { - return U{static_cast(n) * u.rep}; + constexpr auto operator*(IsArithmetic auto const& n, U const& u) { + return make_no_conversion(n * u.rep); } template - inline constexpr auto operator*(U const& u, IsArithmetic auto const& n) { - return U{u.rep * static_cast(n)}; + constexpr auto operator*(U const& u, IsArithmetic auto const& n) { + return make_no_conversion(u.rep * n); } template - inline constexpr auto operator*(U1 const& u1, U2 const& u2) { - return multiply{u1.rep * u2.rep}; + constexpr auto operator*(U1 const& u1, U2 const& u2) { + return make_no_conversion>(u1.rep * u2.rep); } template - inline constexpr auto operator*=(U& u, IsArithmetic auto const& n) { - return u = u * static_cast(n); + constexpr auto operator*=(U& u, IsArithmetic auto const& n) { + return u = u * n; } template - inline constexpr auto operator/(U const& u, IsArithmetic auto const& n) { - return U{u.rep / static_cast(n)}; + constexpr auto operator/(U const& u, IsArithmetic auto const& n) { + return make_no_conversion(u.rep / n); } template - inline constexpr auto operator/(IsArithmetic auto const& n, U const& u) { - return inverse{static_cast(n) / u.rep}; + constexpr auto operator/(IsArithmetic auto const& n, U const& u) { + return make_no_conversion>(n / u.rep); } template - inline constexpr auto operator/(U1 const& u1, U2 const& u2) { - return multiply>{u1.rep / u2.rep}; + constexpr auto operator/(U1 const& u1, U2 const& u2) { + return make_no_conversion>>(u1.rep / u2.rep); } template - inline constexpr auto operator/=(U& u, IsArithmetic auto const& n) { - return u = u / static_cast(n); + constexpr auto operator/=(U& u, IsArithmetic auto const& n) { + return u = u / n; } template - inline constexpr auto operator-(U const& u) { - return U{-u.rep}; + constexpr auto operator-(U const& u) { + return make_no_conversion(-u.rep); } template requires AreExponentsSame - inline constexpr auto operator+(U1 const& u1, U2 const& u2) { - return U1{u1.rep + u2.rep}; + constexpr auto operator+(U1 const& u1, U2 const& u2) { + return make_no_conversion(u1.rep + u2.rep); } template requires AreExponentsSame - inline constexpr auto operator-(U1 const& u1, U2 const& u2) { - return U1{u1.rep - u2.rep}; + constexpr auto operator-(U1 const& u1, U2 const& u2) { + return make_no_conversion(u1.rep - u2.rep); } - inline constexpr auto operator+=(IsUnit auto& u1, IsUnit auto const& u2) { + constexpr auto operator+=(IsUnit auto& u1, IsUnit auto const& u2) { return u1 = u1 + u2; } - inline constexpr auto operator-=(IsUnit auto& u1, IsUnit auto const& u2) { + constexpr auto operator-=(IsUnit auto& u1, IsUnit auto const& u2) { return u1 = u1 - u2; } - inline constexpr auto operator<=>(IsUnit auto const& u1, IsUnit auto const& u2) { + constexpr auto operator<=>(IsUnit auto const& u1, IsUnit auto const& u2) { return u1.rep <=> u2.rep; } template - inline constexpr auto sqrt(U const& u) { - return root{std::sqrt(u.rep)}; + constexpr auto sqrt(U const& u) { + return make_no_conversion>(std::sqrt(u.rep)); + } + + template + constexpr auto abs(U const& u) { + return make_no_conversion(std::fabs(u.rep)); + } + + template + constexpr auto signum(U const& u) -> int { + bool is_zero = std::fabs(u.rep) < std::numeric_limits::epsilon(); + return is_zero ? 0 : std::signbit(u.rep) ? -1 + : 1; } template - inline constexpr auto abs(U const& u) { - return U{std::fabs(u.rep)}; + constexpr auto fmod(U const& u, typename U::rep_t n) { + return make_no_conversion(std::fmod(u.rep, n)); } // @@ -326,91 +367,97 @@ namespace mrover { using Hours = Unit, zero_exp_t, zero_exp_t, std::ratio<1>>; using Hertz = inverse; using Radians = Unit, zero_exp_t, zero_exp_t, zero_exp_t, std::ratio<1>>; - using tau_ratio_t = std::ratio<628318530717958647, 1000000000000000000>; // TODO(quintin) is this 0-head play? + using tau_ratio_t = std::ratio<62831853071795864, 10000000000000000>; // TODO(quintin) is this 0-head play? using Revolutions = Unit>; using RadiansPerSecond = compound_unit>; using Amperes = Unit, zero_exp_t, zero_exp_t, zero_exp_t, zero_exp_t, std::ratio<1>>; using Kelvin = Unit, zero_exp_t, zero_exp_t, zero_exp_t, zero_exp_t, zero_exp_t, std::ratio<1>>; using Bytes = Unit, zero_exp_t, zero_exp_t, zero_exp_t, zero_exp_t, zero_exp_t, zero_exp_t, std::ratio<1>>; - using Volts = Unit, std::ratio<2>, std::ratio<1>, std::ratio<-3>, zero_exp_t, std::ratio<-1>, zero_exp_t>; + using Volts = Unit, std::ratio<2>, std::ratio<1>, std::ratio<-3>, zero_exp_t, std::ratio<-1>>; + using Ticks = Unit, zero_exp_t, zero_exp_t, zero_exp_t, zero_exp_t, zero_exp_t, zero_exp_t, zero_exp_t, std::ratio<1>>; using RevolutionsPerSecond = compound_unit>; using MetersPerSecond = compound_unit>; + using RadiansPerMeter = compound_unit>; // // Literals // - inline constexpr auto operator""_m(unsigned long long int n) { + constexpr auto operator""_m(unsigned long long int n) { return Meters{n}; } - inline constexpr auto operator""_cm(unsigned long long int n) { + constexpr auto operator""_cm(unsigned long long int n) { return Centimeters{n}; } - inline constexpr auto operator""_mm(unsigned long long int n) { + constexpr auto operator""_mm(unsigned long long int n) { return Millimeters{n}; } - inline constexpr auto operator""_Hz(unsigned long long int n) { + constexpr auto operator""_Hz(unsigned long long int n) { return Hertz{n}; } - inline constexpr auto operator""_rad(unsigned long long int n) { + constexpr auto operator""_rad(unsigned long long int n) { return Radians{n}; } - inline constexpr auto operator""_rad_per_s(unsigned long long int n) { + constexpr auto operator""_rad_per_s(unsigned long long int n) { return RadiansPerSecond{n}; } - inline constexpr auto operator""_A(unsigned long long int n) { + constexpr auto operator""_A(unsigned long long int n) { return Amperes{n}; } - inline constexpr auto operator""_K(unsigned long long int n) { + constexpr auto operator""_K(unsigned long long int n) { return Kelvin{n}; } - inline constexpr auto operator""_B(unsigned long long int n) { + constexpr auto operator""_B(unsigned long long int n) { return Bytes{n}; } - inline constexpr auto operator""_V(unsigned long long int n) { + constexpr auto operator""_V(unsigned long long int n) { return Volts{n}; } - inline constexpr auto operator""_mps(unsigned long long int n) { + constexpr auto operator""_mps(unsigned long long int n) { return MetersPerSecond{n}; } - inline constexpr auto operator""_percent(unsigned long long int n) { + constexpr auto operator""_percent(unsigned long long int n) { return Percent{n}; } + constexpr auto operator""_ticks(unsigned long long int n) { + return Ticks{n}; + } + // // Specialization // - inline constexpr auto operator*(IsArithmetic auto const& n, Radians const& r) { - return Radians{std::fmod(static_cast(n) * r.rep, TAU) + static_cast(n) < 0 ? TAU : 0}; - } - - inline constexpr auto operator*(Radians const& r, IsArithmetic auto const& n) { - return operator*(n, r); - } - - inline constexpr auto operator-(Radians const& r) { - return Radians{std::fmod(TAU - r.rep, TAU)}; - } - - inline constexpr auto operator+(Radians const& r1, Radians const& r2) { - return Radians{std::fmod(r1.rep + r2.rep, TAU)}; - } - - inline constexpr auto operator-(Radians const& r1, Radians const& r2) { - return Radians{std::fmod(r1.rep - r2.rep + TAU, TAU)}; - } + // constexpr auto operator*(IsArithmetic auto const& n, Radians const& r) { + // return Radians{std::fmod(static_cast(n) * r.rep, TAU) + static_cast(n) < 0 ? TAU : 0}; + // } + // + // constexpr auto operator*(Radians const& r, IsArithmetic auto const& n) { + // return operator*(n, r); + // } + // + // constexpr auto operator-(Radians const& r) { + // return Radians{std::fmod(TAU - r.rep, TAU)}; + // } + // + // constexpr auto operator+(Radians const& r1, Radians const& r2) { + // return Radians{std::fmod(r1.rep + r2.rep, TAU)}; + // } + // + // constexpr auto operator-(Radians const& r1, Radians const& r2) { + // return Radians{std::fmod(r1.rep - r2.rep + TAU, TAU)}; + // } } // namespace mrover \ No newline at end of file From 5a807673a7d45ede85d4d74e74d3245c7463d764 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Wed, 24 Apr 2024 22:26:01 -0400 Subject: [PATCH 129/145] Bruh --- src/esw/drive_bridge/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/esw/drive_bridge/main.cpp b/src/esw/drive_bridge/main.cpp index 18391baf8..44f44ad7c 100644 --- a/src/esw/drive_bridge/main.cpp +++ b/src/esw/drive_bridge/main.cpp @@ -60,7 +60,7 @@ void moveDrive(geometry_msgs::Twist::ConstPtr const& msg) { RadiansPerSecond left_outer = (forward - delta_outer) * WHEEL_LINEAR_TO_ANGULAR; RadiansPerSecond right_outer = (forward + delta_outer) * WHEEL_LINEAR_TO_ANGULAR; - constexpr auto MAX_SPEED = RevolutionsPerSecond{15}; + constexpr auto MAX_SPEED = RadiansPerSecond{15 * 2 * M_PI}; RadiansPerSecond maximal = std::max(abs(left_outer), abs(right_outer)); if (maximal > MAX_SPEED) { Dimensionless changeRatio = MAX_SPEED / maximal; From 98975cff4404fd996ec355561768aa1ce16412de Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 27 Apr 2024 13:29:41 -0400 Subject: [PATCH 130/145] Refactor streaming --- ansible/roles/build/tasks/main.yml | 1 + .../gst_websocket_streamer.cpp | 125 +++++++++--------- .../gst_websocket_streamer.hpp | 15 +++ .../frontend/src/components/CameraFeed.vue | 77 ++++++----- 4 files changed, 127 insertions(+), 91 deletions(-) diff --git a/ansible/roles/build/tasks/main.yml b/ansible/roles/build/tasks/main.yml index 6d91a9651..c2c6e022f 100644 --- a/ansible/roles/build/tasks/main.yml +++ b/ansible/roles/build/tasks/main.yml @@ -97,6 +97,7 @@ - libgstreamer1.0-dev - libgstreamer-plugins-base1.0-dev - vainfo + - x264 - name: Install Local APT Packages become: True diff --git a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp index a1430549b..dd7af2f46 100644 --- a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp +++ b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp @@ -1,11 +1,5 @@ #include "gst_websocket_streamer.hpp" -#if defined(MROVER_IS_JETSON) -constexpr bool IS_JETSON = true; -#else -constexpr bool IS_JETSON = false; -#endif - namespace mrover { template @@ -26,7 +20,12 @@ namespace mrover { GstBuffer* buffer = gst_sample_get_buffer(sample); GstMapInfo map; gst_buffer_map(buffer, &map, GST_MAP_READ); - mStreamServer->broadcast({reinterpret_cast(map.data), map.size}); + + std::vector chunk(sizeof(ChunkHeader) + map.size); + std::memcpy(chunk.data(), &mChunkHeader, sizeof(ChunkHeader)); + std::memcpy(chunk.data() + sizeof(ChunkHeader), map.data, map.size); + + mStreamServer->broadcast(chunk); gst_buffer_unmap(buffer, &map); gst_sample_unref(sample); @@ -38,65 +37,63 @@ namespace mrover { mMainLoop = gstCheck(g_main_loop_new(nullptr, FALSE)); + // Source std::string launch; if (mCaptureDevice.empty()) { - if constexpr (IS_JETSON) { - // ReSharper disable once CppDFAUnreachableCode - launch = std::format( - // App source is pushed to when we get a ROS BGRA image message - // is-live prevents frames from being pushed when the pipeline is in READY - "appsrc name=imageSource is-live=true " - "! video/x-raw,format=BGRA,width={},height={},framerate=30/1 " - "! videoconvert " // Convert BGRA => I420 (YUV) for the encoder, note we are still on the CPU - "! video/x-raw,format=I420 " - "! nvvidconv " // Upload to GPU memory for the encoder - "! video/x-raw(memory:NVMM),format=I420 " - "! nvv4l2h265enc name=encoder bitrate={} iframeinterval=300 vbv-size=33333 insert-sps-pps=true control-rate=constant_bitrate profile=Main num-B-Frames=0 ratecontrol-enable=true preset-level=UltraFastPreset EnableTwopassCBR=false maxperf-enable=true " - // App sink is pulled from (getting H265 chunks) on another thread and sent to the stream server - // sync=false is needed to avoid weirdness, going from playing => ready => playing will not work otherwise - "! appsink name=streamSink sync=false", - mImageWidth, mImageHeight, mBitrate); - } else { - // ReSharper disable once CppDFAUnreachableCode - launch = std::format( - "appsrc name=imageSource is-live=true " - "! video/x-raw,format=BGRA,width={},height={},framerate=30/1 " - "! nvh265enc name=encoder " - "! appsink name=streamSink sync=false", - mImageWidth, mImageHeight); - } + // App source is pushed to when we get a ROS BGRA image message + // is-live prevents frames from being pushed when the pipeline is in READY + launch += "appsrc name=imageSource is-live=true "; } else { - if constexpr (IS_JETSON) { - // ReSharper disable once CppDFAUnreachableCode - - // TODO(quintin): I had to apply this patch: https://forums.developer.nvidia.com/t/macrosilicon-usb/157777/4 - // nvv4l2camerasrc only supports UYUV by default, but our cameras are YUY2 (YUYV) - - launch = std::format( + launch += std::format("v4l2src device={} ", mCaptureDevice); + } + // Source format + std::string captureFormat = mCaptureDevice.empty() ? "video/x-raw,format=BGRA" : mDecodeJpegFromDevice ? "image/jpeg" + : "video/x-raw,format=YUY2"; + launch += std::format("! {},width={},height={},framerate={}/1 ", + captureFormat, mImageWidth, mImageHeight, mImageFramerate); + // Source decoder and H265 encoder + if (gst_element_factory_find("nvv4l2h265enc")) { + // Most likely on the Jetson + if (captureFormat.contains("jpeg")) { + // Mostly used with USB cameras, MPEG capture uses way less USB bandwidth + launch += + // TODO(quintin): I had to apply this patch: https://forums.developer.nvidia.com/t/macrosilicon-usb/157777/4 + // nvv4l2camerasrc only supports UYUV by default, but our cameras are YUY2 (YUYV) // "nvv4l2camerasrc device={} " - - "v4l2src device={} " - // "! video/x-raw(memory:NVMM),format=YUY2,width={},height={},framerate={}/1 " - "! image/jpeg,width={},height={},framerate={}/1 " - "! nvv4l2decoder mjpeg=1 " - - "! nvvidconv " - "! video/x-raw(memory:NVMM),format=NV12 " - "! nvv4l2h265enc name=encoder bitrate={} iframeinterval=300 vbv-size=33333 insert-sps-pps=true control-rate=constant_bitrate profile=Main num-B-Frames=0 ratecontrol-enable=true preset-level=UltraFastPreset EnableTwopassCBR=false maxperf-enable=true " - "! appsink name=streamSink sync=false", - mCaptureDevice, mImageWidth, mImageHeight, mImageFramerate, mBitrate); + + "! nvv4l2decoder mjpeg=1 " // Hardware-accelerated JPEG decoding, output is apparently some unknown proprietary format + "! nvvidconv " // Convert from proprietary format to NV12 so the encoder understands it + "! video/x-raw(memory:NVMM),format=NV12 "; } else { - // ReSharper disable once CppDFAUnreachableCode - launch = std::format( - "v4l2src device={} " - "! video/x-raw,format=YUY2,width={},height={},framerate=30/1 " - "! videoconvert " - "! nvh265enc name=encoder " - "! appsink name=streamSink sync=false", - mCaptureDevice, mImageWidth, mImageHeight); + launch += "! videoconvert " // Convert to I420 for the encoder, note we are still on the CPU + "! video/x-raw,format=I420 " + "! nvvidconv " // Upload to GPU memory for the encoder + "! video/x-raw(memory:NVMM),format=I420 "; } + launch += std::format("! nvv4l2h265enc name=encoder bitrate={} iframeinterval=300 vbv-size=33333 insert-sps-pps=true control-rate=constant_bitrate profile=Main num-B-Frames=0 ratecontrol-enable=true preset-level=UltraFastPreset EnableTwopassCBR=false maxperf-enable=true ", + mBitrate); + mChunkHeader.codec = ChunkHeader::Codec::H265; + } else if (gst_element_factory_find("nvh265enc")) { + // For desktop/laptops with the custom NVIDIA bad gstreamer plugins built (a massive pain to do!) + if (captureFormat.contains("jpeg")) + launch += "! jpegdec "; + else if (captureFormat.contains("YUY2")) + launch += "! videoconvert "; + launch += "! nvh265enc name=encoder "; + mChunkHeader.codec = ChunkHeader::Codec::H265; + } else { + // For desktop/laptops with no hardware encoder + if (captureFormat.contains("jpeg")) + launch += "! jpegdec "; + else + launch += "! videoconvert "; + launch += "! x264enc name=encoder "; + mChunkHeader.codec = ChunkHeader::Codec::H264; } + // App sink is pulled from (getting H265 chunks) on another thread and sent to the stream server + // sync=false is needed to avoid weirdness, going from playing => ready => playing will not work otherwise + launch += "! appsink name=streamSink sync=false"; ROS_INFO_STREAM(std::format("GStreamer launch: {}", launch)); mPipeline = gstCheck(gst_parse_launch(launch.c_str(), nullptr)); @@ -145,12 +142,20 @@ namespace mrover { } } + auto widthAndHeightToResolution(std::uint32_t width, std::uint32_t height) -> ChunkHeader::Resolution { + if (width == 640 && height == 480) return ChunkHeader::Resolution::EGA; + if (width == 1280 && height == 720) return ChunkHeader::Resolution::HD; + if (width == 1920 && height == 1080) return ChunkHeader::Resolution::FHD; + throw std::runtime_error{"Unsupported resolution"}; + } + auto GstWebsocketStreamerNodelet::onInit() -> void { try { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); mCaptureDevice = mPnh.param("device", ""); + mDecodeJpegFromDevice = mPnh.param("decode_jpeg_from_device", true); mImageTopic = mPnh.param("image_topic", "image"); mImageWidth = mPnh.param("width", 640); mImageHeight = mPnh.param("height", 480); @@ -159,6 +164,8 @@ namespace mrover { auto port = mPnh.param("port", 8081); mBitrate = mPnh.param("bitrate", 2000000); + mChunkHeader.resolution = widthAndHeightToResolution(mImageWidth, mImageHeight); + gst_init(nullptr, nullptr); initPipeline(); @@ -198,7 +205,7 @@ namespace mrover { } } catch (std::exception const& e) { - ROS_ERROR_STREAM(std::format("Exception initializing NVIDIA GST H265 Encoder: {}", e.what())); + ROS_ERROR_STREAM(std::format("Exception initializing gstreamer websocket streamer: {}", e.what())); ros::requestShutdown(); } } diff --git a/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp b/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp index c4950f62c..7682a8a54 100644 --- a/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp +++ b/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp @@ -4,11 +4,24 @@ namespace mrover { + struct ChunkHeader { + enum struct Resolution : std::uint8_t { + EGA, + HD, + FHD, + } resolution; + enum struct Codec : std::uint8_t { + H265, + H264, + } codec; + }; + class GstWebsocketStreamerNodelet final : public nodelet::Nodelet { ros::NodeHandle mNh, mPnh; std::string mCaptureDevice; + bool mDecodeJpegFromDevice{}; std::string mImageTopic; std::uint64_t mBitrate{}; std::uint32_t mImageWidth{}, mImageHeight{}, mImageFramerate{}; @@ -22,6 +35,8 @@ namespace mrover { std::thread mMainLoopThread; std::thread mStreamSinkThread; + ChunkHeader mChunkHeader{}; + auto onInit() -> void override; auto pullStreamSamplesLoop() -> void; diff --git a/src/teleoperation/frontend/src/components/CameraFeed.vue b/src/teleoperation/frontend/src/components/CameraFeed.vue index efbce30af..698838b96 100644 --- a/src/teleoperation/frontend/src/components/CameraFeed.vue +++ b/src/teleoperation/frontend/src/components/CameraFeed.vue @@ -4,26 +4,26 @@

{{ name }} • ID: {{ id }}

- - - - -
- + > + + +
+
diff --git a/src/teleoperation/gui_chromium.sh b/src/teleoperation/gui_chromium.sh index 3f5e50061..e83d1c054 100755 --- a/src/teleoperation/gui_chromium.sh +++ b/src/teleoperation/gui_chromium.sh @@ -4,4 +4,6 @@ # Chromium currently only supports this when using VA-API hardware acceleration # It uses the WebCodecs API to decode # You can easily test if your setup works with this URL: https://w3c.github.io/webcodecs/samples/video-decode-display/ -chromium --enable-features=VaapiVideoDecodeLinuxGL --app=http://localhost:8080 +readonly FLAGS="--enable-features=VaapiVideoDecodeLinuxGL" +readonly ADDRESS="http://localhost:8080" +chromium ${FLAGS} --app=${ADDRESS}/$1 diff --git a/src/teleoperation/gui_chromium_cameras.sh b/src/teleoperation/gui_chromium_cameras.sh new file mode 100755 index 000000000..ef6bade5b --- /dev/null +++ b/src/teleoperation/gui_chromium_cameras.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +$(dirname $0)/gui_chromium.sh Cameras diff --git a/src/teleoperation/gui_chromium_menu.sh b/src/teleoperation/gui_chromium_menu.sh new file mode 100755 index 000000000..a259fcd1c --- /dev/null +++ b/src/teleoperation/gui_chromium_menu.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +$(dirname $0)/gui_chromium.sh From b476237fa5382bc01fac0e1a98fd013ad2815e48 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 27 Apr 2024 20:51:21 -0400 Subject: [PATCH 140/145] Better error msg --- src/esw/can_driver/can_driver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/esw/can_driver/can_driver.cpp b/src/esw/can_driver/can_driver.cpp index 7b9aa81c2..6355105f8 100644 --- a/src/esw/can_driver/can_driver.cpp +++ b/src/esw/can_driver/can_driver.cpp @@ -69,7 +69,7 @@ namespace mrover { } } else { NODELET_WARN("No CAN devices specified or config was invalid. Did you forget to load the correct ROS parameters?"); - NODELET_WARN("For example before testing the devboard run: \"rosparam load config/esw_devboard.yml\""); + NODELET_WARN("For example before testing the devboard run: \"rosparam load config/esw.yml\""); } mCanNetLink = CanNetLink{mInterface}; @@ -201,7 +201,7 @@ namespace mrover { } catch (boost::system::system_error const& error) { // check if ran out of buffer space if (error.code() == boost::asio::error::no_buffer_space) { - NODELET_WARN_STREAM_THROTTLE(1, "No buffer space available to send CAN message"); + NODELET_WARN_STREAM_THROTTLE(1, "No buffer space available to send CAN message. This usually indicates an electrical problem with the bus. CAN will avoid sending out messages if it can not see other devices."); return; } ros::shutdown(); From f4a9902d968a7c65233920dcf4659415f7e6785a Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 27 Apr 2024 20:56:05 -0400 Subject: [PATCH 141/145] Squashed commit of the following: commit b476237fa5382bc01fac0e1a98fd013ad2815e48 Author: qhdwight Date: Sat Apr 27 20:51:21 2024 -0400 Better error msg commit 0508b1be1f5fd8ee72de034686c2c2b694486efa Author: qhdwight Date: Sat Apr 27 17:01:51 2024 -0400 Oops commit e45f80659980bbe106e4a1be06f1c9d03f1f5958 Merge: 7dcfcc752 620179f10 Author: Quintin Date: Sat Apr 27 14:25:53 2024 -0400 Merge pull request #688 from umrover/camera-id Allow camera streaming to select a device from a physical port via udev commit 620179f104543323a06084765a81bb323514c5f3 Author: qhdwight Date: Sat Apr 27 14:22:25 2024 -0400 FOrmat commit 2cfb2b854190286813894813eed499cf9ca376a1 Author: qhdwight Date: Sat Apr 27 14:21:36 2024 -0400 Update launch file commit c1ae562f089bae32247bcf33529abc114e801c0d Author: qhdwight Date: Sat Apr 27 14:16:53 2024 -0400 Reorder commit 1c203f1d366246ba7c6ee35b4a23715dac1ef939 Merge: c29541595 7dcfcc752 Author: qhdwight Date: Sat Apr 27 14:16:19 2024 -0400 Merge remote-tracking branch 'origin/integration' into camera-id commit 7dcfcc7529fcc72579f2d433d8db7f3fd90a8d36 Author: qhdwight Date: Sat Apr 27 14:08:57 2024 -0400 Format commit 35e4a7e5bd1a58288a0b913bf988d07481a35afe Author: qhdwight Date: Sat Apr 27 14:07:06 2024 -0400 Add needed video conv commit 09e3fd6a0c27fe06ade8b9dddd2bae374f345317 Author: qhdwight Date: Sat Apr 27 13:51:49 2024 -0400 Comments commit 659fd47c5c0bf8caaf1ec274abf3f5fd2a30f027 Author: qhdwight Date: Sat Apr 27 13:47:07 2024 -0400 Zero tune latency, add to sim commit 98975cff4404fd996ec355561768aa1ce16412de Author: qhdwight Date: Sat Apr 27 13:29:41 2024 -0400 Refactor streaming commit 0710cfae809aeabeabafe032e2a407dbfd760ce1 Author: neuenfeldttj Date: Wed Apr 24 18:51:33 2024 -0400 fixed bearing commit 0b85e5a9e6f840c0862485c92ed6908dd5342577 Merge: b6d181bfb 4c766ade3 Author: neuenfeldttj Date: Wed Apr 24 18:33:59 2024 -0400 Merge branch 'integration' of github.com:umrover/mrover-ros into integration commit b6d181bfb93cb1e660b4583b69ff16d130646dbc Author: neuenfeldttj Date: Wed Apr 24 18:33:47 2024 -0400 added map stuff commit 4c766ade3c842f9b726d9dea784bb871723660d1 Author: nehakankanala Date: Wed Apr 24 18:16:58 2024 -0400 added gps driver back in commit ba1fd6a1bcb892912b2a95348ff097aa4e2dea5c Author: umroverPerception Date: Wed Apr 24 15:51:58 2024 -0400 make changes for mast gimbal commit b39873370d30a0149585420a4ef3da94e9d00bee Merge: 434d2e43e f2147db82 Author: umroverPerception Date: Wed Apr 24 14:19:22 2024 -0400 Merge remote-tracking branch 'origin/integration' into arm-practice commit f2147db8225576522a1a29c3fb6ab23f7b4370fe Author: jnnanni Date: Tue Apr 23 19:28:52 2024 -0400 SA_z encoder in inches commit 251462816b478707c9e3660e13d2fe897049d4de Author: qhdwight Date: Tue Apr 23 00:02:32 2024 -0400 Supress cuda warning commit 434d2e43ea5adf3d932d14c9b6a7cdd321ad8e5b Author: qhdwight Date: Mon Apr 22 21:50:24 2024 -0400 Add micro adjust for drive commit 270509ec8287004553d8f4ac155b3211007bc7a6 Merge: b93f5746f 546e3550e Author: qhdwight Date: Mon Apr 22 12:00:51 2024 -0400 Merge remote-tracking branch 'origin/integration' into arm-practice commit 546e3550e6760c790541ef92a84b8c1f3ca0ea8b Merge: c45919a41 916db0022 Author: neuenfeldttj Date: Sun Apr 21 21:32:53 2024 -0400 Merge branch 'integration' of github.com:umrover/mrover-ros into integration commit c45919a411c614a411597b7f9c8826c381112542 Author: neuenfeldttj Date: Sun Apr 21 21:32:35 2024 -0400 added zero sa_z commit 916db002230c8de1cbdeedfed6b3dd2eac2712e3 Author: umroverPerception Date: Sun Apr 21 15:23:10 2024 -0400 Fix compile commit b93f5746f75b5e2a9f83f65e8a4d5ccc688e80c9 Author: MRover Laptop Date: Sun Apr 21 14:05:42 2024 -0400 Tune pid commit 2b7704ab28c9d76ba4ca6d27bbb3487315558105 Author: qhdwight Date: Sun Apr 21 13:34:51 2024 -0400 Set non finite variables to 0 for fk commit 0beb164973fb4ee62d073fe6a604e7c133512136 Merge: 10130ea8b 06bf210ef Author: qhdwight Date: Sun Apr 21 12:13:19 2024 -0400 Merge remote-tracking branch 'origin/integration' into arm-practice commit 06bf210ef42cfd76c05fe65020aeb73fb90dabb1 Merge: 8bafb5059 cceaa56e8 Author: neuenfeldttj Date: Sun Apr 21 11:51:05 2024 -0400 pulled integration, added SA position fix commit cceaa56e8eb12ae4fc07e29f8f114650fa97ae17 Author: neuenfeldttj Date: Sun Apr 21 11:39:52 2024 -0400 fixed auton cameras commit 10130ea8b546850aec4f0752956a793312063647 Author: qhdwight Date: Sat Apr 20 18:56:03 2024 -0400 Fix arm controls commit e4e59a13ff725a9d3b987b80a569b00cbf162ea4 Author: umroverPerception Date: Sat Apr 20 18:55:37 2024 -0400 Updates commit 189b267db7c889df242d2ed786a894b030036008 Author: qhdwight Date: Sat Apr 20 17:29:43 2024 -0400 Update commit 671f07156e44fbad1dc923bb7523a2e9947ee6d8 Author: qhdwight Date: Sat Apr 20 17:01:30 2024 -0400 Temp fix commit ddb39f451af9991afc61ea3c1cb84ef37b22a179 Author: qhdwight Date: Sat Apr 20 16:55:46 2024 -0400 Add arm automation init commit 3837e83aabac4262ec797dd6726e9b0af5d854ce Merge: ac5fe3bde 33d430cfc Author: qhdwight Date: Sat Apr 20 15:45:52 2024 -0400 Merge remote-tracking branch 'origin/tjn/threejs_new' into arm-practice commit ac5fe3bde0992d2fddc64aa6383e6ce3c4cf79cb Author: qhdwight Date: Sat Apr 20 15:41:07 2024 -0400 Fix catkin profile commit c5174be310cabf943e712502039e5a354d543401 Author: qhdwight Date: Sat Apr 20 15:35:11 2024 -0400 Lower speed commit 6e357546c282f7a9935db57894e7eb01da40eaa9 Author: qhdwight Date: Sat Apr 20 15:30:55 2024 -0400 Avoid uneeded copy in usb camera, only replcae bits allocator if on gcc 13 commit c2954159503bffc7257b4a313c4b74201486d3a5 Merge: d24a6ba49 65c9dc912 Author: qhdwight Date: Fri Apr 19 00:48:53 2024 -0400 Merge remote-tracking branch 'origin/integration' into camera-id commit d24a6ba496deaf6f42dcbc75925ae914b84254a9 Merge: 40c8d4321 5eb699edf Author: qhdwight Date: Thu Apr 18 23:50:23 2024 -0400 Merge remote-tracking branch 'origin/integration' into camera-id commit 40c8d4321d6f8c25975420bfa5b0efff1192d6e2 Merge: b272f5db3 45dd19f85 Author: qhdwight Date: Thu Apr 18 23:47:30 2024 -0400 Merge remote-tracking branch 'origin/integration' into camera-id commit 33d430cfc96116d4c9cf6e16aba1b4082b74958f Author: qhdwight Date: Thu Apr 18 15:37:53 2024 -0400 Import commit 9612ab7dc091786770b6cc3bc28cd03d8a4bfd83 Merge: b26ea44aa 90d365c97 Author: qhdwight Date: Thu Apr 18 14:31:18 2024 -0400 Merge remote-tracking branch 'origin/integration' into tjn/threejs_new commit b272f5db39a94c39b55dea81ee04b64b4c6cb190 Merge: e102663ed 4319cd67b Author: qhdwight Date: Wed Apr 17 21:26:03 2024 -0400 Merge remote-tracking branch 'origin/integration' into camera-id commit b26ea44aa64031b1eb903d23413370531a9008eb Author: qhdwight Date: Wed Apr 17 21:06:33 2024 -0400 Fix arm base link commit 3a9bcbe450ff83d2c4a594c854dfcc1b5a234157 Merge: 049d49b8a 1aaff3532 Author: qhdwight Date: Wed Apr 17 20:52:43 2024 -0400 Merge remote-tracking branch 'origin/integration' into tjn/threejs_new commit 049d49b8a78584c1229a553224cf63d9ee27130f Author: qhdwight Date: Wed Apr 17 14:35:23 2024 -0400 Update commit 2af6befd842d74ab962d4df1a2351014fcc9cfb8 Author: qhdwight Date: Wed Apr 17 13:53:22 2024 -0400 Format commit 4c875e26ad87244845787b924481372c09995476 Merge: c89e6d5b2 cee42b1bf Author: qhdwight Date: Wed Apr 17 13:53:04 2024 -0400 Merge remote-tracking branch 'origin/integration' into tjn/threejs_new commit c89e6d5b276c8743e4400a207c508838cc549954 Author: qhdwight Date: Wed Apr 17 13:39:03 2024 -0400 Update commit 2841a0b4d7523df31c06c948d8afa0b0046b51e0 Merge: 5891cb14f ee9cbed80 Author: qhdwight Date: Wed Apr 17 11:42:48 2024 -0400 Merge remote-tracking branch 'origin/integration' into tjn/threejs_new commit 5891cb14f23a5b84eab5e6080a506c9da8b4f02a Author: neuenfeldttj Date: Tue Apr 16 22:20:36 2024 -0400 reorganized arm function commit b75e2583064b580a7d733547c0089b78ecb6f261 Author: neuenfeldttj Date: Tue Apr 16 11:35:57 2024 -0400 fixed ik msg commit e102663ed82658d43e8a365eb0332cea6e1c98c8 Author: qhdwight Date: Mon Apr 15 16:15:57 2024 -0400 Add device resizing, rename some things commit e2226605fef802a51279f4e4cfd696e2144b8862 Author: qhdwight Date: Sun Apr 14 15:12:05 2024 -0400 Start working on camera stuff commit 8bafb50593a3ec25e38b1461feb155ee2f55ab45 Author: neuenfeldttj Date: Sun Apr 14 13:22:59 2024 -0400 Fixed bug when gamepad is plugged in commit da25ed1ee4b0cae2dc31a144f906d53992626f37 Author: jnnanni Date: Sun Apr 14 13:11:44 2024 -0400 Fix sending arm positions commit 59687e8982c1b722ff9180ac4af2f3e00d07145c Merge: 8fcb4dc07 268f19d64 Author: neuenfeldttj Date: Sat Apr 13 19:48:09 2024 -0400 pulled integration, ik doesnt work commit 8fcb4dc07e0a9630cdb6fe4b27c18852b2ea5c9d Author: neuenfeldttj Date: Sun Mar 17 16:49:07 2024 -0400 fk and ik working commit e4b4dacf9ccbe757732a2005ca94021febc0fafd Merge: fc979b294 695f8302c Author: neuenfeldttj Date: Sun Mar 17 14:13:13 2024 -0400 Merge remote-tracking branch 'origin/integration' into tjn/threejs_new commit fc979b294f5fad5f40383ef3fb3635e50f6bfc43 Author: neuenfeldttj Date: Sun Mar 17 14:11:17 2024 -0400 Temp commit 4445a32143ef9c2dc5dfa14a1f1f30c53c391498 Author: neuenfeldttj Date: Sun Mar 10 13:39:05 2024 -0400 fk works commit 3bfc63807aac75a0e2ef182c13e2082bb340551d Author: neuenfeldttj Date: Sun Feb 18 13:59:21 2024 -0500 joint a translation works. de does not commit 52166d762268bfdf9f99aa79dcd6959281d22e9c Author: neuenfeldttj Date: Sat Feb 17 22:02:07 2024 -0500 fk works --- CMakeLists.txt | 16 +- action/ArmAction.action | 3 + .../roles/build/files/profiles/ci/config.yaml | 4 +- .../build/files/profiles/debug/config.yaml | 4 +- .../build/files/profiles/release/config.yaml | 4 +- ansible/roles/build/tasks/main.yml | 1 + ansible/roles/esw/tasks/main.yml | 2 +- cmake/deps.cmake | 2 + config/esw.yaml | 15 +- config/moteus/joint_c.cfg | 4 +- config/teleop.yaml | 47 +-- launch/basestation.launch | 9 +- launch/esw_base.launch | 130 +++---- launch/simulator.launch | 5 + scripts/debug_arm_ik.py | 21 +- setup.py | 1 + src/esw/can_driver/can_driver.cpp | 4 +- src/esw/drive_bridge/main.cpp | 3 +- .../gst_websocket_streamer.cpp | 188 +++++---- .../gst_websocket_streamer.hpp | 32 +- src/esw/gst_websocket_streamer/pch.hpp | 14 +- src/preload/bits/allocator.h | 6 +- src/simulator/simulator.cpp | 3 +- src/simulator/simulator.hpp | 1 + src/simulator/simulator.render.cpp | 13 + src/simulator/simulator.sensors.cpp | 20 +- src/teleoperation/arm_automation/__init__.py | 0 .../arm_automation/arm_automation.py | 76 ++++ .../arm_controller/arm_controller.cpp | 20 +- src/teleoperation/arm_controller/pch.hpp | 1 + src/teleoperation/backend/consumers.py | 358 +++++++++--------- src/teleoperation/frontend/index.html | 2 +- src/teleoperation/frontend/package.json | 1 + .../frontend/public/meshes/arm_a.fbx | 3 + .../frontend/public/meshes/arm_b.fbx | 3 + .../frontend/public/meshes/arm_c.fbx | 3 + .../frontend/public/meshes/arm_d.fbx | 3 + .../frontend/public/meshes/arm_e.fbx | 3 + .../frontend/public/meshes/bottle.fbx | 3 + .../frontend/public/meshes/ground.fbx | 3 + .../frontend/public/meshes/hammer.fbx | 3 + .../public/meshes/primitives/cube.fbx | 3 + .../public/meshes/primitives/cylinder.fbx | 3 + .../public/meshes/primitives/sphere.fbx | 3 + .../frontend/public/meshes/rock.fbx | 3 + .../frontend/public/meshes/rover_chassis.fbx | 3 + .../public/meshes/rover_left_bogie.fbx | 3 + .../public/meshes/rover_left_rocker.fbx | 3 + .../public/meshes/rover_left_wheel.fbx | 3 + .../public/meshes/rover_right_bogie.fbx | 3 + .../public/meshes/rover_right_rocker.fbx | 3 + .../public/meshes/rover_right_wheel.fbx | 3 + .../frontend/src/components/ArmControls.vue | 58 ++- .../frontend/src/components/AutonTask.vue | 28 +- .../frontend/src/components/CameraFeed.vue | 103 ++--- .../frontend/src/components/DMESTask.vue | 20 +- .../frontend/src/components/Rover3D.vue | 57 +++ .../frontend/src/components/SAArmControls.vue | 58 ++- .../frontend/src/components/SATask.vue | 29 +- .../frontend/src/router/index.ts | 9 +- src/teleoperation/frontend/src/rover_three.js | 162 ++++++++ urdf/rover/rover.urdf.xacro | 2 +- 62 files changed, 1085 insertions(+), 508 deletions(-) create mode 100644 action/ArmAction.action create mode 100644 src/teleoperation/arm_automation/__init__.py create mode 100755 src/teleoperation/arm_automation/arm_automation.py create mode 100644 src/teleoperation/frontend/public/meshes/arm_a.fbx create mode 100644 src/teleoperation/frontend/public/meshes/arm_b.fbx create mode 100644 src/teleoperation/frontend/public/meshes/arm_c.fbx create mode 100644 src/teleoperation/frontend/public/meshes/arm_d.fbx create mode 100644 src/teleoperation/frontend/public/meshes/arm_e.fbx create mode 100644 src/teleoperation/frontend/public/meshes/bottle.fbx create mode 100644 src/teleoperation/frontend/public/meshes/ground.fbx create mode 100644 src/teleoperation/frontend/public/meshes/hammer.fbx create mode 100644 src/teleoperation/frontend/public/meshes/primitives/cube.fbx create mode 100644 src/teleoperation/frontend/public/meshes/primitives/cylinder.fbx create mode 100644 src/teleoperation/frontend/public/meshes/primitives/sphere.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rock.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rover_chassis.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rover_left_bogie.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rover_left_rocker.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rover_left_wheel.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rover_right_bogie.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rover_right_rocker.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rover_right_wheel.fbx create mode 100644 src/teleoperation/frontend/src/components/Rover3D.vue create mode 100644 src/teleoperation/frontend/src/rover_three.js diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c68974f1..e02e2f0a7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,15 +67,19 @@ set(MROVER_ROS_DEPENDENCIES tf2 tf2_ros tf2_geometry_msgs + actionlib_msgs ) extract_filenames(msg/*.msg MROVER_MESSAGE_FILES) extract_filenames(srv/*.srv MROVER_SERVICE_FILES) +extract_filenames(action/*.action MROVER_ACTION_FILES) + set(MROVER_MESSAGE_DEPENDENCIES std_msgs sensor_msgs + actionlib_msgs ) set(MROVER_PARAMETERS @@ -98,7 +102,7 @@ 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") + set(CMAKE_CUDA_FLAGS "--diag-suppress 20012") # Jetson Xavier NX/AGX has Volta 7.2 architecture # Perception Laptop (A4000, Quadro version of RTX 3080) has Ampere 8.6 architecture set(CMAKE_CUDA_ARCHITECTURES 72 86) @@ -123,6 +127,8 @@ add_message_files(FILES ${MROVER_MESSAGE_FILES}) add_service_files(FILES ${MROVER_SERVICE_FILES}) +add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) + generate_messages(DEPENDENCIES ${MROVER_MESSAGE_DEPENDENCIES}) generate_dynamic_reconfigure_options(${MROVER_PARAMETERS}) @@ -197,9 +203,11 @@ if (CUDA_FOUND) # target_include_directories(nv_vid_codec_h265_enc SYSTEM PUBLIC deps/nvenc) endif () -mrover_add_nodelet(gst_websocket_streamer src/esw/gst_websocket_streamer/*.c* src/esw/gst_websocket_streamer src/esw/gst_websocket_streamer/pch.hpp) -mrover_nodelet_link_libraries(gst_websocket_streamer PRIVATE websocket_server ${Gst_LIBRARIES} ${GstApp_LIBRARIES}) -mrover_nodelet_include_directories(gst_websocket_streamer ${Gst_INCLUDE_DIRS} ${GstApp_INCLUDE_DIRS}) +if (Gst_FOUND AND GstApp_FOUND AND LibUsb_FOUND AND LibUdev_FOUND) + mrover_add_nodelet(gst_websocket_streamer src/esw/gst_websocket_streamer/*.c* src/esw/gst_websocket_streamer src/esw/gst_websocket_streamer/pch.hpp) + mrover_nodelet_link_libraries(gst_websocket_streamer PRIVATE websocket_server ${Gst_LIBRARIES} ${GstApp_LIBRARIES} ${LibUsb_LIBRARIES} ${LibUdev_LIBRARIES} opencv_core opencv_imgproc) + mrover_nodelet_include_directories(gst_websocket_streamer ${Gst_INCLUDE_DIRS} ${GstApp_INCLUDE_DIRS} ${LibUsb_INCLUDE_DIRS} ${LibUdev_INCLUDE_DIRS}) +endif () if (ZED_FOUND) mrover_add_nodelet(object_detector src/perception/object_detector/*.c* src/perception/object_detector src/perception/object_detector/pch.hpp) diff --git a/action/ArmAction.action b/action/ArmAction.action new file mode 100644 index 000000000..9cc4f5321 --- /dev/null +++ b/action/ArmAction.action @@ -0,0 +1,3 @@ +string name +--- +--- diff --git a/ansible/roles/build/files/profiles/ci/config.yaml b/ansible/roles/build/files/profiles/ci/config.yaml index d14216f40..c0346527b 100644 --- a/ansible/roles/build/files/profiles/ci/config.yaml +++ b/ansible/roles/build/files/profiles/ci/config.yaml @@ -5,9 +5,9 @@ catkin_make_args: [ ] cmake_args: - -DCMAKE_BUILD_TYPE=Release - -DCMAKE_C_COMPILER=clang-16 - - -DCMAKE_CXX_FLAGS=-pipe + - -DCMAKE_CXX_FLAGS=-pipe -fgnuc-version=13.1.0 - -DCMAKE_CXX_COMPILER=clang++-16 - - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 + - -DCMAKE_CUDA_HOST_COMPILER=g++-9 - -DMROVER_IS_CI=ON - -Wno-dev devel_layout: linked diff --git a/ansible/roles/build/files/profiles/debug/config.yaml b/ansible/roles/build/files/profiles/debug/config.yaml index 71005790a..434ae5ae9 100644 --- a/ansible/roles/build/files/profiles/debug/config.yaml +++ b/ansible/roles/build/files/profiles/debug/config.yaml @@ -5,11 +5,11 @@ catkin_make_args: [ ] cmake_args: - -DCMAKE_BUILD_TYPE=Debug - -DCMAKE_C_COMPILER=clang-16 - - -DCMAKE_CXX_FLAGS=-pipe + - -DCMAKE_CXX_FLAGS=-pipe -fgnuc-version=13.1.0 - -DCMAKE_CXX_COMPILER=clang++-16 - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache - - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 + - -DCMAKE_CUDA_HOST_COMPILER=g++-9 - -Wno-dev devel_layout: linked devel_space: devel diff --git a/ansible/roles/build/files/profiles/release/config.yaml b/ansible/roles/build/files/profiles/release/config.yaml index f0c29e538..ef35c5cb7 100644 --- a/ansible/roles/build/files/profiles/release/config.yaml +++ b/ansible/roles/build/files/profiles/release/config.yaml @@ -5,11 +5,11 @@ catkin_make_args: [ ] cmake_args: - -DCMAKE_BUILD_TYPE=Release - -DCMAKE_C_COMPILER=clang-16 - - -DCMAKE_CXX_FLAGS=-march=native -pipe + - -DCMAKE_CXX_FLAGS=-march=native -pipe -fgnuc-version=13.1.0 - -DCMAKE_CXX_COMPILER=clang++-16 - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache - - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 + - -DCMAKE_CUDA_HOST_COMPILER=g++-9 - -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON - -Wno-dev devel_layout: linked diff --git a/ansible/roles/build/tasks/main.yml b/ansible/roles/build/tasks/main.yml index 6d91a9651..c2c6e022f 100644 --- a/ansible/roles/build/tasks/main.yml +++ b/ansible/roles/build/tasks/main.yml @@ -97,6 +97,7 @@ - libgstreamer1.0-dev - libgstreamer-plugins-base1.0-dev - vainfo + - x264 - name: Install Local APT Packages become: True diff --git a/ansible/roles/esw/tasks/main.yml b/ansible/roles/esw/tasks/main.yml index f9a81d7a5..9ae41142b 100644 --- a/ansible/roles/esw/tasks/main.yml +++ b/ansible/roles/esw/tasks/main.yml @@ -22,4 +22,4 @@ - name: Install Moteus GUI pip: - name: moteus_gui + name: moteus-gui diff --git a/cmake/deps.cmake b/cmake/deps.cmake index b73637ce8..b3bb4a520 100644 --- a/cmake/deps.cmake +++ b/cmake/deps.cmake @@ -69,3 +69,5 @@ pkg_search_module(NetLink libnl-3.0 QUIET) pkg_search_module(NetLinkRoute libnl-route-3.0 QUIET) pkg_search_module(Gst gstreamer-1.0 QUIET) pkg_search_module(GstApp gstreamer-app-1.0 QUIET) +pkg_search_module(LibUsb libusb-1.0 QUIET) +pkg_search_module(LibUdev libudev QUIET) diff --git a/config/esw.yaml b/config/esw.yaml index 43a539063..14bbfb5ad 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -52,7 +52,7 @@ can: id: 0x15 - name: "joint_a" bus: 1 - id: 0x20 + id: 0x20 - name: "joint_b" bus: 1 id: 0x21 @@ -73,10 +73,10 @@ can: id: 0x26 - name: "mast_gimbal_y" bus: 3 - id: 0x28 + id: 0x27 - name: "mast_gimbal_z" bus: 3 - id: 0x27 + id: 0x28 - name: "sa_x" bus: 1 id: 0x29 @@ -131,7 +131,7 @@ brushless_motors: max_velocity: 20.0 max_torque: 5.0 joint_a: - # 1236.8475 rad/meter, or 196.850393476 rev/meter, and we want a max vel of 40 meter/s. + # 1236.8475 rad/meter, or 196.850393476 rev/meter, and we want a max vel of 40 meter/s. # gear ratio is currently at 0.005080 revolutions = 1 meter min_velocity: -0.05 # this means -0.10 meters per second. max_velocity: 0.05 @@ -153,10 +153,11 @@ brushless_motors: max_backward_pos: 0.0 max_torque: 20.0 joint_c: - min_velocity: -0.04 # in terms of output - max_velocity: 0.04 # max output shaft speed: 5 rpm (for now) + min_velocity: -0.03 # in terms of output + max_velocity: 0.03 # max output shaft speed: 5 rpm (for now) min_position: -0.125 max_position: 0.30 # 220 degrees of motion is the entire range + max_torque: 200.0 joint_de_0: min_velocity: -5.0 max_velocity: 5.0 @@ -603,4 +604,4 @@ auton_led_driver: baud: 115200 science: - shutoff_temp: 50.0f \ No newline at end of file + shutoff_temp: 50.0f diff --git a/config/moteus/joint_c.cfg b/config/moteus/joint_c.cfg index c475f8708..761535fc1 100644 --- a/config/moteus/joint_c.cfg +++ b/config/moteus/joint_c.cfg @@ -1363,9 +1363,9 @@ servo.adc_cur_cycles 2 servo.adc_aux_cycles 47 servo.pid_dq.kp 0.060606 servo.pid_dq.ki 102.321388 -servo.pid_position.kp 14.000000 +servo.pid_position.kp 800.000000 servo.pid_position.ki 0.000000 -servo.pid_position.kd 1.450000 +servo.pid_position.kd 50.000000 servo.pid_position.iratelimit -1.000000 servo.pid_position.ilimit 0.000000 servo.pid_position.max_desired_rate 0.000000 diff --git a/config/teleop.yaml b/config/teleop.yaml index 8c05ad09a..1b8c68952 100644 --- a/config/teleop.yaml +++ b/config/teleop.yaml @@ -6,34 +6,24 @@ teleop: joint_a: multiplier: -1 slow_mode_multiplier: 0.5 - invert: True - xbox_index: 0 joint_b: multiplier: 1 slow_mode_multiplier: 1 - invert: False - xbox_index: 1 joint_c: - multiplier: -1 + multiplier: 1 slow_mode_multiplier: 0.6 - invert: False - xbox_index: 3 joint_de_pitch: - multiplier: 0.5 + multiplier: -1 slow_mode_multiplier: 1 - invert: True joint_de_roll: - multiplier: -0.5 + multiplier: 1 slow_mode_multiplier: 1 - invert: True allen_key: multiplier: 1 slow_mode_multiplier: 1 - invert: False gripper: multiplier: 1 slow_mode_multiplier: 1 - invert: True sa_controls: sa_x: @@ -67,21 +57,32 @@ teleop: enabled: true multiplier: 1 twist: - multiplier: 0.7 + multiplier: 0.6 + tilt: + multiplier: 1 xbox_mappings: - left_js_x: 0 - left_js_y: 1 - right_js_x: 2 - right_js_y: 3 - left_trigger: 6 - right_trigger: 7 + left_x: 0 + left_y: 1 + right_x: 2 + right_y: 3 a: 0 b: 1 x: 2 y: 3 left_bumper: 4 right_bumper: 5 + left_trigger: 6 + right_trigger: 7 + back: 8 + forward: 9 + left_stick_click: 10 + right_stick_click: 11 + dpad_up: 12 + dpad_down: 13 + dpad_left: 14 + dpad_right: 15 + home: 16 joystick_mappings: left_right: 0 @@ -119,6 +120,6 @@ teleop: bar: ["/tf_static", "/rosout", "/tf"] ik_multipliers: - x: 1 - y: 1 - z: 1 \ No newline at end of file + x: 0.1 + y: 0.1 + z: 0.1 \ No newline at end of file diff --git a/launch/basestation.launch b/launch/basestation.launch index b5039bcbe..47f7dd691 100644 --- a/launch/basestation.launch +++ b/launch/basestation.launch @@ -4,14 +4,17 @@ + + + - - - + + + diff --git a/launch/esw_base.launch b/launch/esw_base.launch index ca02aaf98..a9452a46b 100644 --- a/launch/esw_base.launch +++ b/launch/esw_base.launch @@ -1,77 +1,73 @@ - + - - - - - - - + + + + + + + - + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - + + + + + + - + - - - - - + + + + + - + diff --git a/launch/simulator.launch b/launch/simulator.launch index 23230736f..6c3e96e33 100644 --- a/launch/simulator.launch +++ b/launch/simulator.launch @@ -17,6 +17,11 @@ + + + + diff --git a/scripts/debug_arm_ik.py b/scripts/debug_arm_ik.py index 4757343cc..8fb2e95e4 100755 --- a/scripts/debug_arm_ik.py +++ b/scripts/debug_arm_ik.py @@ -1,7 +1,8 @@ #!/usr/bin/env python3 import rospy -from geometry_msgs.msg import Pose, Quaternion, Point, PointStamped +from geometry_msgs.msg import Pose, PoseStamped, Quaternion, Point, PointStamped +from std_msgs.msg import Header from mrover.msg import IK if __name__ == "__main__": @@ -13,18 +14,24 @@ pub.publish( IK( - pose=Pose( - position=Point(x=0.5, y=0.5, z=0.5), - orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + target=PoseStamped( + header=Header(stamp=rospy.Time.now(), frame_id="base_link"), + pose=Pose( + position=Point(x=0.8, y=1.0, z=0.5), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) ) ) def on_clicked_point(clicked_point: PointStamped): ik = IK( - pose=Pose( - position=clicked_point.point, - orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + target=PoseStamped( + header=Header(stamp=rospy.Time.now(), frame_id="base_link"), + pose=Pose( + position=clicked_point.point, + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) ) pub.publish(ik) diff --git a/setup.py b/setup.py index 48ba751d1..e5807a321 100644 --- a/setup.py +++ b/setup.py @@ -13,6 +13,7 @@ "navigation.failure_identification", "esw", "teleoperation.teleoperation", + "teleoperation.arm_controller", ], package_dir={"": "src"}, ) diff --git a/src/esw/can_driver/can_driver.cpp b/src/esw/can_driver/can_driver.cpp index 7b9aa81c2..6355105f8 100644 --- a/src/esw/can_driver/can_driver.cpp +++ b/src/esw/can_driver/can_driver.cpp @@ -69,7 +69,7 @@ namespace mrover { } } else { NODELET_WARN("No CAN devices specified or config was invalid. Did you forget to load the correct ROS parameters?"); - NODELET_WARN("For example before testing the devboard run: \"rosparam load config/esw_devboard.yml\""); + NODELET_WARN("For example before testing the devboard run: \"rosparam load config/esw.yml\""); } mCanNetLink = CanNetLink{mInterface}; @@ -201,7 +201,7 @@ namespace mrover { } catch (boost::system::system_error const& error) { // check if ran out of buffer space if (error.code() == boost::asio::error::no_buffer_space) { - NODELET_WARN_STREAM_THROTTLE(1, "No buffer space available to send CAN message"); + NODELET_WARN_STREAM_THROTTLE(1, "No buffer space available to send CAN message. This usually indicates an electrical problem with the bus. CAN will avoid sending out messages if it can not see other devices."); return; } ros::shutdown(); diff --git a/src/esw/drive_bridge/main.cpp b/src/esw/drive_bridge/main.cpp index 9e836e462..afa2092ec 100644 --- a/src/esw/drive_bridge/main.cpp +++ b/src/esw/drive_bridge/main.cpp @@ -43,7 +43,8 @@ auto main(int argc, char** argv) -> int { } void moveDrive(geometry_msgs::Twist::ConstPtr const& msg) { - // See 11.5.1 in "Controls Engineering in the FIRST Robotics Competition" for the math + // See 13.3.1.4 in "Modern Robotics" for the math + // Link: https://hades.mech.northwestern.edu/images/7/7f/MR.pdf auto forward = MetersPerSecond{msg->linear.x}; auto turn = RadiansPerSecond{msg->angular.z}; // TODO(quintin) Don't ask me to explain perfectly why we need to cancel out a meters unit in the numerator diff --git a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp index a1430549b..1033a1cc6 100644 --- a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp +++ b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp @@ -1,13 +1,9 @@ #include "gst_websocket_streamer.hpp" -#if defined(MROVER_IS_JETSON) -constexpr bool IS_JETSON = true; -#else -constexpr bool IS_JETSON = false; -#endif - namespace mrover { + using namespace std::string_view_literals; + template auto gstCheck(T* t) -> T* { if (!t) throw std::runtime_error{"Failed to create"}; @@ -26,7 +22,13 @@ namespace mrover { GstBuffer* buffer = gst_sample_get_buffer(sample); GstMapInfo map; gst_buffer_map(buffer, &map, GST_MAP_READ); - mStreamServer->broadcast({reinterpret_cast(map.data), map.size}); + + // Prefix the encoded chunk with metadata for the decoder on the browser side + std::vector chunk(sizeof(ChunkHeader) + map.size); + std::memcpy(chunk.data(), &mChunkHeader, sizeof(ChunkHeader)); + std::memcpy(chunk.data() + sizeof(ChunkHeader), map.data, map.size); + + mStreamServer->broadcast(chunk); gst_buffer_unmap(buffer, &map); gst_sample_unref(sample); @@ -38,70 +40,68 @@ namespace mrover { mMainLoop = gstCheck(g_main_loop_new(nullptr, FALSE)); + // Source std::string launch; - if (mCaptureDevice.empty()) { - if constexpr (IS_JETSON) { - // ReSharper disable once CppDFAUnreachableCode - launch = std::format( - // App source is pushed to when we get a ROS BGRA image message - // is-live prevents frames from being pushed when the pipeline is in READY - "appsrc name=imageSource is-live=true " - "! video/x-raw,format=BGRA,width={},height={},framerate=30/1 " - "! videoconvert " // Convert BGRA => I420 (YUV) for the encoder, note we are still on the CPU - "! video/x-raw,format=I420 " - "! nvvidconv " // Upload to GPU memory for the encoder - "! video/x-raw(memory:NVMM),format=I420 " - "! nvv4l2h265enc name=encoder bitrate={} iframeinterval=300 vbv-size=33333 insert-sps-pps=true control-rate=constant_bitrate profile=Main num-B-Frames=0 ratecontrol-enable=true preset-level=UltraFastPreset EnableTwopassCBR=false maxperf-enable=true " - // App sink is pulled from (getting H265 chunks) on another thread and sent to the stream server - // sync=false is needed to avoid weirdness, going from playing => ready => playing will not work otherwise - "! appsink name=streamSink sync=false", - mImageWidth, mImageHeight, mBitrate); - } else { - // ReSharper disable once CppDFAUnreachableCode - launch = std::format( - "appsrc name=imageSource is-live=true " - "! video/x-raw,format=BGRA,width={},height={},framerate=30/1 " - "! nvh265enc name=encoder " - "! appsink name=streamSink sync=false", - mImageWidth, mImageHeight); - } + if (mDeviceNode.empty()) { + // App source is pushed to when we get a ROS BGRA image message + // is-live prevents frames from being pushed when the pipeline is in READY + launch += "appsrc name=imageSource is-live=true "; } else { - if constexpr (IS_JETSON) { - // ReSharper disable once CppDFAUnreachableCode - - // TODO(quintin): I had to apply this patch: https://forums.developer.nvidia.com/t/macrosilicon-usb/157777/4 - // nvv4l2camerasrc only supports UYUV by default, but our cameras are YUY2 (YUYV) - - launch = std::format( + launch += std::format("v4l2src device={} ", mDeviceNode); + } + // Source format + std::string captureFormat = mDeviceNode.empty() ? "video/x-raw,format=BGRA" : mDecodeJpegFromDevice ? "image/jpeg" + : "video/x-raw,format=YUY2"; + launch += std::format("! {},width={},height={},framerate={}/1 ", + captureFormat, mImageWidth, mImageHeight, mImageFramerate); + // Source decoder and H265 encoder + if (gst_element_factory_find("nvv4l2h265enc")) { + // Most likely on the Jetson + if (captureFormat.contains("jpeg")) { + // Mostly used with USB cameras, MPEG capture uses way less USB bandwidth + launch += + // TODO(quintin): I had to apply this patch: https://forums.developer.nvidia.com/t/macrosilicon-usb/157777/4 + // nvv4l2camerasrc only supports UYUV by default, but our cameras are YUY2 (YUYV) // "nvv4l2camerasrc device={} " - - "v4l2src device={} " - // "! video/x-raw(memory:NVMM),format=YUY2,width={},height={},framerate={}/1 " - "! image/jpeg,width={},height={},framerate={}/1 " - "! nvv4l2decoder mjpeg=1 " - - "! nvvidconv " - "! video/x-raw(memory:NVMM),format=NV12 " - "! nvv4l2h265enc name=encoder bitrate={} iframeinterval=300 vbv-size=33333 insert-sps-pps=true control-rate=constant_bitrate profile=Main num-B-Frames=0 ratecontrol-enable=true preset-level=UltraFastPreset EnableTwopassCBR=false maxperf-enable=true " - "! appsink name=streamSink sync=false", - mCaptureDevice, mImageWidth, mImageHeight, mImageFramerate, mBitrate); + + "! nvv4l2decoder mjpeg=1 " // Hardware-accelerated JPEG decoding, output is apparently some unknown proprietary format + "! nvvidconv " // Convert from proprietary format to NV12 so the encoder understands it + "! video/x-raw(memory:NVMM),format=NV12 "; } else { - // ReSharper disable once CppDFAUnreachableCode - launch = std::format( - "v4l2src device={} " - "! video/x-raw,format=YUY2,width={},height={},framerate=30/1 " - "! videoconvert " - "! nvh265enc name=encoder " - "! appsink name=streamSink sync=false", - mCaptureDevice, mImageWidth, mImageHeight); + launch += "! videoconvert " // Convert to I420 for the encoder, note we are still on the CPU + "! video/x-raw,format=I420 " + "! nvvidconv " // Upload to GPU memory for the encoder + "! video/x-raw(memory:NVMM),format=I420 "; } + launch += std::format("! nvv4l2h265enc name=encoder bitrate={} iframeinterval=300 vbv-size=33333 insert-sps-pps=true control-rate=constant_bitrate profile=Main num-B-Frames=0 ratecontrol-enable=true preset-level=UltraFastPreset EnableTwopassCBR=false maxperf-enable=true ", + mBitrate); + mChunkHeader.codec = ChunkHeader::Codec::H265; + } else if (gst_element_factory_find("nvh265enc")) { + // For desktop/laptops with the custom NVIDIA bad gstreamer plugins built (a massive pain to do!) + if (captureFormat.contains("jpeg")) + launch += "! jpegdec "; + else + launch += "! videoconvert "; + launch += "! nvh265enc name=encoder "; + mChunkHeader.codec = ChunkHeader::Codec::H265; + } else { + // For desktop/laptops with no hardware encoder + if (captureFormat.contains("jpeg")) + launch += "! jpegdec "; + else + launch += "! videoconvert "; + launch += std::format("! x264enc tune=zerolatency bitrate={} name=encoder ", mBitrate); + mChunkHeader.codec = ChunkHeader::Codec::H264; } + // App sink is pulled from (getting H265 chunks) on another thread and sent to the stream server + // sync=false is needed to avoid weirdness, going from playing => ready => playing will not work otherwise + launch += "! appsink name=streamSink sync=false"; ROS_INFO_STREAM(std::format("GStreamer launch: {}", launch)); mPipeline = gstCheck(gst_parse_launch(launch.c_str(), nullptr)); - if (mCaptureDevice.empty()) mImageSource = gstCheck(gst_bin_get_by_name(GST_BIN(mPipeline), "imageSource")); + if (mDeviceNode.empty()) mImageSource = gstCheck(gst_bin_get_by_name(GST_BIN(mPipeline), "imageSource")); mStreamSink = gstCheck(gst_bin_get_by_name(GST_BIN(mPipeline), "streamSink")); if (gst_element_set_state(mPipeline, GST_STATE_PAUSED) == GST_STATE_CHANGE_FAILURE) @@ -127,14 +127,22 @@ namespace mrover { if (mStreamServer->clientCount() == 0) return; if (msg->encoding != sensor_msgs::image_encodings::BGRA8) throw std::runtime_error{"Unsupported encoding"}; - if (msg->width != mImageWidth || msg->height != mImageHeight) throw std::runtime_error{"Unsupported resolution"}; + + cv::Size receivedSize{static_cast(msg->width), static_cast(msg->height)}; + cv::Mat bgraFrame{receivedSize, CV_8UC4, const_cast(msg->data.data()), msg->step}; + + if (cv::Size targetSize{static_cast(mImageWidth), static_cast(mImageHeight)}; + receivedSize != targetSize) { + ROS_WARN_ONCE("Image size does not match pipeline app source size, will resize"); + resize(bgraFrame, bgraFrame, targetSize); + } // "step" is the number of bytes (NOT pixels) in an image row - std::size_t size = msg->step * msg->height; + std::size_t size = bgraFrame.step * bgraFrame.rows; GstBuffer* buffer = gstCheck(gst_buffer_new_allocate(nullptr, size, nullptr)); GstMapInfo info; gst_buffer_map(buffer, &info, GST_MAP_WRITE); - std::memcpy(info.data, msg->data.data(), size); + std::memcpy(info.data, bgraFrame.data, size); gst_buffer_unmap(buffer, &info); gst_app_src_push_buffer(GST_APP_SRC(mImageSource), buffer); @@ -145,12 +153,56 @@ namespace mrover { } } + auto widthAndHeightToResolution(std::uint32_t width, std::uint32_t height) -> ChunkHeader::Resolution { + if (width == 640 && height == 480) return ChunkHeader::Resolution::EGA; + if (width == 1280 && height == 720) return ChunkHeader::Resolution::HD; + if (width == 1920 && height == 1080) return ChunkHeader::Resolution::FHD; + throw std::runtime_error{"Unsupported resolution"}; + } + + auto findDeviceNode(std::string_view devicePath) -> std::string { + udev* udevContext = udev_new(); + if (!udevContext) throw std::runtime_error{"Failed to initialize udev"}; + + udev_enumerate* enumerate = udev_enumerate_new(udevContext); + if (!enumerate) throw std::runtime_error{"Failed to create udev enumeration"}; + + udev_enumerate_add_match_subsystem(enumerate, "video4linux"); + udev_enumerate_scan_devices(enumerate); + + udev_list_entry* devices = udev_enumerate_get_list_entry(enumerate); + if (!devices) throw std::runtime_error{"Failed to get udev device list"}; + + udev_device* device{}; + udev_list_entry* entry; + udev_list_entry_foreach(entry, devices) { + device = udev_device_new_from_syspath(udevContext, udev_list_entry_get_name(entry)); + + if (udev_device_get_devpath(device) != devicePath) continue; + + if (!device) throw std::runtime_error{"Failed to get udev device"}; + + break; + } + if (!device) throw std::runtime_error{"Failed to find udev device"}; + + std::string deviceNode = udev_device_get_devnode(device); + + udev_device_unref(device); + udev_enumerate_unref(enumerate); + udev_unref(udevContext); + + return deviceNode; + } + auto GstWebsocketStreamerNodelet::onInit() -> void { try { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mCaptureDevice = mPnh.param("device", ""); + mDeviceNode = mPnh.param("dev_node", ""); + mDevicePath = mPnh.param("dev_path", ""); + mDecodeJpegFromDevice = mPnh.param("decode_jpeg_from_device", true); mImageTopic = mPnh.param("image_topic", "image"); mImageWidth = mPnh.param("width", 640); mImageHeight = mPnh.param("height", 480); @@ -159,6 +211,10 @@ namespace mrover { auto port = mPnh.param("port", 8081); mBitrate = mPnh.param("bitrate", 2000000); + mChunkHeader.resolution = widthAndHeightToResolution(mImageWidth, mImageHeight); + + if (!mDevicePath.empty()) mDeviceNode = findDeviceNode(mDevicePath); + gst_init(nullptr, nullptr); initPipeline(); @@ -193,12 +249,12 @@ namespace mrover { ROS_INFO("Stopped GStreamer pipeline"); }); - if (mCaptureDevice.empty()) { + if (mDeviceNode.empty()) { mImageSubscriber = mNh.subscribe(mImageTopic, 1, &GstWebsocketStreamerNodelet::imageCallback, this); } } catch (std::exception const& e) { - ROS_ERROR_STREAM(std::format("Exception initializing NVIDIA GST H265 Encoder: {}", e.what())); + ROS_ERROR_STREAM(std::format("Exception initializing gstreamer websocket streamer: {}", e.what())); ros::requestShutdown(); } } diff --git a/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp b/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp index c4950f62c..69520febe 100644 --- a/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp +++ b/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp @@ -2,14 +2,42 @@ #include "pch.hpp" +// Uses gstreamer to encode and stream video over a websocket +// The input can be a ROS BGRA image topic or a USB device +// Hardware accelerated is used when possible (with the Jetson or NVIDIA GPUs) +// Run "export GST_DEBUG=2" to debug gstreamer issues + namespace mrover { + struct ChunkHeader { + enum struct Resolution : std::uint8_t { + EGA, // 640x480 + HD, // 1280x720 + FHD, // 1920x1080 + } resolution; + enum struct Codec : std::uint8_t { + H265, + H264, + } codec; + }; + class GstWebsocketStreamerNodelet final : public nodelet::Nodelet { ros::NodeHandle mNh, mPnh; - std::string mCaptureDevice; + // For example, /dev/video0 + // These device paths are not garunteed to stay the same between reboots + // Prefer sys path for non-debugging purposes + std::string mDeviceNode; + bool mDecodeJpegFromDevice{}; // Uses less USB hub bandwidth, which is limited since we are using 2.0 std::string mImageTopic; + // To find the sys path: + // 1) Disconnect all cameras + // 2) Confirm there are no /dev/video* devices + // 2) Connect the camera you want to use + // 3) Run "ls /dev/video*" to verify the device is connected + // 4) Run "udevadm info -q path -n /dev/video0" to get the sys path + std::string mDevicePath; std::uint64_t mBitrate{}; std::uint32_t mImageWidth{}, mImageHeight{}, mImageFramerate{}; @@ -22,6 +50,8 @@ namespace mrover { std::thread mMainLoopThread; std::thread mStreamSinkThread; + ChunkHeader mChunkHeader{}; + auto onInit() -> void override; auto pullStreamSamplesLoop() -> void; diff --git a/src/esw/gst_websocket_streamer/pch.hpp b/src/esw/gst_websocket_streamer/pch.hpp index 2c224a3ec..2769438ad 100644 --- a/src/esw/gst_websocket_streamer/pch.hpp +++ b/src/esw/gst_websocket_streamer/pch.hpp @@ -1,20 +1,26 @@ #pragma once -#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/preload/bits/allocator.h b/src/preload/bits/allocator.h index 815b7f850..c33fb23b3 100644 --- a/src/preload/bits/allocator.h +++ b/src/preload/bits/allocator.h @@ -6,9 +6,7 @@ // C++ should have never broke this backwards compatibility! // How silly! -#if defined(__linux__) && defined(__GNUC__) - -#if (__GNUC__ == 13) +#if defined(__linux__) && defined(__GLIBCXX__) && !defined(__CUDACC__) // Allocators -*- C++ -*- @@ -292,5 +290,3 @@ namespace std _GLIBCXX_VISIBILITY(default) { #include_next #endif - -#endif diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index 499e0acb9..0fbc4639a 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -1,4 +1,5 @@ #include "simulator.hpp" +#include namespace mrover { @@ -23,7 +24,7 @@ namespace mrover { mMotorStatusPub = mNh.advertise("/drive_status", 1); mDriveControllerStatePub = mNh.advertise("/drive_controller_data", 1); mArmControllerStatePub = mNh.advertise("/arm_controller_data", 1); - mArmJointStatePub = mNh.advertise("/arm_joint_states", 1); + mArmJointStatePub = mNh.advertise("/arm_joint_data", 1); mIkTargetPub = mNh.advertise("/arm_ik", 1); diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 3e12b9485..ddd4f0f86 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -4,6 +4,7 @@ #include "glfw_pointer.hpp" #include "wgpu_objects.hpp" +#include using namespace std::literals; diff --git a/src/simulator/simulator.render.cpp b/src/simulator/simulator.render.cpp index ce2bf2247..9f725cc11 100644 --- a/src/simulator/simulator.render.cpp +++ b/src/simulator/simulator.render.cpp @@ -208,6 +208,19 @@ namespace mrover { mWindow = GlfwPointer{w, h, WINDOW_NAME, nullptr, nullptr}; NODELET_INFO_STREAM(std::format("Created window of size: {}x{}", w, h)); + if (cv::Mat logo = imread(std::filesystem::path{std::source_location::current().file_name()}.parent_path() / "mrover_logo.png", cv::IMREAD_UNCHANGED); + logo.type() == CV_8UC4) { + cvtColor(logo, logo, cv::COLOR_BGRA2RGBA); + GLFWimage logoImage{ + .width = logo.cols, + .height = logo.rows, + .pixels = logo.data, + }; + glfwSetWindowIcon(mWindow.get(), 1, &logoImage); + } else { + ROS_WARN_STREAM("Failed to load logo image"); + } + if (glfwRawMouseMotionSupported()) glfwSetInputMode(mWindow.get(), GLFW_RAW_MOUSE_MOTION, GLFW_TRUE); glfwSetWindowUserPointer(mWindow.get(), this); diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index caf0bbf53..a6b42f703 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -1,4 +1,5 @@ #include "simulator.hpp" +#include namespace mrover { @@ -249,11 +250,19 @@ namespace mrover { mDriveControllerStatePub.publish(driveControllerState); ControllerState armControllerState; + sensor_msgs::JointState armJointState; + std::vector zeros = {0, 0, 0, 0, 0, 0}; + armJointState.header.stamp = ros::Time::now(); for (auto& linkName: {"arm_a_link", "arm_b_link", "arm_c_link", "arm_d_link", "arm_e_link"}) { armControllerState.name.emplace_back(armMsgToUrdf.backward(linkName).value()); armControllerState.state.emplace_back("Armed"); armControllerState.error.emplace_back("None"); + armJointState.name.emplace_back(armMsgToUrdf.backward(linkName).value()); + armJointState.position.emplace_back(rover.physics->getJointPos(rover.linkNameToMeta.at(linkName).index)); + armJointState.velocity.emplace_back(rover.physics->getJointVel(rover.linkNameToMeta.at(linkName).index)); + armJointState.effort.emplace_back(rover.physics->getJointTorque(rover.linkNameToMeta.at(linkName).index)); + std::uint8_t limitSwitches = 0b000; if (auto limits = rover.model.getLink(linkName)->parent_joint->limits) { double joinPosition = rover.physics->getJointPos(rover.linkNameToMeta.at(linkName).index); @@ -264,16 +273,7 @@ namespace mrover { armControllerState.limit_hit.push_back(limitSwitches); } mArmControllerStatePub.publish(armControllerState); - - sensor_msgs::JointState jointState; - jointState.header.stamp = ros::Time::now(); - for (auto& linkName: {"arm_a_link", "arm_b_link", "arm_c_link", "arm_d_link", "arm_e_link"}) { - jointState.name.push_back(armMsgToUrdf.backward(linkName).value()); - jointState.position.push_back(rover.physics->getJointPos(rover.linkNameToMeta.at(linkName).index)); - jointState.velocity.push_back(rover.physics->getJointVel(rover.linkNameToMeta.at(linkName).index)); - jointState.effort.push_back(rover.physics->getJointTorque(rover.linkNameToMeta.at(linkName).index)); - } - mArmJointStatePub.publish(jointState); + mArmJointStatePub.publish(armJointState); } } diff --git a/src/teleoperation/arm_automation/__init__.py b/src/teleoperation/arm_automation/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/teleoperation/arm_automation/arm_automation.py b/src/teleoperation/arm_automation/arm_automation.py new file mode 100755 index 000000000..4ce34b92a --- /dev/null +++ b/src/teleoperation/arm_automation/arm_automation.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 + +import rospy + +import actionlib + +from mrover.msg import ArmActionAction, ArmActionGoal, ArmActionResult, Position + +from sensor_msgs.msg import JointState + +import numpy as np + + +def arm_automation() -> None: + rospy.init_node("arm_automation") + + server = None + joint_state = None + + pos_pub = rospy.Publisher("arm_position_cmd", Position, queue_size=1) + + def joint_state_callback(msg: JointState): + nonlocal joint_state + joint_state = msg + + rospy.Subscriber("arm_joint_data", JointState, joint_state_callback) + + def execute_callback(goal: ArmActionGoal): + if joint_state is None: + rospy.logerr("No joint state data available") + server.set_aborted(ArmActionResult()) + return + + if goal.name == "de_home": + target_names = ["joint_de_pitch", "joint_de_roll"] + target_positions = [joint_state.position[joint_state.name.index("joint_de_pitch")], np.pi / 8] + rospy.loginfo(f"Moving to {target_positions} for {target_names}") + else: + rospy.logerr("Invalid goal name") + server.set_aborted(ArmActionResult()) + return + + rate = rospy.Rate(20) + while not rospy.is_shutdown(): + if server.is_preempt_requested(): + server.set_preempted() + rospy.loginfo("Preempted") + server.set_aborted(ArmActionResult()) + return + + pos_pub.publish(Position(names=target_names, positions=target_positions)) + + feedback = [ + joint_state.position[joint_state.name.index("joint_de_pitch")], + joint_state.position[joint_state.name.index("joint_de_roll")], + ] + + if np.allclose(target_positions, feedback, atol=0.1): + rospy.loginfo("Reached target") + break + + rate.sleep() + + server.set_succeeded(ArmActionResult()) + + rospy.sleep(1) + + server = actionlib.SimpleActionServer("arm_action", ArmActionAction, execute_cb=execute_callback, auto_start=False) + server.start() + + +if __name__ == "__main__": + try: + arm_automation() + except rospy.ROSInterruptException: + pass diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index b0d7d23af..dbbe35771 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -16,19 +16,19 @@ namespace mrover { Position positions; positions.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"}; positions.positions.resize(positions.names.size()); - SE3d target_frame_to_arm_b_static; + SE3d targetFrameToArmBaseLink; try { - target_frame_to_arm_b_static = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "arm_base_link"); - } catch (...) { - ROS_WARN_THROTTLE(1, "Failed to resolve information about target frame"); + targetFrameToArmBaseLink = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "arm_base_link"); + } catch (tf2::TransformException const& exception) { + ROS_WARN_STREAM_THROTTLE(1, std::format("Failed to get transform from {} to arm_base_link: {}", ik_target.target.header.frame_id, exception.what())); return; } - Eigen::Vector4d target{ik_target.target.pose.position.x, ik_target.target.pose.position.y, ik_target.target.pose.position.z, 1}; - Eigen::Vector4d target_in_arm_b_static = target_frame_to_arm_b_static.transform() * target; - double x = target_in_arm_b_static.x() - END_EFFECTOR_LENGTH; // shift back by the length of the end effector - double z = target_in_arm_b_static.z(); - double y = target_in_arm_b_static.y(); - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", target_frame_to_arm_b_static); + SE3d endEffectorInTarget{{ik_target.target.pose.position.x, ik_target.target.pose.position.y, ik_target.target.pose.position.z}, SO3d::Identity()}; + SE3d endEffectorInArmBaseLink = targetFrameToArmBaseLink * endEffectorInTarget; + double x = endEffectorInArmBaseLink.translation().x() - END_EFFECTOR_LENGTH; // shift back by the length of the end effector + double y = endEffectorInArmBaseLink.translation().y(); + double z = endEffectorInArmBaseLink.translation().z(); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", endEffectorInArmBaseLink); double gamma = 0; double x3 = x - LINK_DE * std::cos(gamma); diff --git a/src/teleoperation/arm_controller/pch.hpp b/src/teleoperation/arm_controller/pch.hpp index 2f68bad08..104f5832c 100644 --- a/src/teleoperation/arm_controller/pch.hpp +++ b/src/teleoperation/arm_controller/pch.hpp @@ -2,6 +2,7 @@ #include #include +#include #include diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 346015fae..cf6b24347 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -3,7 +3,7 @@ import os import threading from datetime import datetime -from math import copysign, pi +from math import copysign, pi, isfinite import pytz from channels.generic.websocket import JsonWebsocketConsumer @@ -11,10 +11,14 @@ import rospy import tf2_ros from backend.models import AutonWaypoint, BasicWaypoint -from geometry_msgs.msg import Twist, Pose, Point, Quaternion, Vector3 +from geometry_msgs.msg import Twist, Pose, Point, Quaternion, Vector3, PoseStamped + +import actionlib # from cv_bridge import CvBridge from mrover.msg import ( + ArmActionAction, + ArmActionGoal, PDLB, ControllerState, GPSWaypoint, @@ -33,8 +37,8 @@ HeaterData, ) from mrover.srv import EnableAuton, AdjustMotor, ChangeCameras, CapturePanorama -from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity -from std_msgs.msg import String +from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity, JointState +from std_msgs.msg import String, Header from std_srvs.srv import SetBool, Trigger from tf.transformations import euler_from_quaternion from util.SE3 import SE3 @@ -98,6 +102,8 @@ def connect(self): self.arm_moteus_sub = rospy.Subscriber( "/arm_controller_data", ControllerState, self.arm_controller_callback ) + self.arm_joint_sub = rospy.Subscriber("/arm_joint_data", JointState, self.arm_joint_callback) + self.sa_joint_sub = rospy.Subscriber("/sa_joint_data", JointState, self.sa_joint_callback) self.drive_moteus_sub = rospy.Subscriber( "/drive_controller_data", ControllerState, self.drive_controller_callback ) @@ -130,27 +136,11 @@ def connect(self): self.capture_panorama_srv = rospy.ServiceProxy("capture_panorama", CapturePanorama) self.heater_auto_shutoff_srv = rospy.ServiceProxy("science_change_heater_auto_shutoff_state", SetBool) - # ROS Parameters - self.mappings = rospy.get_param("teleop/joystick_mappings") - self.drive_config = rospy.get_param("teleop/drive_controls") - self.max_wheel_speed = rospy.get_param("rover/max_speed") - self.wheel_radius = rospy.get_param("wheel/radius") - self.rover_width = rospy.get_param("rover/width") - self.max_angular_speed = self.max_wheel_speed / (self.rover_width / 2) - self.ra_config = rospy.get_param("teleop/ra_controls") - self.ik_names = rospy.get_param("teleop/ik_multipliers") - self.RA_NAMES = rospy.get_param("teleop/ra_names") - self.brushless_motors = rospy.get_param("brushless_motors/controllers") - self.brushed_motors = rospy.get_param("brushed_motors/controllers") - self.xbox_mappings = rospy.get_param("teleop/xbox_mappings") - self.sa_config = rospy.get_param("teleop/sa_controls") - self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) self.flight_thread = threading.Thread(target=self.flight_attitude_listener) self.flight_thread.start() - - except rospy.ROSException as e: + except Exception as e: rospy.logerr(e) def disconnect(self, close_code): @@ -197,8 +187,8 @@ def receive(self, text_data): self.send_auton_command(message) elif message["type"] == "teleop_enabled": self.send_teleop_enabled(message) - elif message["type"] == "auton_tfclient": - self.auton_bearing() + elif message["type"] == "bearing": + self.bearing() elif message["type"] == "mast_gimbal": self.mast_gimbal(message) elif message["type"] == "max_streams": @@ -230,16 +220,18 @@ def receive(self, text_data): except Exception as e: rospy.logerr(e) + @staticmethod def filter_xbox_axis( - self, - value: int, + value: float, deadzone_threshold: float = DEFAULT_ARM_DEADZONE, quad_control: bool = False, ) -> float: - deadzoned_val = deadzone(value, deadzone_threshold) - return quadratic(deadzoned_val) if quad_control else deadzoned_val + value = deadzone(value, deadzone_threshold) + if quad_control: + value = quadratic(value) + return value - def filter_xbox_button(self, button_array: "List[int]", pos_button: str, neg_button: str) -> float: + def filter_xbox_button(self, button_array: list[float], pos_button: str, neg_button: str) -> float: """ Applies various filtering functions to an axis for controlling the arm :return: Return -1, 0, or 1 depending on what buttons are being pressed @@ -268,158 +260,156 @@ def to_velocity(self, input: int, joint_name: str, brushless: bool = True) -> fl / 2 ) - def handle_controls_message(self, msg): - CACHE = ["cache"] - SA_NAMES = ["sa_x", "sa_y", "sa_z", "sampler", "sensor_actuator"] - RA_NAMES = self.RA_NAMES - ra_slow_mode = False - raw_left_trigger = msg["buttons"][self.xbox_mappings["left_trigger"]] - left_trigger = raw_left_trigger if raw_left_trigger > 0 else 0 - raw_right_trigger = msg["buttons"][self.xbox_mappings["right_trigger"]] - right_trigger = raw_right_trigger if raw_right_trigger > 0 else 0 - arm_pubs = [self.arm_position_cmd_pub, self.arm_velocity_cmd_pub, self.arm_throttle_cmd_pub, self.arm_ik_pub] - sa_pubs = [self.sa_position_cmd_pub, self.sa_velocity_cmd_pub, self.sa_throttle_cmd_pub] - cache_pubs = [self.cache_position_cmd_pub, self.cache_velocity_cmd_pub, self.cache_throttle_cmd_pub] - publishers = [] - controls_names = [] - if msg["type"] == "cache_values": - controls_names = CACHE - publishers = cache_pubs - elif msg["type"] == "arm_values": - controls_names = RA_NAMES - publishers = arm_pubs - elif msg["type"] == "sa_arm_values": - controls_names = SA_NAMES - publishers = sa_pubs - - if msg["arm_mode"] == "ik": - ee_in_map = SE3.from_tf_tree(self.tf_buffer, "base_link", "arm_e_link") - - left_trigger = self.filter_xbox_axis(msg["axes"][self.xbox_mappings["left_trigger"]]) - if left_trigger < 0: - left_trigger = 0 - - right_trigger = self.filter_xbox_axis(msg["axes"][self.xbox_mappings["right_trigger"]]) - if right_trigger < 0: - right_trigger = 0 - ee_in_map.position[0] += ( - self.ik_names["x"] * self.filter_xbox_axis(msg["axes"][self.xbox_mappings["left_js_x"]]), - ) - ee_in_map.position[1] += ( - self.ik_names["y"] * self.filter_xbox_axis(msg["axes"][self.xbox_mappings["left_js_y"]]), + def publish_ik(self, axes: list[float], buttons: list[float]) -> None: + ee_in_map = SE3.from_tf_tree(self.tf_buffer, "base_link", "arm_d_link") + + ee_in_map.position[0] += self.ik_names["x"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_x"]]) + ee_in_map.position[1] += self.ik_names["y"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_y"]]) + ee_in_map.position[2] += self.ik_names["z"] * self.filter_xbox_button(buttons, "right_trigger", "left_trigger") + + self.send( + text_data=json.dumps( + { + "type": "ik", + "target": { + "position": ee_in_map.position.tolist(), + "quaternion": ee_in_map.rotation.quaternion.tolist(), + }, + } ) - ee_in_map.position[2] -= self.ik_names["z"] * left_trigger + self.ik_names["z"] * right_trigger + ) - arm_ik_cmd = IK( - pose=Pose( - position=Point(*ee_in_map.position), - orientation=Quaternion(*ee_in_map.rotation.quaternion), + self.arm_ik_pub.publish( + IK( + target=PoseStamped( + header=Header(stamp=rospy.Time.now(), frame_id="base_link"), + pose=Pose( + position=Point(*ee_in_map.position), + orientation=Quaternion(*ee_in_map.rotation.quaternion), + ), ) ) - publishers[3].publish(arm_ik_cmd) - - elif msg["arm_mode"] == "position": - position_names = controls_names - if msg["type"] == "arm_values": - position_names = ["joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"] - position_cmd = Position( - names=position_names, - positions=msg["positions"], - ) - publishers[0].publish(position_cmd) - - elif msg["arm_mode"] == "velocity": - velocity_cmd = Velocity() - velocity_cmd.names = controls_names - if msg["type"] == "cache_values": - velocity_cmd.velocities = [ - self.sa_config["cache"]["multiplier"] - * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper") - ] - elif msg["type"] == "sa_arm_values": - velocity_cmd.velocities = [ - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_x"]["xbox_index"]]), "sa_x", False - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_y"]["xbox_index"]]), "sa_y", False - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_z"]["xbox_index"]]), "sa_z", True - ), - self.sa_config["sampler"]["multiplier"] * (right_trigger - left_trigger), - self.sa_config["sensor_actuator"]["multiplier"] - * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper"), - ] - elif msg["type"] == "arm_values": - velocity_cmd.velocities = [ - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_a"]["xbox_index"]]), "joint_a" - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_b"]["xbox_index"]]), "joint_b", False - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_c"]["xbox_index"]]), "joint_c" - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_de_pitch"]["xbox_index"]]), "joint_de_0" - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_de_roll"]["xbox_index"]]), "joint_de_1" - ), - self.ra_config["allen_key"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "y", "a"), - self.ra_config["gripper"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "b", "x"), - ] - publishers[1].publish(velocity_cmd) - - elif msg["arm_mode"] == "throttle": - throttle_cmd = Throttle() - throttle_cmd.names = controls_names - if msg["type"] == "cache_values": - throttle_cmd.throttles = [ - self.sa_config["cache"]["multiplier"] - * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper") - ] - elif msg["type"] == "arm_values": - # d_pad_x = msg["axes"][self.xbox_mappings["d_pad_x"]] - # if d_pad_x > 0.5: - # ra_slow_mode = True - # elif d_pad_x < -0.5: - # ra_slow_mode = False - - throttle_cmd.throttles = [ - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_a"]["xbox_index"]]), - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_b"]["xbox_index"]]), - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_c"]["xbox_index"]]), - self.filter_xbox_button(msg["buttons"], "right_trigger", "left_trigger"), - self.filter_xbox_button(msg["buttons"], "left_bumper", "right_bumper"), - self.ra_config["allen_key"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "y", "a"), - self.ra_config["gripper"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "x", "b"), - ] - - for i, name in enumerate(self.RA_NAMES): - if ra_slow_mode: - throttle_cmd.throttles[i] *= self.ra_config[name]["slow_mode_multiplier"] - if self.ra_config[name]["invert"]: - throttle_cmd.throttles[i] *= -1 - elif msg["type"] == "sa_arm_values": - throttle_cmd.throttles = [ - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_x"]["xbox_index"]]), - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_y"]["xbox_index"]]), - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_z"]["xbox_index"]]), - self.sa_config["sampler"]["multiplier"] * (right_trigger - left_trigger), - self.sa_config["sensor_actuator"]["multiplier"] - * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper"), - ] - - fast_mode_activated = msg["buttons"][self.xbox_mappings["a"]] or msg["buttons"][self.xbox_mappings["b"]] - if not fast_mode_activated: - for i, name in enumerate(SA_NAMES): - # When going up (vel < 0) with SA joint 2, we DON'T want slow mode. - if not (name == "sa_z" and throttle_cmd.throttles[i] < 0): - throttle_cmd.throttles[i] *= self.sa_config[name]["slow_mode_multiplier"] - publishers[2].publish(throttle_cmd) + ) + + def publish_position(self, type, names, positions): + position_cmd = Position( + names=names, + positions=positions, + ) + if type == "arm_values": + self.arm_position_cmd_pub.publish(position_cmd) + elif type == "sa_arm_values": + self.sa_position_cmd_pub.publish(position_cmd) + elif type == "cache_values": + self.cache_position_cmd_pub.publish(position_cmd) + + def publish_velocity(self, type, names, axes, buttons): + left_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["left_trigger"]]) + right_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["right_trigger"]]) + velocity_cmd = Velocity() + velocity_cmd.names = names + if type == "cache_values": + velocity_cmd.velocities = [ + self.sa_config["cache"]["multiplier"] * self.filter_xbox_button(buttons, "right_bumper", "left_bumper") + ] + self.cache_velocity_cmd_pub.publish(velocity_cmd) + elif type == "sa_arm_values": + velocity_cmd.velocities = [ + self.to_velocity(self.filter_xbox_axis(axes[self.sa_config["sa_x"]["xbox_index"]]), "sa_x", False), + self.to_velocity(self.filter_xbox_axis(axes[self.sa_config["sa_y"]["xbox_index"]]), "sa_y", False), + self.to_velocity(self.filter_xbox_axis(axes[self.sa_config["sa_z"]["xbox_index"]]), "sa_z", True), + self.sa_config["sampler"]["multiplier"] * (right_trigger - left_trigger), + self.sa_config["sensor_actuator"]["multiplier"] + * self.filter_xbox_button(buttons, "right_bumper", "left_bumper"), + ] + self.sa_velocity_cmd_pub.publish(velocity_cmd) + elif type == "arm_values": + velocity_cmd.velocities = [ + self.to_velocity(self.filter_xbox_axis(axes[self.ra_config["joint_a"]["xbox_index"]]), "joint_a"), + self.to_velocity( + self.filter_xbox_axis(axes[self.ra_config["joint_b"]["xbox_index"]]), "joint_b", False + ), + self.to_velocity(self.filter_xbox_axis(axes[self.ra_config["joint_c"]["xbox_index"]]), "joint_c"), + self.to_velocity( + self.filter_xbox_axis(axes[self.ra_config["joint_de_pitch"]["xbox_index"]]), "joint_de_0" + ), + self.to_velocity( + self.filter_xbox_axis(axes[self.ra_config["joint_de_roll"]["xbox_index"]]), "joint_de_1" + ), + self.ra_config["allen_key"]["multiplier"] * self.filter_xbox_button(buttons, "y", "a"), + self.ra_config["gripper"]["multiplier"] * self.filter_xbox_button(buttons, "b", "x"), + ] + self.arm_velocity_cmd_pub.publish(velocity_cmd) + + def publish_throttle(self, type, names, axes, buttons): + left_trigger = self.filter_xbox_axis(buttons[self.xbox_mappings["left_trigger"]]) + right_trigger = self.filter_xbox_axis(buttons[self.xbox_mappings["right_trigger"]]) + throttle_cmd = Throttle() + throttle_cmd.names = names + if type == "cache_values": + throttle_cmd.throttles = [ + self.sa_config["cache"]["multiplier"] * self.filter_xbox_button(buttons, "right_bumper", "left_bumper") + ] + self.cache_throttle_cmd_pub.publish(throttle_cmd) + elif type == "arm_values": + throttle_cmd.throttles = [ + self.ra_config["joint_a"]["multiplier"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_x"]]), + self.ra_config["joint_b"]["multiplier"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_y"]]), + self.ra_config["joint_c"]["multiplier"] + * quadratic(self.filter_xbox_axis(axes[self.xbox_mappings["right_y"]])), + self.ra_config["joint_de_pitch"]["multiplier"] + * self.filter_xbox_axis( + buttons[self.xbox_mappings["right_trigger"]] - buttons[self.xbox_mappings["left_trigger"]] + ), + self.ra_config["joint_de_roll"]["multiplier"] + * self.filter_xbox_axis( + buttons[self.xbox_mappings["right_bumper"]] - buttons[self.xbox_mappings["left_bumper"]] + ), + self.ra_config["allen_key"]["multiplier"] * self.filter_xbox_button(buttons, "y", "a"), + self.ra_config["gripper"]["multiplier"] * self.filter_xbox_button(buttons, "b", "x"), + ] + self.arm_throttle_cmd_pub.publish(throttle_cmd) + elif type == "sa_arm_values": + throttle_cmd.throttles = [ + self.filter_xbox_axis(axes[self.sa_config["sa_x"]["xbox_index"]]), + self.filter_xbox_axis(axes[self.sa_config["sa_y"]["xbox_index"]]), + self.filter_xbox_axis(axes[self.sa_config["sa_z"]["xbox_index"]]), + self.sa_config["sampler"]["multiplier"] * (right_trigger - left_trigger), + self.sa_config["sensor_actuator"]["multiplier"] + * self.filter_xbox_button(buttons, "right_bumper", "left_bumper"), + ] + self.sa_throttle_cmd_pub.publish(throttle_cmd) + + def handle_controls_message(self, msg): + names = ["joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll", "allen_key", "gripper"] + if msg["type"] == "sa_arm_values": + names = ["sa_x", "sa_y", "sa_z", "sampler", "sensor_actuator"] + elif msg["type"] == "cache_values": + names = ["cache"] + + if msg["buttons"][self.xbox_mappings["home"]] > 0.5: + rospy.loginfo("Homing DE") + + client = actionlib.SimpleActionClient("arm_action", ArmActionAction) + if client.wait_for_server(timeout=rospy.Duration(1)): + goal = ArmActionGoal(name="de_home") + client.send_goal(goal) + + client.wait_for_result() + else: + rospy.logwarn("Arm action server not available") + else: + if msg["arm_mode"] == "ik": + self.publish_ik(msg["axes"], msg["buttons"]) + + elif msg["arm_mode"] == "position": + self.publish_position(type=msg["type"], names=names, positions=msg["positions"]) + + elif msg["arm_mode"] == "velocity": + self.publish_velocity(type=msg["type"], names=names, axes=msg["axes"], buttons=msg["buttons"]) + + elif msg["arm_mode"] == "throttle": + self.publish_throttle(type=msg["type"], names=names, axes=msg["axes"], buttons=msg["buttons"]) def handle_joystick_message(self, msg): # Tiny deadzone so we can safely e-stop with dampen switch @@ -446,6 +436,8 @@ def get_axes_input( # angular_from_lateral = get_axes_input("left_right", 0.4, True) angular = get_axes_input("twist", 0.03, True, self.max_angular_speed * dampen) + linear += get_axes_input("tilt", 0.5, scale=0.1) + self.twist_pub.publish( Twist( linear=Vector3(x=linear), @@ -483,6 +475,11 @@ def arm_controller_callback(self, msg): ) ) + def sa_joint_callback(self, msg): + names = msg.name + z = msg.position[names.index("sa_z")] + self.send(text_data=json.dumps({"type": "sa_z", "sa_z": z})) + def drive_controller_callback(self, msg): hits = [] for n in msg.limit_hit: @@ -657,12 +654,12 @@ def ish_thermistor_data_callback(self, msg): temps = [x.temperature for x in msg.temps] self.send(text_data=json.dumps({"type": "thermistor", "temps": temps})) - def auton_bearing(self): + def bearing(self): base_link_in_map = SE3.from_tf_tree(self.tf_buffer, "map", "base_link") self.send( text_data=json.dumps( { - "type": "auton_tfclient", + "type": "bearing", "rotation": base_link_in_map.rotation.quaternion.tolist(), } ) @@ -814,6 +811,11 @@ def flight_attitude_listener(self): rate.sleep() + def arm_joint_callback(self, msg: JointState) -> None: + # Set non-finite values to zero + msg.position = [x if isfinite(x) else 0 for x in msg.position] + self.send(text_data=json.dumps({"type": "fk", "positions": msg.position})) + def science_spectral_callback(self, msg): self.send( text_data=json.dumps({"type": "spectral_data", "site": msg.site, "data": msg.data, "error": msg.error}) diff --git a/src/teleoperation/frontend/index.html b/src/teleoperation/frontend/index.html index a88854489..e3f02c525 100644 --- a/src/teleoperation/frontend/index.html +++ b/src/teleoperation/frontend/index.html @@ -4,7 +4,7 @@ - Vite App + MRover
diff --git a/src/teleoperation/frontend/package.json b/src/teleoperation/frontend/package.json index 8638e8b32..76d04eba6 100644 --- a/src/teleoperation/frontend/package.json +++ b/src/teleoperation/frontend/package.json @@ -20,6 +20,7 @@ "html2canvas": "^1.4.1", "leaflet": "^1.9.4", "quaternion-to-euler": "^0.5.0", + "three": "^0.163.0", "vue": "^3.3.4", "vue-flight-indicators": "^0.1.1", "vue-router": "^4.2.4", diff --git a/src/teleoperation/frontend/public/meshes/arm_a.fbx b/src/teleoperation/frontend/public/meshes/arm_a.fbx new file mode 100644 index 000000000..8f7c71b41 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/arm_a.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d72481ca801642e73fea847317225514516d10667d777c0287021a90c77befba +size 869948 diff --git a/src/teleoperation/frontend/public/meshes/arm_b.fbx b/src/teleoperation/frontend/public/meshes/arm_b.fbx new file mode 100644 index 000000000..e9360724a --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/arm_b.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0578a7fe0557e7bc86e3949cc748275386e5a472d564367a00f722f0060d47c1 +size 1127196 diff --git a/src/teleoperation/frontend/public/meshes/arm_c.fbx b/src/teleoperation/frontend/public/meshes/arm_c.fbx new file mode 100644 index 000000000..5fbded9a7 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/arm_c.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e3bad4fb02574c6faf02cac83665d7a48972640301d0ff3089d1a7200f9b81ff +size 883884 diff --git a/src/teleoperation/frontend/public/meshes/arm_d.fbx b/src/teleoperation/frontend/public/meshes/arm_d.fbx new file mode 100644 index 000000000..be192a925 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/arm_d.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d270eeab807b552603478d239cbe1d5dc0cc374c11cfd8aff8d49b78340df9d3 +size 316844 diff --git a/src/teleoperation/frontend/public/meshes/arm_e.fbx b/src/teleoperation/frontend/public/meshes/arm_e.fbx new file mode 100644 index 000000000..5630b17d9 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/arm_e.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b42cb29f991ff0dd2752e613388cef21e9fa5030b50bb023d70f8f9f891a0bea +size 560812 diff --git a/src/teleoperation/frontend/public/meshes/bottle.fbx b/src/teleoperation/frontend/public/meshes/bottle.fbx new file mode 100644 index 000000000..25d7a8eed --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/bottle.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:df9dfe054c07005d820d7c81906136bd1bf621ebf4e4dcf3fbfb52f7e0fb4716 +size 17596 diff --git a/src/teleoperation/frontend/public/meshes/ground.fbx b/src/teleoperation/frontend/public/meshes/ground.fbx new file mode 100644 index 000000000..05b93f260 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/ground.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8323c262d0e44f0f1d128a3c68752ef95b70a9b4602de2ff06f43cef106d4b31 +size 43772 diff --git a/src/teleoperation/frontend/public/meshes/hammer.fbx b/src/teleoperation/frontend/public/meshes/hammer.fbx new file mode 100644 index 000000000..9f8b6b9e4 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/hammer.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4b5e36777f78ac597bcd49076a0cf2b674d886c315b50b5d1ff75c7bcea3123a +size 16620 diff --git a/src/teleoperation/frontend/public/meshes/primitives/cube.fbx b/src/teleoperation/frontend/public/meshes/primitives/cube.fbx new file mode 100644 index 000000000..9326aa8ca --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/primitives/cube.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3fc4c443d8ae8ca4cca8d98e6aa37a811db6fc23ce4eb169abc3314625a5f27a +size 11756 diff --git a/src/teleoperation/frontend/public/meshes/primitives/cylinder.fbx b/src/teleoperation/frontend/public/meshes/primitives/cylinder.fbx new file mode 100644 index 000000000..02941d306 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/primitives/cylinder.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4678acc34ec93779fa178d326f321b8cc67976c7e8df754c88fea29b34ee0239 +size 12460 diff --git a/src/teleoperation/frontend/public/meshes/primitives/sphere.fbx b/src/teleoperation/frontend/public/meshes/primitives/sphere.fbx new file mode 100644 index 000000000..07b9cab65 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/primitives/sphere.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b5eb2df5136ad610d75ba102a17a9bee8fbb0cb253a45cc6e8bad1c527559c25 +size 13948 diff --git a/src/teleoperation/frontend/public/meshes/rock.fbx b/src/teleoperation/frontend/public/meshes/rock.fbx new file mode 100644 index 000000000..0bd101bbe --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rock.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8098184699873dfc775269d78e3a2d069344d89b69fe67d38d06a3dba6f92c4d +size 18940 diff --git a/src/teleoperation/frontend/public/meshes/rover_chassis.fbx b/src/teleoperation/frontend/public/meshes/rover_chassis.fbx new file mode 100644 index 000000000..9e4fe6a3d --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_chassis.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:497810bf42e67701d823becb224787bd04121c788a59303bfd6d485d1268fa01 +size 1966956 diff --git a/src/teleoperation/frontend/public/meshes/rover_left_bogie.fbx b/src/teleoperation/frontend/public/meshes/rover_left_bogie.fbx new file mode 100644 index 000000000..4d3a9e550 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_left_bogie.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ad295bd7c739fd5b8e4d05f02db3494db37253b4315c59b60e06942fb93e3a8f +size 1569308 diff --git a/src/teleoperation/frontend/public/meshes/rover_left_rocker.fbx b/src/teleoperation/frontend/public/meshes/rover_left_rocker.fbx new file mode 100644 index 000000000..dcfad7ae4 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_left_rocker.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:70a8a1a16c7223473ea44751b1a42d2f5e2cbfe41e312cdf1172ccfa0f8c8bcc +size 1062556 diff --git a/src/teleoperation/frontend/public/meshes/rover_left_wheel.fbx b/src/teleoperation/frontend/public/meshes/rover_left_wheel.fbx new file mode 100644 index 000000000..ab6089917 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_left_wheel.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:13129b734d971654de759255f9a6e65c2571bab60ad668a2c325f1a92b377612 +size 556396 diff --git a/src/teleoperation/frontend/public/meshes/rover_right_bogie.fbx b/src/teleoperation/frontend/public/meshes/rover_right_bogie.fbx new file mode 100644 index 000000000..93b721e5a --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_right_bogie.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:607783822c09899857fe0e556d6f8241130f3fbae96d12356ba4beb8f79bad06 +size 1567468 diff --git a/src/teleoperation/frontend/public/meshes/rover_right_rocker.fbx b/src/teleoperation/frontend/public/meshes/rover_right_rocker.fbx new file mode 100644 index 000000000..73f8b8f0e --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_right_rocker.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f90f0f99210eea2963f2ac4a257ff8f908f6906b82dc4bd57fa61f590786da24 +size 1062780 diff --git a/src/teleoperation/frontend/public/meshes/rover_right_wheel.fbx b/src/teleoperation/frontend/public/meshes/rover_right_wheel.fbx new file mode 100644 index 000000000..bb5022926 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_right_wheel.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:201aaec237d10542c6a97ee8afd7b7bca7c01b6463952872e22a3369b31fa3cb +size 550204 diff --git a/src/teleoperation/frontend/src/components/ArmControls.vue b/src/teleoperation/frontend/src/components/ArmControls.vue index 5a0bf132f..f9520a8b4 100644 --- a/src/teleoperation/frontend/src/components/ArmControls.vue +++ b/src/teleoperation/frontend/src/components/ArmControls.vue @@ -86,6 +86,7 @@ ]" /> + @@ -96,6 +97,7 @@ import ToggleButton from './ToggleButton.vue' import CalibrationCheckbox from './CalibrationCheckbox.vue' import MotorAdjust from './MotorAdjust.vue' import LimitSwitch from './LimitSwitch.vue' +import Rover3D from './Rover3D.vue' // In seconds const updateRate = 0.05 @@ -106,7 +108,8 @@ export default defineComponent({ ToggleButton, CalibrationCheckbox, MotorAdjust, - LimitSwitch + LimitSwitch, + Rover3D }, data() { return { @@ -137,6 +140,7 @@ export default defineComponent({ } }, positions: [], + send_positions: false // Only send after submit is clicked for the first time } }, @@ -152,11 +156,17 @@ export default defineComponent({ alert('Toggling Arm Laser failed.') } } + }, + arm_mode(newMode) { + if (newMode !== 'position') { + this.positions = [] + this.send_positions = false + } } }, - mounted: function() { - document.addEventListener('keydown', this.keyDown); + mounted: function () { + document.addEventListener('keydown', this.keyDown) }, beforeUnmount: function () { @@ -166,22 +176,26 @@ export default defineComponent({ created: function () { interval = window.setInterval(() => { - const gamepads = navigator.getGamepads() - for (let i = 0; i < 4; i++) { - const gamepad = gamepads[i] - if (gamepad) { - // Microsoft and Xbox for old Xbox 360 controllers - // X-Box for new PowerA Xbox One controllers - if ( - gamepad.id.includes('Microsoft') || - gamepad.id.includes('Xbox') || - gamepad.id.includes('X-Box') - ) { - let buttons = gamepad.buttons.map((button) => { - return button.value - }) + if (this.send_positions) { + this.publishJoystickMessage([], [], this.arm_mode, this.positions) + } else if (this.arm_mode !== "position") { + const gamepads = navigator.getGamepads() + for (let i = 0; i < 4; i++) { + const gamepad = gamepads[i] + if (gamepad) { + // Microsoft and Xbox for old Xbox 360 controllers + // X-Box for new PowerA Xbox One controllers + if ( + gamepad.id.includes('Microsoft') || + gamepad.id.includes('Xbox') || + gamepad.id.includes('X-Box') + ) { + let buttons = gamepad.buttons.map((button) => { + return button.value + }) - this.publishJoystickMessage(gamepad.axes, buttons, this.arm_mode, this.positions) + this.publishJoystickMessage(gamepad.axes, buttons, this.arm_mode, []) + } } } } @@ -221,13 +235,15 @@ export default defineComponent({ this.positions = Object.values(this.temp_positions).map( (obj) => (Number(obj.value) * Math.PI) / 180 ) + this.send_positions = true + this.publishJoystickMessage([], [], this.arm_mode, this.positions) }, - keyDown: function(event: { key: string }) { + keyDown: function (event: { key: string }) { if (event.key == ' ') { - this.arm_mode = 'arm_disabled'; + this.arm_mode = 'arm_disabled' } - }, + } } }) diff --git a/src/teleoperation/frontend/src/components/AutonTask.vue b/src/teleoperation/frontend/src/components/AutonTask.vue index 35018caa2..bf06605f8 100644 --- a/src/teleoperation/frontend/src/components/AutonTask.vue +++ b/src/teleoperation/frontend/src/components/AutonTask.vue @@ -20,11 +20,11 @@

Joystick Values

-
- -
+
+ +
@@ -156,7 +156,7 @@ export default defineComponent({ this.odom.latitude_deg = msg.latitude this.odom.longitude_deg = msg.longitude this.odom.altitude = msg.altitude - } else if (msg.type == 'auton_tfclient') { + } else if (msg.type == 'bearing') { this.odom.bearing_deg = quaternionToMapAngle(msg.rotation) } else if (msg.type == "center_map") { this.odom.latitude_deg = msg.latitude @@ -179,7 +179,7 @@ export default defineComponent({ this.sendMessage({ "type": "center_map" }); }, 250) interval = setInterval(() => { - this.sendMessage({ type: 'auton_tfclient' }) + this.sendMessage({ type: 'bearing' }) }, 1000) }, @@ -190,15 +190,15 @@ export default defineComponent({ .wrapper { display: grid; grid-gap: 10px; - grid-template-columns: 60% 40%; + grid-template-columns: auto 30% 30%; grid-template-rows: repeat(6, auto); grid-template-areas: - 'header header' - 'map waypoints' - 'data waypoints' - 'data conditions' - 'moteus moteus' - 'cameras cameras'; + 'header header header' + 'feed map waypoints' + 'data data waypoints' + 'data data conditions' + 'moteus moteus moteus' + 'cameras cameras cameras'; font-family: sans-serif; height: auto; @@ -308,4 +308,8 @@ h2 { .data { grid-area: data; } + +.feed { + grid-area: feed; +} diff --git a/src/teleoperation/frontend/src/components/CameraFeed.vue b/src/teleoperation/frontend/src/components/CameraFeed.vue index efbce30af..8b96eefb8 100644 --- a/src/teleoperation/frontend/src/components/CameraFeed.vue +++ b/src/teleoperation/frontend/src/components/CameraFeed.vue @@ -4,26 +4,26 @@

{{ name }} • ID: {{ id }}

- - - - -
- + > + + +
+ diff --git a/src/teleoperation/frontend/src/components/Rover3D.vue b/src/teleoperation/frontend/src/components/Rover3D.vue new file mode 100644 index 000000000..3c53d78ea --- /dev/null +++ b/src/teleoperation/frontend/src/components/Rover3D.vue @@ -0,0 +1,57 @@ + + + + + \ No newline at end of file diff --git a/src/teleoperation/frontend/src/components/SAArmControls.vue b/src/teleoperation/frontend/src/components/SAArmControls.vue index d624b8036..5047bd2db 100644 --- a/src/teleoperation/frontend/src/components/SAArmControls.vue +++ b/src/teleoperation/frontend/src/components/SAArmControls.vue @@ -45,6 +45,8 @@
+ +

SA Z Position: {{ z_position }} inches

{ + return button.value + }) + + this.publishJoystickMessage(gamepad.axes, buttons, this.arm_mode, []) + } } } } @@ -146,6 +155,15 @@ export default defineComponent({ alert('Toggling Arm Laser failed.') } } + else if (msg.type == 'sa_z') { + this.z_position = msg.sa_z * metersToInches + } + }, + arm_mode(newMode) { + if (newMode !== 'position') { + this.positions = [] + this.send_positions = false + } } }, @@ -178,6 +196,12 @@ export default defineComponent({ this.positions = Object.values(this.temp_positions).map( (obj) => (Number(obj.value) * Math.PI) / 180 ) + this.send_positions = true + this.publishJoystickMessage([], [], this.arm_mode, this.positions) + }, + + zero: function() { + this.sendMessage({type: "arm_adjust", name: "sa_z", value: 0}) } } }) diff --git a/src/teleoperation/frontend/src/components/SATask.vue b/src/teleoperation/frontend/src/components/SATask.vue index 45af1e6fa..4382b7c56 100644 --- a/src/teleoperation/frontend/src/components/SATask.vue +++ b/src/teleoperation/frontend/src/components/SATask.vue @@ -78,13 +78,6 @@ :topic_name="'sa_calibrate_sensor_actuator'" />
-
@@ -113,7 +106,9 @@ import MotorAdjust from './MotorAdjust.vue' import OdometryReading from './OdometryReading.vue' import SAArmControls from './SAArmControls.vue' import { disableAutonLED, quaternionToMapAngle } from '../utils.js' -import { mapState } from 'vuex' +import { mapState, mapActions } from 'vuex' + +let interval: number export default { components: { @@ -182,10 +177,26 @@ export default { this.moteusState.error = msg.error this.moteusState.limit_hit = msg.limit_hit } + else if (msg.type == 'nav_sat_fix') { + this.odom.latitude_deg = msg.latitude + this.odom.longitude_deg = msg.longitude + this.odom.altitude = msg.altitude + } + else if (msg.type == 'bearing') { + this.odom.bearing_deg = quaternionToMapAngle(msg.rotation) + } } }, - created: function () {} + methods: { + ...mapActions('websocket', ['sendMessage']), + }, + + created: function () { + interval = setInterval(() => { + this.sendMessage({ type: 'bearing' }) + }, 1000) + } } diff --git a/src/teleoperation/frontend/src/router/index.ts b/src/teleoperation/frontend/src/router/index.ts index f34e022f1..c8681c3d0 100644 --- a/src/teleoperation/frontend/src/router/index.ts +++ b/src/teleoperation/frontend/src/router/index.ts @@ -4,6 +4,7 @@ import DMESTask from '../components/DMESTask.vue' import AutonTask from '../components/AutonTask.vue' import ISHTask from '../components/ISHTask.vue' import SATask from '../components/SATask.vue' +import Rover3D from '../components/Rover3D.vue' const routes = [ { @@ -41,7 +42,13 @@ const routes = [ path: '/ISHTask', name: 'ISHTask', component: ISHTask - } + }, + { + path: "/Control", + name: "Control", + component: Rover3D, + }, + ] const router = createRouter({ history: createWebHistory(), diff --git a/src/teleoperation/frontend/src/rover_three.js b/src/teleoperation/frontend/src/rover_three.js new file mode 100644 index 000000000..ac83a7d02 --- /dev/null +++ b/src/teleoperation/frontend/src/rover_three.js @@ -0,0 +1,162 @@ +import * as THREE from 'three'; +import { FBXLoader } from 'three/examples/jsm/loaders/FBXLoader' +import { TrackballControls } from 'three/addons/controls/TrackballControls.js'; + +export function threeSetup(containerId) { + // Get the container where the scene should be placed + const container = document.getElementById(containerId); + + const canvas = { + width: container.clientWidth, + height: container.clientHeight + } + + const scene = new THREE.Scene(); + const camera = new THREE.PerspectiveCamera(75, canvas.width / canvas.height, 0.1, 1000); + camera.up.set(0, 0, 1); + + const directionalLight = new THREE.DirectionalLight(0xffffff, 0.5); + directionalLight.position.set(1, 2, 3); + scene.add(directionalLight); + + const renderer = new THREE.WebGLRenderer(); + renderer.setSize(canvas.width, canvas.height); + container.appendChild(renderer.domElement); // append it to your specific HTML element + + scene.background = new THREE.Color(0xcccccc); + + //controls to orbit around model + var controls = new TrackballControls(camera, renderer.domElement); + controls.rotateSpeed = 1.0; + controls.zoomSpeed = 1.2; + controls.panSpeed = 0.8; + controls.noZoom = false; + controls.noPan = false; + controls.staticMoving = true; + controls.dynamicDampingFactor = 0.3; + + const joints = [ + { + name: "chassis", + file: "rover_chassis.fbx", + translation: [0.164882, -0.200235, 0.051497], + rotation: [0, 0, 0], + }, + { + name: "a", + file: "arm_a.fbx", + translation: [0.034346, 0, 0.049024], + rotation: [0, 0, 0], + }, + { + name: "b", + file: "arm_b.fbx", + translation: [0.534365, 0, 0.009056], + rotation: [0, 0, 0], + }, + { + name: "c", + file: "arm_c.fbx", + translation: [0.546033, 0, 0.088594], + rotation: [0, 0, 0], + }, + { + name: "d", + file: "arm_d.fbx", + translation: [0.044886, 0, 0], + rotation: [0, 0, 0], + }, + { + name: "e", + file: "arm_e.fbx", + translation: [0.044886, 0, 0], + rotation: [0, 0, 0], + } + ] + + const loader = new FBXLoader(); + for (let i = 0; i < joints.length; ++i) { + loader.load('/meshes/' + joints[i].file, (fbx) => { + // Traverse the loaded scene to find meshes or objects + fbx.traverse((child)=> { + if (child.isMesh) { + if(joints[i].name == "d" || joints[i].name == "e") { + child.material = new THREE.MeshStandardMaterial({color: 0x00ff00}); + } + else child.material = new THREE.MeshStandardMaterial({color: 0xffffff}); + scene.add(child); + child.scale.set(1, 1, 1); + child.position.set(0, 0, 0); + } + }); + }, + (xhr) => { + console.log((xhr.loaded / xhr.total) * 100 + '% loaded') + }, + (error) => { + console.log(error) + }); + } + + camera.position.x = 2; + camera.position.y = 2; + camera.position.z = 2; + camera.lookAt(new THREE.Vector3(0, 0, 0)); + + const targetCube = new THREE.Mesh( + new THREE.BoxGeometry( 0.1, 0.1, 0.1 ), + new THREE.MeshBasicMaterial({color: 0x00ff00}) + ); + scene.add( targetCube ); + + function animate() { + requestAnimationFrame(animate); + controls.update(); + renderer.render(scene, camera); + } + + animate(); + + function fk(positions) { + + let cumulativeMatrix = new THREE.Matrix4(); + + cumulativeMatrix.makeTranslation(new THREE.Vector3(0, 0, 0.439675)); //makes meshes relative to base_link + + for(let i = 0; i < joints.length; ++i){ + let mesh = scene.getObjectByName(joints[i].name); + + let localMatrix = new THREE.Matrix4(); + + let rotationAngle = positions[i]; // Angle for this joint + if(joints[i].name == "chassis") { + localMatrix.makeTranslation(0,rotationAngle,0); + } + else { + localMatrix.makeRotationY(rotationAngle); + } + + let offset = new THREE.Vector3().fromArray(joints[i].translation); + + localMatrix.setPosition( + new THREE.Vector3().setFromMatrixPosition(localMatrix).add(offset) + ); + + mesh.matrixAutoUpdate = false; + mesh.matrix = cumulativeMatrix.clone(); + + cumulativeMatrix.multiply(localMatrix); + } + } + + function ik(target) { + let quaternion = new THREE.Quaternion(...target.quaternion); + targetCube.position.set(...target.position); + targetCube.setRotationFromQuaternion(quaternion); + } + + // Return an object with the updateJointAngles() function + return { + fk, ik + } +}; diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 3aedbbc29..f79b800bb 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -204,7 +204,7 @@ - + From 81c3de8e4a0760f15585ad3259971861e60ab951 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 27 Apr 2024 20:58:02 -0400 Subject: [PATCH 142/145] Format --- src/localization/gps_linearization.py | 1 + src/navigation/context.py | 6 ++++-- src/navigation/long_range.py | 2 +- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 84aac80a4..390037622 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -16,6 +16,7 @@ QUEUE_SIZE = 10 SLOP = 0.5 + class GPSLinearization: """ This node subscribes to GPS and IMU data, linearizes the GPS data diff --git a/src/navigation/context.py b/src/navigation/context.py index 68ca8ab20..3face5797 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -168,7 +168,7 @@ def push_frame(self, tags: List[LongRangeTag]) -> None: time_difference = rospy.get_time() - cur_tag.time if time_difference > LONG_RANGE_TAG_EXPIRATION_TIME_SECONDS: del self.__data[cur_tag.tag.id] - # if we do see one of our tags in the new message, increment its hit count + # if we do see one of our tags in the new message, increment its hit count else: cur_tag.hit_count += INCREMENT_WEIGHT if cur_tag.hit_count > self.max_hits: @@ -195,7 +195,8 @@ def get(self, tag_id: int) -> Optional[LongRangeTag]: time_difference = rospy.get_time() - self.__data[tag_id].time if ( # self.__data[tag_id].hit_count >= self.min_hits and - time_difference <= LONG_RANGE_TAG_EXPIRATION_TIME_SECONDS + time_difference + <= LONG_RANGE_TAG_EXPIRATION_TIME_SECONDS ): return self.__data[tag_id].tag else: @@ -271,6 +272,7 @@ def get_approach_target_state(self) -> Optional[State]: if self.ctx.env.current_target_pos() is not None: return approach_post.ApproachPostState() # if we see the tag in the long range camera, go to LongRangeState + assert current_waypoint is not None if self.ctx.env.long_range_tags.get(current_waypoint.tag_id) is not None: return long_range.LongRangeState() elif self.look_for_object(): diff --git a/src/navigation/long_range.py b/src/navigation/long_range.py index 1924c5284..669acde75 100644 --- a/src/navigation/long_range.py +++ b/src/navigation/long_range.py @@ -42,7 +42,7 @@ def get_target_pos(self, context) -> Optional[np.ndarray]: rover_direction = pose.rotation.direction_vector() bearing_to_tag = np.radians(tag.bearing) - # If you have not seen the tag in a while but are waiting until the expiration time is up, + # If you have not seen the tag in a while but are waiting until the expiration time is up, # keep going towards where it was last seen (the direction you are heading), don't use an old bearing value if context.env.long_range_tags._LongRangeTagStore__data[tag_id].hit_count <= 0: bearing_to_tag = 0 From 2b44e4a60d78c211a8277d939c98b9b41708b7ed Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 28 Apr 2024 00:45:31 -0400 Subject: [PATCH 143/145] added rtk basestation launch to basestation launch file and changed coords to be baja coords --- config/localization.yaml | 4 ++-- launch/basestation.launch | 3 +++ launch/localization.launch | 2 +- src/teleoperation/frontend/src/components/AutonTask.vue | 4 ++-- 4 files changed, 8 insertions(+), 5 deletions(-) diff --git a/config/localization.yaml b/config/localization.yaml index 698a69056..b6ad42762 100644 --- a/config/localization.yaml +++ b/config/localization.yaml @@ -4,8 +4,8 @@ rover_frame: "base_link" use_odom_frame: false gps_linearization: - reference_point_latitude: 42.293195 - reference_point_longitude: -83.7096706 + reference_point_latitude: 42.30008806193693 + reference_point_longitude: -83.6931540297569 reference_point_altitude: 234.1 reference_heading: 90.0 use_both_gps: false diff --git a/launch/basestation.launch b/launch/basestation.launch index 10679cc71..7b0c24ccc 100644 --- a/launch/basestation.launch +++ b/launch/basestation.launch @@ -7,6 +7,7 @@ + @@ -23,6 +24,8 @@ + + diff --git a/launch/localization.launch b/launch/localization.launch index 45803a5a9..93b2694f4 100644 --- a/launch/localization.launch +++ b/launch/localization.launch @@ -1,6 +1,6 @@ - + diff --git a/src/teleoperation/frontend/src/components/AutonTask.vue b/src/teleoperation/frontend/src/components/AutonTask.vue index f0da85182..deca11a99 100644 --- a/src/teleoperation/frontend/src/components/AutonTask.vue +++ b/src/teleoperation/frontend/src/components/AutonTask.vue @@ -91,8 +91,8 @@ export default defineComponent({ return { // Default coordinates are at MDRS odom: { - latitude_deg: 42.293195, - longitude_deg: -83.7096706, + latitude_deg: 42.30008806193693, + longitude_deg: -83.6931540297569, bearing_deg: 0, altitude: 0 }, From c5bf74e93c3d5c41fc036e8bbceceab0d635c736 Mon Sep 17 00:00:00 2001 From: neuenfeldttj Date: Sun, 28 Apr 2024 15:51:24 -0400 Subject: [PATCH 144/145] added waypoint modal --- .../frontend/src/components/AutonRoverMap.vue | 6 +- .../src/components/AutonWaypointEditor.vue | 66 ++++++++++++++++++- 2 files changed, 68 insertions(+), 4 deletions(-) diff --git a/src/teleoperation/frontend/src/components/AutonRoverMap.vue b/src/teleoperation/frontend/src/components/AutonRoverMap.vue index 71cc8ebde..74d5a0421 100644 --- a/src/teleoperation/frontend/src/components/AutonRoverMap.vue +++ b/src/teleoperation/frontend/src/components/AutonRoverMap.vue @@ -175,10 +175,10 @@ export default { }) }, // Event listener for setting store values to get data to waypoint Editor - getClickedLatLon: function (e: { latLng: { lat: any; lng: any } }) { + getClickedLatLon: function (e: { latlng: { lat: any; lng: any } }) { this.setClickPoint({ - lat: e.latLng.lat, - lon: e.latLng.lng + lat: e.latlng.lat, + lon: e.latlng.lng }) }, diff --git a/src/teleoperation/frontend/src/components/AutonWaypointEditor.vue b/src/teleoperation/frontend/src/components/AutonWaypointEditor.vue index a078ef511..adf670053 100644 --- a/src/teleoperation/frontend/src/components/AutonWaypointEditor.vue +++ b/src/teleoperation/frontend/src/components/AutonWaypointEditor.vue @@ -4,6 +4,7 @@

All Waypoints

+
{{ waypoint.name }}
@@ -42,6 +43,36 @@
+ +