Merge pull request #605 from jw098/sandwich

Fix Sandwich maker

Author: Mysticial

SerialPrograms/Source/PokemonSV/Inference/Picnics/PokemonSV_SandwichPlateDetector.cpp

@@ -5,6 +5,7 @@
  */
 
 
+#include "Common/Cpp/Exceptions.h"
 #include "CommonFramework/ImageTypes/ImageViewRGB32.h"
 #include "CommonFramework/VideoPipeline/VideoOverlayScopes.h"
 #include "CommonTools/Images/ImageFilter.h"
@@ -35,6 +36,9 @@ SandwichPlateDetector::SandwichPlateDetector(Logger& logger, Color color, Langua
     case Side::RIGHT:
         m_box = ImageFloatBox(0.699, 0.269, 0.201, 0.044);
         break;
+    default:
+        throw InternalProgramError(&logger, PA_CURRENT_FUNCTION,
+            "Invalid Side for SandwichPlateDetector()");
     }
 }
 

SerialPrograms/Source/PokemonSV/Inference/Picnics/PokemonSV_SandwichPlateDetector.h

@@ -32,7 +32,8 @@ class SandwichPlateDetector : public StaticScreenDetector{
     enum class Side{
         LEFT,
         MIDDLE,
-        RIGHT
+        RIGHT,
+        NOT_APPLICABLE,
     };
     SandwichPlateDetector(Logger& logger, Color color, Language language, Side side);
     virtual ~SandwichPlateDetector();

SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp

@@ -32,6 +32,9 @@
 #include "PokemonSV/Programs/Sandwiches/PokemonSV_IngredientSession.h"
 #include "PokemonSV/Inference/Picnics/PokemonSV_SandwichPlateDetector.h"
 
+// #include <iostream>
+// using std::cout;
+// using std::endl;
 
 namespace PokemonAutomation{
 namespace NintendoSwitch{
@@ -347,22 +350,56 @@ bool move_then_recover_sandwich_hand_position(
     );
 }
 
+// return true if the current plate is empty.
+// i.e. the current plate label is not yellow.
+// Assumes that we are in a grabbing state.
+bool check_plate_empty(VideoStream& stream, SandwichPlateDetector::Side target_plate_label, Language language){
+    auto screen = stream.video().snapshot();
+    // screen.frame->save("test.png");
+
+    // switch(target_plate_label){
+    //     case SandwichPlateDetector::Side::LEFT:
+    //         cout << "left" << endl;
+    //         break;
+    //     case SandwichPlateDetector::Side::MIDDLE:
+    //         cout << "middle" << endl;
+    //         break;
+    //     case SandwichPlateDetector::Side::RIGHT:
+    //         cout << "right" << endl;
+    //         break;                        
+    //     default:
+    //         break;
+    // }
+
+    SandwichPlateDetector plate_detector = SandwichPlateDetector(stream.logger(), COLOR_RED, language, target_plate_label);
+              
+    return !plate_detector.is_label_yellow(screen);
+}
+
+struct HandMoveData{
+    ImageFloatBox end_box;
+    bool plate_empty;
+};
+
+
 /* 
 - moves the sandwich hand from start_box to end_box
 - It detects the location of the sandwich hand, from within the bounds of the last frame's 
 expanded_hand_bb (i.e. m_box field in SandwichHandLocator). 
 Then updates the current location of expanded_hand_bb. 
 Then moves the sandwich hand closer towards end_box. 
  */
-ImageFloatBox move_sandwich_hand(
+HandMoveData move_sandwich_hand_and_check_if_plates_empty(
     const ProgramInfo& info,
     AsyncDispatcher& dispatcher,
     VideoStream& stream,
     ProControllerContext& context,
     SandwichHandType hand_type,
     bool pressing_A,
     const ImageFloatBox& start_box,
-    const ImageFloatBox& end_box
+    const ImageFloatBox& end_box,
+    SandwichPlateDetector::Side target_plate_label = SandwichPlateDetector::Side::NOT_APPLICABLE,
+    Language language = Language::None
 ){
     context.wait_for_all_requests();
     stream.log("Start moving sandwich hand: " + SANDWICH_HAND_TYPE_NAMES(hand_type)
@@ -454,14 +491,24 @@ ImageFloatBox move_sandwich_hand(
 
         std::pair<double, double> dif(target_loc.first - cur_loc.first, target_loc.second - cur_loc.second);
         // console.log("float diff to target: " + std::to_string(dif.first) + ", " + std::to_string(dif.second));
+
+
+        // Reached the Target
         if (std::fabs(dif.first) < end_box.width/2 && std::fabs(dif.second) < end_box.height/2){
             stream.log(SANDWICH_HAND_TYPE_NAMES(hand_type) + " hand reached target.");
+            bool plate_empty = false;
+
+            // check if the plate is empty. but only if the target_plate_label isn't NOT_APPLICABLE.
+            if (target_plate_label != SandwichPlateDetector::Side::NOT_APPLICABLE){
+                plate_empty = check_plate_empty(stream, target_plate_label, language);
+            }
+
             move_session.stop_session_and_rethrow(); // Stop the commands
             if (hand_type == SandwichHandType::GRABBING){
                 // wait for some time to let hand release ingredient
                 context.wait_for(std::chrono::milliseconds(100));
             }
-            return hand_bb;
+            return {hand_bb, plate_empty};
         }
 
         // Assume screen width is 16.0, then the screen height is 9.0
@@ -517,6 +564,19 @@ ImageFloatBox move_sandwich_hand(
     }
 }
 
+ImageFloatBox move_sandwich_hand(
+    const ProgramInfo& info,
+    AsyncDispatcher& dispatcher,
+    VideoStream& stream,
+    ProControllerContext& context,
+    SandwichHandType hand_type,
+    bool pressing_A,
+    const ImageFloatBox& start_box,
+    const ImageFloatBox& end_box
+){
+    return move_sandwich_hand_and_check_if_plates_empty(info, dispatcher, stream, context, hand_type, pressing_A, start_box, end_box, SandwichPlateDetector::Side::NOT_APPLICABLE, Language::None).end_box;
+}
+
 } // end anonymous namespace
 
 void finish_sandwich_eating(
@@ -1013,7 +1073,8 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl
     SandwichPlateDetector left_plate_detector(stream.logger(), COLOR_RED, language, SandwichPlateDetector::Side::LEFT);
     SandwichPlateDetector middle_plate_detector(stream.logger(), COLOR_RED, language, SandwichPlateDetector::Side::MIDDLE);
     SandwichPlateDetector right_plate_detector(stream.logger(), COLOR_RED, language, SandwichPlateDetector::Side::RIGHT);
-
+    bool left_plate_absent = false;
+    bool right_plate_absent = false;
     {
         VideoSnapshot screen = stream.video().snapshot();
 
@@ -1045,7 +1106,8 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl
 
         //Get left (2nd) ingredient
         std::string left_filling = left_plate_detector.detect_filling_name(screen);
-        if (left_filling.empty()){
+        left_plate_absent = left_filling.empty();
+        if (left_plate_absent){
             stream.log("No ingredient found on left label.");
             stream.overlay().add_log("No left plate");
         }else{
@@ -1056,7 +1118,8 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl
 
         //Get right (3rd) ingredient
         std::string right_filling = right_plate_detector.detect_filling_name(screen);
-        if (right_filling.empty()){
+        right_plate_absent = right_filling.empty();
+        if (right_plate_absent){
             stream.log("No ingredient found on right label.");
             stream.overlay().add_log("No right plate");
         }else{
@@ -1139,12 +1202,14 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl
     //Find fillings and add them in order
     for (const std::string& i : fillings_sorted){
         //cout << "Placing " << i << endl;
+        stream.log("Placing " + i, COLOR_WHITE);
         stream.overlay().add_log("Placing " + i, COLOR_WHITE);
 
         int times_to_place = (int)(FillingsCoordinates::instance().get_filling_information(i).piecesPerServing) * (fillings.find(i)->second);
         int placement_number = 0;
 
         //cout << "Times to place: " << times_to_place << endl;
+        stream.log("Times to place: " + std::to_string(times_to_place), COLOR_WHITE);
         stream.overlay().add_log("Times to place: " + std::to_string(times_to_place), COLOR_WHITE);
 
         std::vector<int> plate_index;
@@ -1159,16 +1224,20 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl
         for (int j = 0; j < (int)plate_index.size(); j++){
             //Navigate to plate and set target plate
             //cout << "Target plate: " << plate_index.at(j) << endl;
+            stream.log("Target plate: " + std::to_string(plate_index.at(j)), COLOR_WHITE);
             stream.overlay().add_log("Target plate: " + std::to_string(plate_index.at(j)), COLOR_WHITE);
+            SandwichPlateDetector::Side target_plate_label = SandwichPlateDetector::Side::MIDDLE;
             switch (plate_index.at(j)){
             case 0:
                 target_plate = center_plate;
                 break;
             case 1:
                 target_plate = left_plate;
+                target_plate_label = SandwichPlateDetector::Side::LEFT;
                 break;
             case 2:
                 target_plate = right_plate;
+                target_plate_label = SandwichPlateDetector::Side::RIGHT;
                 break;
             case 3: case 4: case 5: case 6:
                 //Press R the appropriate number of times
@@ -1181,10 +1250,12 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl
                 break;
             }
 
+            // place down all the ingredients for the current plate.
             //Place the fillings until label does not light up yellow on grab/the piece count is not hit
             while (true){
                 //Break out after placing all pieces of the filling
                 if (placement_number == times_to_place){
+                    stream.log("We have placed down enough ingredients of that type, so we assume our current plate is empty. Move on to the next plate/ingredient.", COLOR_ORANGE);
                     break;
                 }
 
@@ -1202,26 +1273,27 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl
                 ImageFloatBox placement_target = FillingsCoordinates::instance().get_filling_information(i).placementCoordinates.at(
                     (int)fillings.find(i)->second).at(placement_number);
 
-                end_box = move_sandwich_hand(
+                HandMoveData hand_move_data = move_sandwich_hand_and_check_if_plates_empty(
                     env.program_info(), env.realtime_dispatcher(),
                     stream, context,
                     SandwichHandType::GRABBING,
                     true,
                     expand_box(end_box),
-                    placement_target
+                    placement_target,
+                    target_plate_label
                 );
+                end_box = hand_move_data.end_box;
                 context.wait_for_all_requests();
 
-                //If any of the labels are yellow continue. Otherwise assume plate is empty move on to the next.
-                auto screen = stream.video().snapshot();
-
-                //The label check is needed for ingredients with multiple plates as we don't know which plate has what amount
-                if (!left_plate_detector.is_label_yellow(screen) && !middle_plate_detector.is_label_yellow(screen)
-                    && !right_plate_detector.is_label_yellow(screen)){
+                // If the current plate is empty, break out of the loop and move on to the next plate.
+                if (hand_move_data.plate_empty){
                     context.wait_for_all_requests();
+                    stream.log("Our current plate label is NOT yellow, so we assume our current plate is empty. Move on to the next plate.", COLOR_ORANGE);
                     break;
                 }
 
+                stream.log("Our current plate label is yellow, so we assume our current plate is NOT empty. Continue with the current plate.", COLOR_YELLOW);
+
                 //If the plate is empty the increment is skipped using the above break
                 placement_number++;
             }
@@ -1232,8 +1304,12 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl
             }
         }
     }
+
+    context.wait_for_all_requests();
+    context.wait_for(Milliseconds(500));
+    stream.log("All ingredients should now be empty. Wait for upper bread.", COLOR_YELLOW);
     // Handle top slice by tossing it away
-    SandwichHandWatcher grabbing_hand(SandwichHandType::FREE, { 0, 0, 1.0, 1.0 });
+    SandwichHandWatcher grabbing_hand(SandwichHandType::GRABBING, { 0, 0, 1.0, 1.0 });
     int ret = wait_until(stream, context, std::chrono::seconds(30), { grabbing_hand });
     if (ret < 0){
         OperationFailedException::fire(