Improved the Audio Interaction for the Demo

config/gen2_feeding_demo.yaml

@@ -121,14 +121,14 @@ foodItems:
   #pickUpAngleModes: [0, 2, 2, 0, 0, 0, 0]
   names: ["apple", "banana", "bell_pepper", "broccoli", "cantaloupe", "carrot", "cauliflower", "celery", "cherry_tomato", "grape", "honeydew", "kiwi", "strawberry", "lettuce", "spinach", "kale"]
   forces: [10, 10, 10, 10, 15, 22, 7, 22, 7, 10, 7, 7, 15, 7, 7, 7]
-  pickUpAngleModes: [1, 5, 1, 2, 1, 1, 0, 1, 1, 3, 3, 3, 0, 1, 2, 0]
+  pickUpAngleModes: [1, 5, 1, 2, 1, 1, 0, 1, 1, 1, 3, 3, 0, 1, 2, 0]
 
 humanStudy:
   autoAcquisition: true
   autoTiming: true
   autoTransfer: true
   createError: false
-  useAlexa: true
+  useAlexa: false
   foodTopic: /alexa_msgs
   actionTopic: /alexa_msgs
 

config/gen3_feeding_demo.yaml

@@ -130,6 +130,7 @@ humanStudy:
   autoTiming: true
   autoTransfer: true
   createError: false
+  useAlexa: false
   foodTopic: /study_food_msgs
   actionTopic: /study_action_msgs
 

include/feeding/FeedingDemo.hpp

@@ -36,13 +36,14 @@ class FeedingDemo {
   /// simulation.
   /// \param[in] useFTSensing turns the FTSensor and the
   /// MoveUntilTouchController on and off
-  /// \param[in] useVisualServo If true, perception servo is used.
+  /// \param[in] useVisualServo If true, perception servo is used.\
+  /// \param[in] useSound If true, use audio to explain each step of the process.
   /// \param[in] allowFreeRotation, If true, items specified as rotationFree
   /// get rotational freedom.
   /// \param[in] nodeHandle Handle of the ros node.
   FeedingDemo(bool adaReal, std::shared_ptr<ros::NodeHandle> nodeHandle,
               bool useFTSensingToStopTrajectories, bool useVisualServo,
-              bool allowFreeRotation,
+              bool allowFreeRotation, bool useSound,
               std::shared_ptr<FTThresholdHelper> ftThresholdHelper = nullptr,
               bool autoContinueDemo = false);
 
@@ -104,6 +105,9 @@ class FeedingDemo {
   std::unordered_map<std::string, double> mPersonTSRParameters;
   double mMoveOufOfFoodLength;
 
+  bool mUseSound;
+  bool mUseAlexa;
+
   double mPlanningTimeout;
   int mMaxNumTrials;
   int mBatchSize;

include/feeding/util.hpp

@@ -44,7 +44,7 @@ static aikido::rviz::InteractiveMarkerViewerPtr VIEWER;
 /// \param[out] dataCollectorPath Directory to store data collection
 /// on and off
 void handleArguments(int argc, char **argv, bool &adaReal,
-                     bool &autoContinueDemo, bool &useFTSensing,
+                     bool &autoContinueDemo, bool &useFTSensing, bool& useSound,
                      std::string &demoType, std::string &foodName,
                      std::size_t &directionIndex, std::size_t &trialIndex,
                      std::string &dataCollectorPath,
@@ -61,7 +61,7 @@ void dumpSplinePhasePlot(const aikido::trajectory::Spline &spline,
 /// param[in] food_only If true, only food choices are valid
 /// param[in]] nodeHandle Ros Node to set food name for detection.
 std::string getUserFoodInput(bool food_only, ros::NodeHandle &nodeHandle,
-                             bool useAlexa = true, double timeout = 5);
+                             bool useAlexa = true, double timeout = 10);
 
 int getUserInputWithOptions(const std::vector<std::string> &optionPrompts,
                             const std::string &prompt);
@@ -101,7 +101,7 @@ std::string getInputFromTopic(std::string topic,
                               const ros::NodeHandle &nodeHandle,
                               bool validateAsFood, double timeout = 20);
 
-void talk(const std::string &, bool background = false);
+void talk(const std::string&, bool background = true);
 
 void initTopics(ros::NodeHandle *nodeHandle);
 

scripts/demo.cpp

@@ -31,7 +31,10 @@ void demo(
   auto workspace = feedingDemo.getWorkspace();
   auto plate = workspace->getPlate()->getRootBodyNode()->getWorldTransform();
 
-  talk("Hello, my name is aid uh. It's my pleasure to serve you today!");
+  bool useSound = feedingDemo.mUseSound;
+
+  if (useSound) 
+    talk("Hello, my name is aid uh. I'm your feeding companion!");
 
   srand(time(NULL));
 
@@ -40,9 +43,10 @@ void demo(
     if (feedingDemo.getFTThresholdHelper())
         feedingDemo.getFTThresholdHelper()->setThresholds(STANDARD_FT_THRESHOLD);
 
-    talk("What food would you like?");
+    if (useSound)
+      talk("What food would you like?", false);
 
-    auto foodName = getUserFoodInput(false, nodeHandle);// "cantaloupe";//
+    auto foodName = getUserFoodInput(false, nodeHandle, feedingDemo.mUseAlexa);// "cantaloupe";//
     if (foodName == std::string("quit")) {
         break;
     }
@@ -55,34 +59,37 @@ void demo(
     ROS_INFO_STREAM("Running bite transfer study for " << foodName);
 
 /*
-    switch((rand() % 10)) {
-        case 0:
-        talk("Good choice!");
-        break;
-        case 1:
-        talk(std::string("Great! I love ") + foodName + std::string("'s!"));
-        break;
-        case 2:
-        talk("Sounds delicious. I wish I had taste buds.");
-        break;
-        case 4:
-        talk("Roger Roger.");
-        break;
-        case 5:
-        talk("Nothing beats fresh fruit.");
-        break;
-        case 6:
-        talk("Nothing escapes my fork!");
-        break;
-        case 7:
-        talk("Thanks Alexa!");
-        break;
-        default:
-        talk("Alright.");
+    if (useSound) {
+      switch((rand() % 10)) {
+          case 0:
+          talk("Good choice!");
+          break;
+          case 1:
+          talk(std::string("Great! I love ") + foodName + std::string("'s!"));
+          break;
+          case 2:
+          talk("Sounds delicious. I wish I had taste buds.");
+          break;
+          case 4:
+          talk("Roger Roger.");
+          break;
+          case 5:
+          talk("Nothing beats fresh fruit.");
+          break;
+          case 6:
+          talk("Nothing escapes my fork!");
+          break;
+          case 7:
+          talk("Thanks Alexa!");
+          break;
+          default:
+          talk("Alright.");
+      }
     }
 */
 
-    talk(std::string("One ") + foodName + std::string(" coming right up!"), true);
+    if (useSound) 
+      talk(std::string("One ") + foodName + std::string(" coming right up!"), true);
 
     // ===== FORQUE PICKUP =====
     if (foodName == "pickupfork")
@@ -135,6 +142,7 @@ void demo(
 
   // ===== DONE =====
   ROS_INFO("Demo finished.");
-  talk("Thank you, I hope I was helpful!");
+  if (useSound)
+    talk("Thank you, I look forward to feeding you again!");
 }
 };

scripts/main.cpp

@@ -45,6 +45,9 @@ int main(int argc, char** argv)
   // the FT sensing can stop trajectories if the forces are too big
   bool useFTSensingToStopTrajectories = false;
 
+  // should the ADA speak between steps?
+  bool useSound = false;
+
   bool TERMINATE_AT_USER_PROMPT = true;
 
   std::string demoType{"default"};
@@ -56,7 +59,7 @@ int main(int argc, char** argv)
   std::size_t trialIndex{0};
 
   handleArguments(argc, argv,
-    adaReal, autoContinueDemo, useFTSensingToStopTrajectories,
+    adaReal, autoContinueDemo, useFTSensingToStopTrajectories, useSound,
     demoType, foodName, directionIndex, trialIndex, dataCollectorPath);
 
   bool useVisualServo = true;
@@ -113,6 +116,7 @@ int main(int argc, char** argv)
     useFTSensingToStopTrajectories,
     useVisualServo,
     allowRotationFree,
+    useSound,
     ftThresholdHelper,
     autoContinueDemo);
 

src/FeedingDemo.cpp

@@ -29,12 +29,12 @@ namespace feeding {
 FeedingDemo::FeedingDemo(bool adaReal,
                          std::shared_ptr<ros::NodeHandle> nodeHandle,
                          bool useFTSensingToStopTrajectories,
-                         bool useVisualServo, bool allowFreeRotation,
+                         bool useVisualServo, bool allowFreeRotation, bool useSound,
                          std::shared_ptr<FTThresholdHelper> ftThresholdHelper,
                          bool autoContinueDemo)
     : mAdaReal(adaReal), mNodeHandle(nodeHandle),
       mFTThresholdHelper(ftThresholdHelper), mVisualServo(useVisualServo),
-      mAllowRotationFree(allowFreeRotation),
+      mAllowRotationFree(allowFreeRotation), mUseSound(useSound),
       mAutoContinueDemo(autoContinueDemo),
       mIsFTSensingEnabled(useFTSensingToStopTrajectories) {
   mWorld = std::make_shared<aikido::planner::World>("feeding");
@@ -167,6 +167,8 @@ FeedingDemo::FeedingDemo(bool adaReal,
   }
 
   mTableHeight = getRosParam<double>("/study/tableHeight", *mNodeHandle);
+
+  mUseAlexa = getRosParam<bool>("/humanStudy/useAlexa", *mNodeHandle);
 }
 
 //==============================================================================

src/action/DetectAndMoveAboveFood.cpp

@@ -20,14 +20,17 @@ detectAndMoveAboveFood(const std::shared_ptr<Perception> &perception,
                        const std::string &foodName, double rotationTolerance,
                        FeedingDemo *feedingDemo, double *angleGuess,
                        int actionOverride) {
+  bool useSound = feedingDemo->mUseSound;
+
   std::vector<std::unique_ptr<FoodItem>> candidateItems;
   while (true) {
     // Perception returns the list of good candidates, any one of them is good.
     // Multiple candidates are preferrable since planning may fail.
     candidateItems = perception->perceiveFood(foodName);
 
     if (candidateItems.size() == 0) {
-      // talk("I can't find that food. Try putting it on the plate.");
+      // if (useSound) 
+      //   talk("I can't find that food. Try putting it on the plate.");
       ROS_WARN_STREAM(
           "Failed to detect any food. Please place food on the plate.");
     } else
@@ -58,7 +61,8 @@ detectAndMoveAboveFood(const std::shared_ptr<Perception> &perception,
         action->getTiltStyle(), rotationTolerance, feedingDemo, angleGuess);
     if (!moveAboveSuccessful) {
       ROS_INFO_STREAM("Failed to move above " << item->getName());
-      talk("Sorry, I'm having a little trouble moving. Let's try again.");
+      if (useSound) 
+        talk("Sorry, I'm having a little trouble moving. Let's try again.");
       return nullptr;
     }
 
@@ -69,8 +73,10 @@ detectAndMoveAboveFood(const std::shared_ptr<Perception> &perception,
 
   if (!moveAboveSuccessful) {
     ROS_ERROR("Failed to move above any food.");
-    talk("Sorry, I'm having a little trouble moving. Mind if I get a little "
-         "help?");
+    if (useSound) 
+      talk(
+          "Sorry, I'm having a little trouble moving. Mind if I get a little "
+          "help?");
     return nullptr;
   }
 

src/action/FeedFoodToPerson.cpp

@@ -34,6 +34,7 @@ void feedFoodToPerson(const std::shared_ptr<Perception> &perception,
   // feedingDemo->getCollisionConstraintWithWallFurtherBack();
   const Eigen::Isometry3d &personPose = workspace->getPersonPose();
   std::chrono::milliseconds waitAtPerson = feedingDemo->mWaitTimeForPerson;
+  bool useSound = feedingDemo->mUseSound;
   double distanceToPerson = feedingDemo->mPersonTSRParameters.at("distance");
   double horizontalToleranceForPerson =
       feedingDemo->mPersonTSRParameters.at("horizontalTolerance");
@@ -60,7 +61,8 @@ void feedFoodToPerson(const std::shared_ptr<Perception> &perception,
     moveIFOSuccess = moveIFOPerson();
     if (!moveIFOSuccess) {
       ROS_WARN_STREAM("Failed to move in front of person, retry");
-      talk("Sorry, I'm having a little trouble moving. Let me try again.");
+      if (useSound) 
+        talk("Sorry, I'm having a little trouble moving. Let me try again.");
       continue;
     } else
       break;
@@ -73,7 +75,8 @@ void feedFoodToPerson(const std::shared_ptr<Perception> &perception,
   auto overrideTiltOffset = tiltOffset;
 
   if (!getRosParam<bool>("/humanStudy/autoTransfer", *nodeHandle)) {
-    talk("Should I tilt the food item?", false);
+    if (useSound) 
+      talk("Should I tilt the food item?", false);
     std::string done = "";
     std::string actionTopic;
     nodeHandle->param<std::string>("/humanStudy/actionTopic", actionTopic,
@@ -106,7 +109,8 @@ void feedFoodToPerson(const std::shared_ptr<Perception> &perception,
 
   // Check autoTiming, and if false, wait for topic
   if (!getRosParam<bool>("/humanStudy/autoTiming", *nodeHandle)) {
-    talk("Let me know when you are ready.", false);
+    if (useSound) 
+      talk("Let me know when you are ready.", false);
     std::string done = "";
     while (done != "continue") {
       std::string actionTopic;
@@ -116,7 +120,8 @@ void feedFoodToPerson(const std::shared_ptr<Perception> &perception,
     }
   } else {
     nodeHandle->setParam("/feeding/facePerceptionOn", true);
-    talk("Open your mouth when ready.", false);
+    if (useSound) 
+      talk("Open your mouth when ready.", false);
     // TODO: Add mouth-open detection.
     while (true) {
       std::this_thread::sleep_for(std::chrono::milliseconds(100));
@@ -132,7 +137,8 @@ void feedFoodToPerson(const std::shared_ptr<Perception> &perception,
     if (getRosParam<bool>("/humanStudy/createError", *nodeHandle)) {
       // Wait an extra 5 seconds
       ROS_WARN_STREAM("Error Requested for Timing!");
-      talk("Calculating...");
+      if (useSound) 
+        talk("Calculating...");
       std::this_thread::sleep_for(std::chrono::milliseconds(5000));
     }
   }
@@ -163,7 +169,8 @@ void feedFoodToPerson(const std::shared_ptr<Perception> &perception,
         success = false;
       }
       std::this_thread::sleep_for(std::chrono::milliseconds(3000));
-      talk("Oops, let me try that again.", true);
+      if (useSound) 
+        talk("Oops, let me try that again.", true);
       moveIFOSuccess = moveInFrontOfPerson(
           ada->getArm()->getSelfCollisionConstraint(), personPose,
           distanceToPerson, horizontalToleranceForPerson,
@@ -215,7 +222,8 @@ void feedFoodToPerson(const std::shared_ptr<Perception> &perception,
     double slowFactor = (velocityLimits[0] > 0.5) ? 2.0 : 1.0;
     slowerVelocity /= slowFactor;
 
-    talk("Tilting, hold tight.", true);
+    if (useSound) 
+      talk("Tilting, hold tight.", true);
 
     auto trajectory = ada->getArm()->planToConfiguration(
         ada->getArm()->getNamedConfiguration("home_config"),
@@ -261,12 +269,14 @@ void feedFoodToPerson(const std::shared_ptr<Perception> &perception,
   if (moveIFOSuccess) {
     // ===== EATING =====
     ROS_WARN("Human is eating");
-    talk("Ready to eat!");
+    if (useSound) 
+      talk("Ready to eat!");
     std::this_thread::sleep_for(waitAtPerson);
 
     // Backward
     ada::util::waitForUser("Move backward", ada);
-    talk("Let me get out of your way.", true);
+    if (useSound) 
+      talk("Let me get out of your way.", true);
     Eigen::Vector3d goalDirection(0, -1, 0);
     bool success = moveInFrontOfPerson(
         ada->getArm()->getWorldCollisionConstraint(
@@ -281,7 +291,8 @@ void feedFoodToPerson(const std::shared_ptr<Perception> &perception,
 
   // TODO: add a back-out motion and then do move above plate with
   // collisionFree.
-  talk("And now back to the plate.", true);
+  // if (useSound) 
+  //   talk("And now back to the plate.", true);
   moveAbovePlate(plate, plateEndEffectorTransform, feedingDemo);
 
   publishTimingDoneToWeb((ros::NodeHandle *)nodeHandle);

src/action/MoveAbove.cpp

@@ -20,6 +20,7 @@ bool moveAbove(const Eigen::Isometry3d &targetTransform,
   // Load necessary parameters from feedingDemo
   const std::shared_ptr<::ada::Ada> &ada = feedingDemo->getAda();
   const CollisionFreePtr &collisionFree = feedingDemo->getCollisionConstraint();
+  bool useSound = feedingDemo->mUseSound;
   double planningTimeout = feedingDemo->mPlanningTimeout;
   int maxNumTrials = feedingDemo->mMaxNumTrials;
   int batchSize = feedingDemo->mBatchSize;
@@ -83,7 +84,8 @@ bool moveAbove(const Eigen::Isometry3d &targetTransform,
 
   } while (rotationTolerance <= 2.0);
   if (!trajectoryCompleted) {
-    // talk("No trajectory, check T.S.R.", true);
+    // if (useSound) 
+    //   talk("No trajectory, check T.S.R.", true);
     if (feedingDemo && feedingDemo->getViewer()) {
       feedingDemo->getViewer()->addTSRMarker(target);
       std::cout << "Check TSR" << std::endl;