ADA Feeding Demo

This package demonstrates the use of the ADA robot for robot-assisted feeding.

It uses a Behavior Tree paradigm (v4.0).

See [trees] for a list of all demos.

See [nodes] for a list of all created CPP nodes.

Installation

ada_feeding is a ROS1 catkin package most recently tested on Ubuntu 20.04 LTS (Focal Fossa).

APT Dependencies

DISTRO=noetic
sudo apt install libopencv-dev libblas-dev liblapack-dev libmicrohttpd-dev libeigen3-dev ros-$DISTRO-control-toolbox ros-$DISTRO-ompl ros-$DISTRO-force-torque-sensor-controller ros-$DISTRO-srdfdom python3-wstool ros-$DISTRO-octomap-ros ros-$DISTRO-joint-trajectory-controller ros-$DISTRO-transmission-interface ros-$DISTRO-cv-bridge ros-$DISTRO-image-transport ros-$DISTRO-image-geometry ros-$DISTRO-diagnostic-updater ros-$DISTRO-controller-manager ros-$DISTRO-rviz python3-catkin-tools

You might also need to install pybind: pip install pybind11[global]

You should also symlink python to python3 otherwise some scripts will be unable to find the python binary, causing "No such file or directory" errors when running roslaunch:

sudo apt install python-is-python3

PRL Git Packages

We can install these all at once with wstool.

If running everything in simulation, use ada-feeding-sim (which installs fewer packages), otherwise, use ada-feeding.

$ git clone https://github.com/personalrobotics/pr-rosinstalls.git ~/pr-rosinstalls
$ cd my_catkin_workspace/src
$ wstool init # exclude if already have .rosinstall
$ wstool merge ~/pr-rosinstalls/ada-feeding[-sim].rosinstall
$ wstool up

Note that some of the directories installed with the above rosinstall file may have special dependencies that were unmentioned in the README. If you run into errors building/running specific packages, refer to the READMEs of those packages for more details.

Kinova JACO SDK (Optional: Real Robot Only)

Download the Gen2 SDK ZIP file from Kinova's Website. Install the Debian Package inside.

It should be in Ubuntu/16_04/64 bits/.

You can install it using dpkg, e.g. (for version 6.1.0):

sudo dpkg -i KinovaAPi-6.1.0-amd64.deb

Note that the current demo has only been tested on the JACO 2.

Running the Demo in Simulation

Ensure that your Workspace is built: cd <catkin_ws>; catkin build; . devel/setup.bash
Start up ROS and rviz: roscore and roslaunch ada_feeding rviz.launch
Start up simulated perception: roslaunch ada_feeding perception.launch sim:=true
Run Simulation: roslaunch ada_feeding feeding.launch sim:=true. This runs trees/feeding.xml by default.
Start up an RQT Publisher: rosrun rqt_publisher rqt_publisher
Set up publications to the following topics:
1. /watchdog: (std_mgs/Bool). A heartbeat that triggers E-Stop if it stops publishing. Publish True at 100Hz. (Note: This will start the demo)
2. /feeding/check_acquire (std_msgs/Bool). Is checked for True/False after acquisition to determine success.
3. /feeding/user_ready (std_msgs/Bool). Is checked for True for pre-transfer and after transfer to determine when to advance the demo.
4. /alexa_msgs (std_msgs/String). (Mapped from ~food_request). Is checked pre-acquisition to determine which food type to acquire.

Running the Demo on the JACO 2

Additional Workspace Setup

Build your workspace with catkin build
Download the checkpoint by going into src/pytorch_retinanet and running load_checkpoint.sh (or train your own checkpoint)
Do the same in src/bite_selection_package: run load_checkpoint.sh (or train your own checkpoint)
Make sure your source devel/setup.bash in every terminal you use.

Demo Run Steps

Ensure that your Workspace is built: cd <catkin_ws>; catkin build; . devel/setup.bash
Start up ROS and Rviz: roscore and roslaunch ada_feeding rviz.launch
Turn on and home ADA. Once the lights on the joystick go solid, home ADA by holding the orange button until the robot stops moving.
Start the Camera: ssh nano (you may need to add nano to your .ssh/config, this is the Nvidia Jetson Nano on the robot).
1. Once there, set your ROS Master using usemaster <hostname> (e.g. usemaster weebo or usemaster ed209)
2. Execute roslaunch realsense2_camera rs_aligned_depth.launch initial_reset:=true to start streaming RGBD data.
3. Note: SSH Key for Nano is available on secrets drive for convenient access
4. Check the image stream via Rviz (/camera/color/image_raw/image). If some area is too bright and look burnt or saturated, reduce the exposure.
Run F/T Sensor: roslaunch forque_sensor_hardware forque.launch (Optionally add forque_ip:=<IPv4> if your Net-FT is on a non-default IP)
Run Face Detection: rosrun face_detection face_detection
(Optional) Run Alexa code: cd to the ADA_Talk directory, and run: a) roslaunch rosbridge_server rosbridge_websocket.launch b) bst proxy lambda index.js
Start Demo Code: roslaunch ada_feeding feeding.launch sim:=false (Note: this should also set use_forque:=true and use_apriltag_calib:=true)
Start Perception: roslaunch ada_feeding perception.launch
Follows steps 5-6 of "Running the Demo in Simulation" to actually run the demo with rqt_publisher.

Other things to note

After running the demo one time, the Joystick switches from cartesian control to joint control until you restart Ada.

Compilation Troubleshooting

DLIB_NO_GUI_SUPPORT: If you get this error when building face_detection: un-comment the #define statement in /usr/include/dlib/config.h.
/usr/include/dlib/opencv/cv_image.h:37:29: error: conversion from ‘const cv::Mat’ to non-scalar type ‘IplImage’ {aka ‘_IplImage’} requested 37 | IplImage temp = img;: If you get this error when building 'face_detection': replace line 37 in /usr/include/dlib/opencv/cv_image.h with IplImage temp = cvIplImage(img);

Additional workspace notes

There are some repositories that have ada in their name but are out of date! Only the repositories in the rosinstall above should be required.
openrave is out of date and not required for this project.
Whenever you install something to fix dependencies, make sure to clean the affected repositories before you build them again!
Whenever you run something, make sure to source the setup.bash in the workspace in every terminal you use! We recommend putting it in your ~/.bashrc file.
If you have dartsim in the workspace, it might not link to libnlopt correctly and you might see an error message when compiling libada. When this happens, remove dartsim and install sudo apt-get libdart6-all-dev.

Safety notes

You can stop Ada's movement by Ctrl-C-ing feeding.launch.
Never use the joystick while the controllers (step 7) are running. Both will fight over control of Ada and they will not care about collision boxes.
Be familiar with the location of Ada's on/off-switch :)

Misc Notes

3D models and details for the Jetson Nano, which mounts the RealSense onto the gen2 arm, can be found here.

ruiiu / ada_feeding