Merge pull request #15 from personalrobotics/egordon/062022_working_demo

Demo updates

Author: taylorkf

README.md

@@ -111,19 +111,23 @@ Run the following commands from your ROS workspace:
 3) Once the lights on the joystick go solid, home ADA by holding the orange button until the robot stops moving.
 4) `ssh nano` (you may need to add `nano` to your `.ssh/config`, this is the Jetson on the robot). Once there, set your ROS Master using `uselovelace`, `useweebo`, or `useweebowired` (or set your ROS_MASTER_URI manually), execute `./run_camera.sh` to start streaming RGBD data.
    * You may have to adjust the camera exposure, depending on the lighting condition. Either run `run_adjust_camera_daylight.sh` or `run_adjust_camera_all.sh` after running `run_camera.sh`. Check the image stream via rviz, by adding the image topic `/camera/color/image_raw/color`. If some area is too bright and look burnt or saturated, reduce the exposure.
-6) `roslaunch forque_sensor_hardware forque.launch` (Optionally add `forque_ip:=<IPv4>` if your Net-FT is on a non-default IP)
+5) `roslaunch forque_sensor_hardware forque.launch` (Optionally add `forque_ip:=<IPv4>` if your Net-FT is on a non-default IP)
 6) `rosrun face_detection face_detection`
-7) `roslaunch libada default.launch use_forque:=true perception:=true detector:=spanet use_apriltag_calib:=true`
+7) If you are also running the Alexa code, cd to the `ADA_Talk` directory, and run:
+      a) `roslaunch rosbridge_server rosbridge_websocket.launch`
+      b) `bst proxy lambda index.js`
+8) `roslaunch libada default.launch use_forque:=true perception:=true detector:=spanet use_apriltag_calib:=true`
    * Optionally, run with `perception:=false` if you want to run perception manually in another terminal.
    * To run perception manually in another terminal: `rosrun food_detector run_perception_module.py --demo-type spanet`
-8) `roslaunch ada_feeding feeding.launch` (will quit after writing ROS parameters)
+9) `roslaunch ada_feeding feeding.launch` (will quit after writing ROS parameters)
    * Optionally run `roslaunch ada_feeding data_collection.launch` after `feeding.launch` if you're doing data collection.
-9) `cd ~/Workspace/ada_ws/devel/bin/` and `./feeding -af`
+10) `cd ~/Workspace/ada_ws/devel/bin/` and `./feeding -af`
     * `-a`: specified that this is the real robot, and not a simulation
     * `-f`: enables the force/torque sensor on the real robot (**REQUIRED when picking up food for safety**)
     * `-c`: causes demo to proceed without the user pressing \[ENTER\] between steps
     * `-d`: Select demo to run.
-10) Advance the demo by pressing Return or terminate nicely by pressing `n` and then return
+    * `-s`: Whether to run it with sound or not.
+11) Advance the demo by pressing Return or terminate nicely by pressing `n` and then return
 
 ## Running with acquisition detection
 To run with acquisition detection (and not require manual success/failure input from the supervisor) run

config/gen2_feeding_demo.yaml

@@ -56,7 +56,7 @@ wheelchair:
 feedingDemo:
   heightAbovePlate: 0.15
   heightAboveFood: 0.08
-  heightIntoFood: 0.085
+  heightIntoFood: 0.08
   moveOutofFood: 0.025
   #distanceToPerson: 0.25
   distanceToPerson: 0.05
@@ -120,15 +120,15 @@ foodItems:
   #forces: [15,3,3,7,22,15,7]
   #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]
+  forces: [10, 10, 10, 10, 7, 15, 7, 15, 7, 7, 7, 7, 15, 7, 7, 7]
   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: false
+  useAlexa: true
   foodTopic: /alexa_msgs
   actionTopic: /alexa_msgs
 

include/feeding/util.hpp

@@ -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 = 10);
+                             bool useAlexa = true, double timeout = 20);
 
 int getUserInputWithOptions(const std::vector<std::string> &optionPrompts,
                             const std::string &prompt);

src/action/FeedFoodToPerson.cpp

@@ -179,9 +179,9 @@ void feedFoodToPerson(const std::shared_ptr<Perception> &perception,
 
     nodeHandle->setParam("/feeding/facePerceptionOn", true);
 
-    if (overrideTiltOffset == nullptr) {
-      distanceToPerson = 0;
-    }
+    // if (overrideTiltOffset == nullptr) {
+    //   distanceToPerson = 0;
+    // }
 
     ROS_INFO_STREAM("Move towards person");
     moveSuccess =
@@ -277,6 +277,10 @@ void feedFoodToPerson(const std::shared_ptr<Perception> &perception,
     ada::util::waitForUser("Move backward", ada);
     if (useSound) 
       talk("Let me get out of your way.", true);
+
+    if (feedingDemo->getFTThresholdHelper())
+        feedingDemo->getFTThresholdHelper()->setThresholds(STANDARD_FT_THRESHOLD, true);
+
     Eigen::Vector3d goalDirection(0, -1, 0);
     bool success = moveInFrontOfPerson(
         ada->getArm()->getWorldCollisionConstraint(

src/action/MoveInto.cpp

@@ -92,7 +92,7 @@ bool moveInto(const std::shared_ptr<Perception> &perception, TargetItem item,
     // Collision constraint is not set because f/t sensor stops execution.
     auto trajectory =
         ada->getArm()->planToOffset(ada->getEndEffectorBodyNode()->getName(),
-                                    Eigen::Vector3d{0.0, 0.0, -heightIntoFood});
+                                    heightIntoFood * endEffectorDirection);
 
     bool success = true;
     try {

src/action/Skewer.cpp

@@ -253,6 +253,11 @@ bool skewer(const std::shared_ptr<Perception> &perception,
     // ===== INTO FOOD =====
     // if (useSound) 
     //   talk("Here we go!", true);
+
+    // Set end effector to move into food
+    endEffectorDirection = perception->getTrackedFoodItemPose().translation() - ada->getEndEffectorBodyNode()->getTransform().translation();
+    endEffectorDirection.normalize();
+
     auto moveIntoSuccess = moveInto(perception, TargetItem::FOOD,
                                     endEffectorDirection, feedingDemo);
 

src/util.cpp

@@ -200,11 +200,11 @@ std::string getInputFromTopic(std::string topic,
   ROS_INFO_STREAM("Got Input " << foodWord);
   if (validateAsFood) {
     for (std::size_t i = 0; i < FOOD_NAMES.size(); ++i) {
-      if (FOOD_NAMES[i].compare(foodWord) == 0) {
+      if (foodWord.find(FOOD_NAMES[i]) != std::string::npos) {
         ROS_INFO_STREAM("Sucessfully returned");
-        nodeHandle.setParam("/deep_pose/forceFoodName", foodWord);
-        nodeHandle.setParam("/deep_pose/spnet_food_name", foodWord);
-        return foodWord;
+        nodeHandle.setParam("/deep_pose/forceFoodName", FOOD_NAMES[i]);
+        nodeHandle.setParam("/deep_pose/spnet_food_name", FOOD_NAMES[i]);
+        return FOOD_NAMES[i];
       }
     }
     return "";