diff --git a/config/gen2_feeding_demo.yaml b/config/gen2_feeding_demo.yaml index 985382fe..8a8677b5 100644 --- a/config/gen2_feeding_demo.yaml +++ b/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 diff --git a/config/gen3_feeding_demo.yaml b/config/gen3_feeding_demo.yaml index 3915749c..2005d466 100644 --- a/config/gen3_feeding_demo.yaml +++ b/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 diff --git a/include/feeding/FeedingDemo.hpp b/include/feeding/FeedingDemo.hpp index a8007be5..276ea43b 100644 --- a/include/feeding/FeedingDemo.hpp +++ b/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 nodeHandle, bool useFTSensingToStopTrajectories, bool useVisualServo, - bool allowFreeRotation, + bool allowFreeRotation, bool useSound, std::shared_ptr ftThresholdHelper = nullptr, bool autoContinueDemo = false); @@ -104,6 +105,9 @@ class FeedingDemo { std::unordered_map mPersonTSRParameters; double mMoveOufOfFoodLength; + bool mUseSound; + bool mUseAlexa; + double mPlanningTimeout; int mMaxNumTrials; int mBatchSize; diff --git a/include/feeding/util.hpp b/include/feeding/util.hpp index bf4ebb4c..91dbcc51 100644 --- a/include/feeding/util.hpp +++ b/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 &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); diff --git a/scripts/demo.cpp b/scripts/demo.cpp index 47699af4..7cf84ca4 100644 --- a/scripts/demo.cpp +++ b/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!"); } }; diff --git a/scripts/main.cpp b/scripts/main.cpp index 1631a4c4..035adb23 100644 --- a/scripts/main.cpp +++ b/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); diff --git a/src/FeedingDemo.cpp b/src/FeedingDemo.cpp index 6b203c1e..2aaea4b7 100644 --- a/src/FeedingDemo.cpp +++ b/src/FeedingDemo.cpp @@ -29,12 +29,12 @@ namespace feeding { FeedingDemo::FeedingDemo(bool adaReal, std::shared_ptr nodeHandle, bool useFTSensingToStopTrajectories, - bool useVisualServo, bool allowFreeRotation, + bool useVisualServo, bool allowFreeRotation, bool useSound, std::shared_ptr 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("feeding"); @@ -167,6 +167,8 @@ FeedingDemo::FeedingDemo(bool adaReal, } mTableHeight = getRosParam("/study/tableHeight", *mNodeHandle); + + mUseAlexa = getRosParam("/humanStudy/useAlexa", *mNodeHandle); } //============================================================================== diff --git a/src/action/DetectAndMoveAboveFood.cpp b/src/action/DetectAndMoveAboveFood.cpp index c80edff1..0534ebd4 100644 --- a/src/action/DetectAndMoveAboveFood.cpp +++ b/src/action/DetectAndMoveAboveFood.cpp @@ -20,6 +20,8 @@ detectAndMoveAboveFood(const std::shared_ptr &perception, const std::string &foodName, double rotationTolerance, FeedingDemo *feedingDemo, double *angleGuess, int actionOverride) { + bool useSound = feedingDemo->mUseSound; + std::vector> candidateItems; while (true) { // Perception returns the list of good candidates, any one of them is good. @@ -27,7 +29,8 @@ detectAndMoveAboveFood(const std::shared_ptr &perception, 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, 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, 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; } diff --git a/src/action/FeedFoodToPerson.cpp b/src/action/FeedFoodToPerson.cpp index 947280a5..14cf0298 100644 --- a/src/action/FeedFoodToPerson.cpp +++ b/src/action/FeedFoodToPerson.cpp @@ -34,6 +34,7 @@ void feedFoodToPerson(const std::shared_ptr &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, 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, auto overrideTiltOffset = tiltOffset; if (!getRosParam("/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("/humanStudy/actionTopic", actionTopic, @@ -106,7 +109,8 @@ void feedFoodToPerson(const std::shared_ptr &perception, // Check autoTiming, and if false, wait for topic if (!getRosParam("/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, } } 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, if (getRosParam("/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, 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, 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, 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, // 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); diff --git a/src/action/MoveAbove.cpp b/src/action/MoveAbove.cpp index 3c229f50..208f35d1 100644 --- a/src/action/MoveAbove.cpp +++ b/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; diff --git a/src/action/Skewer.cpp b/src/action/Skewer.cpp index 4fdf52e4..06cd0afc 100644 --- a/src/action/Skewer.cpp +++ b/src/action/Skewer.cpp @@ -31,6 +31,7 @@ bool skewer(const std::shared_ptr &perception, feedingDemo->getCollisionConstraint(); const std::unordered_map &foodSkeweringForces = feedingDemo->mFoodSkeweringForces; + bool useSound = feedingDemo->mUseSound; double heightAboveFood = feedingDemo->mFoodTSRParameters.at("height"); double rotationToleranceForFood = feedingDemo->mFoodTSRParameters.at("rotationTolerance"); @@ -53,8 +54,10 @@ bool skewer(const std::shared_ptr &perception, moveAbovePlate(plate, plateEndEffectorTransform, feedingDemo); if (!abovePlaceSuccess) { - 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?"); ROS_WARN_STREAM("Move above plate failed. Please restart"); return false; } @@ -64,7 +67,8 @@ bool skewer(const std::shared_ptr &perception, if (!getRosParam("/humanStudy/autoAcquisition", *(feedingDemo->getNodeHandle()))) { // Read Action from Topic - talk("How should I pick up the food?", true); + if (useSound) + talk("How should I pick up the food?", true); ROS_INFO_STREAM("Waiting for action..."); std::string actionName; std::string actionTopic; @@ -72,7 +76,9 @@ bool skewer(const std::shared_ptr &perception, "/humanStudy/actionTopic", actionTopic, "/study_action_msgs"); actionName = getInputFromTopic(actionTopic, *(feedingDemo->getNodeHandle()), false, -1); - talk("Alright, let me use " + actionName, false); + if (useSound) + talk("Alright, let me use " + actionName, false); + if (actionName == "skewer") { actionOverride = 1; @@ -121,10 +127,12 @@ bool skewer(const std::shared_ptr &perception, std::unique_ptr item; for (std::size_t i = 0; i < 2; ++i) { if (i == 0) { - talk(std::string("Planning to the ") + foodName, true); + if (useSound) + talk(std::string("Planning to the ") + foodName, true); } if (i == 1) { - talk("Adjusting, hold tight!", true); + // if (useSound) + // talk("Adjusting, hold tight!", true); std::this_thread::sleep_for(std::chrono::milliseconds(500)); // Set Action Override @@ -148,8 +156,8 @@ bool skewer(const std::shared_ptr &perception, Eigen::Vector3d foodVec = item->getPose().rotation() * Eigen::Vector3d::UnitX(); double baseRotateAngle = atan2(foodVec[1], foodVec[0]); - detectAndMoveAboveFood(perception, foodName, rotationToleranceForFood, - feedingDemo, &baseRotateAngle, actionNum); + // detectAndMoveAboveFood(perception, foodName, rotationToleranceForFood, + // feedingDemo, &baseRotateAngle, actionNum); auto tiltStyle = item->getAction()->getTiltStyle(); if (tiltStyle == TiltStyle::ANGLED) { // Apply base rotation of food @@ -208,7 +216,8 @@ bool skewer(const std::shared_ptr &perception, feedingDemo, nullptr, actionOverride); if (!item) { - talk("Failed, let me start from the beginning"); + if (useSound) + talk("Failed, let me start from the beginning"); return false; } @@ -242,13 +251,15 @@ bool skewer(const std::shared_ptr &perception, torqueThreshold); // ===== INTO FOOD ===== - talk("Here we go!", true); + // if (useSound) + // talk("Here we go!", true); auto moveIntoSuccess = moveInto(perception, TargetItem::FOOD, endEffectorDirection, feedingDemo); if (!moveIntoSuccess) { ROS_INFO_STREAM("Failed. 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."); return false; } @@ -263,12 +274,14 @@ bool skewer(const std::shared_ptr &perception, 1) // true)// { ROS_INFO_STREAM("Successful"); - talk("Success."); + if (useSound) + talk("Success."); return true; } ROS_INFO_STREAM("Failed."); - talk("Failed, let me try again."); + if (useSound) + talk("Failed, let me try again."); } return false; } diff --git a/src/util.cpp b/src/util.cpp index 5e5bc96c..3624db80 100644 --- a/src/util.cpp +++ b/src/util.cpp @@ -29,7 +29,7 @@ inline int sgn(double x) { return (x < 0) ? -1 : (x > 0); } //============================================================================== 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, @@ -44,6 +44,9 @@ void handleArguments(int argc, char **argv, bool &adaReal, "Continue Demo automatically")("ftSensing,f", po::bool_switch(&useFTSensing), "Use Force/Torque sensing")( + "useSound,s", + po::bool_switch(&useSound), + "Whether the play sound during the demo")( "demoType,d", po::value(&demoType), "Demo type")("foodName", po::value(&foodName), "Name of food (for data collection)")(