├── .gitignore ├── .travis.rosinstall ├── fetch_pbd_interaction ├── msg │ ├── Condition.msg │ ├── OrientationRPY.msg │ ├── GripperAction.msg │ ├── WorldState.msg │ ├── Landmark.msg │ ├── ExecutionStatus.msg │ ├── ArmState.msg │ ├── RobotSound.msg │ └── SessionState.msg ├── srv │ ├── GetArmMovement.srv │ ├── GetGazeGoal.srv │ ├── GetJointStates.srv │ ├── GetGripperState.srv │ ├── Ping.srv │ ├── SetGripperState.srv │ ├── GetEEPose.srv │ ├── GetSessionState.srv │ ├── GetObjectList.srv │ ├── MoveArm.srv │ ├── ExecuteActionById.srv │ ├── GetObjectFromName.srv │ ├── MoveArmTraj.srv │ ├── GetNearestObject.srv │ ├── GetMostSimilarObject.srv │ └── GuiInput.srv ├── .gitignore ├── web_interface │ ├── .gitignore │ └── fetch-pbd-gui │ │ ├── build_frontend.sh │ │ ├── manifest.json │ │ ├── install_web_dependencies.sh │ │ ├── package.json │ │ ├── polymer.json │ │ ├── elements │ │ ├── imports.html │ │ ├── marker_icons.html │ │ ├── scripts │ │ │ ├── STLLoader.min.js │ │ │ └── eventemitter2.min.js │ │ ├── pbd-gui.html │ │ └── my-actions.html │ │ ├── .eslintrc.json │ │ ├── compiled_frontend │ │ ├── bundled-es6 │ │ │ ├── index.html │ │ │ └── elements │ │ │ │ ├── my-actions.html │ │ │ │ └── current-action.html │ │ └── polymer.json │ │ ├── index.html │ │ └── bower.json ├── sounds │ ├── ERROR.wav │ ├── OTHER.wav │ ├── SUCCESS.wav │ ├── POSE_SAVED.wav │ ├── CREATED_ACTION.wav │ ├── EXECUTION_ENDED.wav │ ├── START_TRAJECTORY.wav │ ├── ALL_POSES_DELETED.wav │ ├── MICROPHONE_WORKING.wav │ └── STARTING_EXECUTION.wav ├── scripts │ └── serve_web_interface.sh ├── config │ ├── zones.yaml │ └── fetch_pbd.rviz ├── src │ ├── fetch_pbd_interaction │ │ ├── __init__.py │ │ ├── primitive.py │ │ ├── robot.py │ │ └── arm_control.py │ └── world_landmark.cpp ├── setup.py ├── launch │ ├── pbd_rviz_viewer.launch │ ├── pbd_web_prereqs.launch │ ├── pbd.launch │ └── pbd_prereqs.launch ├── nodes │ ├── pbd_arm_control_node.py │ ├── pbd_interaction_node.py │ ├── pbd_world_node.cpp │ └── demo.py ├── include │ └── fetch_pbd_interaction │ │ ├── world_landmark.h │ │ └── world.h ├── package.xml ├── CHANGELOG.rst └── CMakeLists.txt ├── fetch_social_gaze ├── .gitignore ├── launch │ └── gaze.launch ├── action │ └── Gaze.action ├── package.xml ├── CHANGELOG.rst ├── CMakeLists.txt └── nodes │ └── social_gaze_server.py ├── fetch_arm_control ├── msg │ └── GripperState.msg ├── src │ └── fetch_arm_control │ │ └── __init__.py ├── launch │ ├── arm_interaction_frontend.launch │ └── arm_interaction_backend.launch ├── setup.py ├── nodes │ └── interactive_arm_control.py ├── package.xml ├── CHANGELOG.rst ├── CMakeLists.txt └── config │ └── fetch_arm_control.rviz ├── .travis.yml ├── LICENSE └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | -------------------------------------------------------------------------------- /.travis.rosinstall: -------------------------------------------------------------------------------- 1 | #empty 2 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/msg/Condition.msg: -------------------------------------------------------------------------------- 1 | float64 gripperPosition -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/GetArmMovement.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool moving -------------------------------------------------------------------------------- /fetch_pbd_interaction/.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | msg_gen 3 | srv_gen 4 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/GetGazeGoal.srv: -------------------------------------------------------------------------------- 1 | --- 2 | uint8 gaze_goal -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/GetJointStates.srv: -------------------------------------------------------------------------------- 1 | --- 2 | float64[] joints -------------------------------------------------------------------------------- /fetch_social_gaze/.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | msg 3 | msg_gen 4 | src 5 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/GetGripperState.srv: -------------------------------------------------------------------------------- 1 | --- 2 | uint8 gripper_state -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/Ping.srv: -------------------------------------------------------------------------------- 1 | string ping 2 | --- 3 | string pong 4 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/SetGripperState.srv: -------------------------------------------------------------------------------- 1 | uint8 gripper_state 2 | --- 3 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/msg/OrientationRPY.msg: -------------------------------------------------------------------------------- 1 | float64 r 2 | float64 p 3 | float64 y -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/GetEEPose.srv: -------------------------------------------------------------------------------- 1 | --- 2 | geometry_msgs/PoseStamped ee_pose -------------------------------------------------------------------------------- /fetch_pbd_interaction/msg/GripperAction.msg: -------------------------------------------------------------------------------- 1 | fetch_arm_control/GripperState gripper 2 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/msg/WorldState.msg: -------------------------------------------------------------------------------- 1 | fetch_pbd_interaction/Landmark[] object_list -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/GetSessionState.srv: -------------------------------------------------------------------------------- 1 | --- 2 | fetch_pbd_interaction/SessionState state -------------------------------------------------------------------------------- /fetch_arm_control/msg/GripperState.msg: -------------------------------------------------------------------------------- 1 | uint8 OPEN = 0 2 | uint8 CLOSED = 1 3 | uint8 state 4 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/GetObjectList.srv: -------------------------------------------------------------------------------- 1 | --- 2 | fetch_pbd_interaction/Landmark[] object_list -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/MoveArm.srv: -------------------------------------------------------------------------------- 1 | fetch_pbd_interaction/ArmState arm_state 2 | --- 3 | bool success -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/.gitignore: -------------------------------------------------------------------------------- 1 | bower_components 2 | dist 3 | node_modules 4 | build 5 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/ExecuteActionById.srv: -------------------------------------------------------------------------------- 1 | string action_id # ID in MongoDB database of action to execute 2 | --- 3 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/sounds/ERROR.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZebraDevs/fetch_pbd/HEAD/fetch_pbd_interaction/sounds/ERROR.wav -------------------------------------------------------------------------------- /fetch_pbd_interaction/sounds/OTHER.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZebraDevs/fetch_pbd/HEAD/fetch_pbd_interaction/sounds/OTHER.wav -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/GetObjectFromName.srv: -------------------------------------------------------------------------------- 1 | string ref_name 2 | --- 3 | bool has_object 4 | fetch_pbd_interaction/Landmark obj -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/MoveArmTraj.srv: -------------------------------------------------------------------------------- 1 | fetch_pbd_interaction/ArmState[] arm_states 2 | float64[] times 3 | --- 4 | bool success -------------------------------------------------------------------------------- /fetch_pbd_interaction/sounds/SUCCESS.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZebraDevs/fetch_pbd/HEAD/fetch_pbd_interaction/sounds/SUCCESS.wav -------------------------------------------------------------------------------- /fetch_pbd_interaction/sounds/POSE_SAVED.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZebraDevs/fetch_pbd/HEAD/fetch_pbd_interaction/sounds/POSE_SAVED.wav -------------------------------------------------------------------------------- /fetch_pbd_interaction/sounds/CREATED_ACTION.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZebraDevs/fetch_pbd/HEAD/fetch_pbd_interaction/sounds/CREATED_ACTION.wav -------------------------------------------------------------------------------- /fetch_pbd_interaction/sounds/EXECUTION_ENDED.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZebraDevs/fetch_pbd/HEAD/fetch_pbd_interaction/sounds/EXECUTION_ENDED.wav -------------------------------------------------------------------------------- /fetch_pbd_interaction/sounds/START_TRAJECTORY.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZebraDevs/fetch_pbd/HEAD/fetch_pbd_interaction/sounds/START_TRAJECTORY.wav -------------------------------------------------------------------------------- /fetch_pbd_interaction/msg/Landmark.msg: -------------------------------------------------------------------------------- 1 | string name 2 | geometry_msgs/Pose pose 3 | geometry_msgs/Vector3 dimensions 4 | sensor_msgs/PointCloud2 point_cloud -------------------------------------------------------------------------------- /fetch_pbd_interaction/sounds/ALL_POSES_DELETED.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZebraDevs/fetch_pbd/HEAD/fetch_pbd_interaction/sounds/ALL_POSES_DELETED.wav -------------------------------------------------------------------------------- /fetch_pbd_interaction/sounds/MICROPHONE_WORKING.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZebraDevs/fetch_pbd/HEAD/fetch_pbd_interaction/sounds/MICROPHONE_WORKING.wav -------------------------------------------------------------------------------- /fetch_pbd_interaction/sounds/STARTING_EXECUTION.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZebraDevs/fetch_pbd/HEAD/fetch_pbd_interaction/sounds/STARTING_EXECUTION.wav -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/GetNearestObject.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped pose 2 | --- 3 | bool has_nearest 4 | fetch_pbd_interaction/Landmark nearest_object -------------------------------------------------------------------------------- /fetch_arm_control/src/fetch_arm_control/__init__.py: -------------------------------------------------------------------------------- 1 | #autogenerated by ROS python message generators 2 | from .arm import Arm 3 | from .arm_control_marker import ArmControlMarker -------------------------------------------------------------------------------- /fetch_social_gaze/launch/gaze.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/GetMostSimilarObject.srv: -------------------------------------------------------------------------------- 1 | fetch_pbd_interaction/Landmark original_object 2 | --- 3 | bool has_similar 4 | fetch_pbd_interaction/Landmark similar_object 5 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/build_frontend.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | ./node_modules/polymer-cli/bin/polymer.js build 3 | rm -r compiled_frontend 4 | mv build compiled_frontend 5 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/scripts/serve_web_interface.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | path=`rospack find fetch_pbd_interaction` 3 | cd $path/web_interface/fetch-pbd-gui/compiled_frontend/bundled-es6 4 | python -m SimpleHTTPServer 8080 5 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/msg/ExecutionStatus.msg: -------------------------------------------------------------------------------- 1 | uint8 PREEMPTED = 0 2 | uint8 SUCCEEDED = 1 3 | uint8 NO_IK = 2 4 | uint8 OBSTRUCTED = 3 5 | uint8 NOT_EXECUTING = 4 6 | uint8 EXECUTING = 5 7 | uint8 CONDITION_ERROR = 6 8 | uint8 status -------------------------------------------------------------------------------- /fetch_pbd_interaction/msg/ArmState.msg: -------------------------------------------------------------------------------- 1 | uint8 ROBOT_BASE=0 2 | uint8 FIXED_LINK=1 3 | uint8 PREVIOUS_TARGET=2 4 | uint8 OBJECT=3 5 | uint8 ref_type 6 | geometry_msgs/PoseStamped ee_pose 7 | float64[] joint_pose 8 | float64[] velocities 9 | Landmark ref_landmark -------------------------------------------------------------------------------- /fetch_pbd_interaction/config/zones.yaml: -------------------------------------------------------------------------------- 1 | - name: "Table Zone" 2 | remove_surface: true 3 | parent_frame_id: "base_link" 4 | child_frame_id: "head_camera_depth_frame" 5 | bounding_frame_id: "base_link" 6 | segmentation_frame_id: "base_link" 7 | z_min: 0.10 -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/manifest.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Fetch Programming by Demonstration", 3 | "short_name": "Fetch Programming by Demonstration", 4 | "start_url": "/", 5 | "display": "standalone", 6 | "background_color": "#3f51b5", 7 | "theme_color": "#3f51b5" 8 | } 9 | -------------------------------------------------------------------------------- /fetch_social_gaze/action/Gaze.action: -------------------------------------------------------------------------------- 1 | uint8 LOOK_FORWARD = 0 2 | uint8 FOLLOW_EE = 1 3 | uint8 GLANCE_EE = 2 4 | uint8 NOD = 3 5 | uint8 SHAKE = 4 6 | uint8 FOLLOW_FACE = 5 7 | uint8 LOOK_AT_POINT = 6 8 | uint8 LOOK_DOWN = 7 9 | uint8 NONE = 8 10 | uint8 action 11 | geometry_msgs/Point point 12 | uint8 repeat 13 | --- 14 | --- 15 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/src/fetch_pbd_interaction/__init__.py: -------------------------------------------------------------------------------- 1 | from .interaction import Interaction 2 | from .session import Session 3 | from .arm_control import ArmControl 4 | from .robot import Robot 5 | from .primitive import Primitive 6 | from .arm_trajectory import ArmTrajectory 7 | from .arm_target import ArmTarget 8 | from .grasp import Grasp 9 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/install_web_dependencies.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | curl -o- https://raw.githubusercontent.com/creationix/nvm/v0.31.6/install.sh | bash 3 | export NVM_DIR="$HOME/.nvm" 4 | [ -s "$NVM_DIR/nvm.sh" ] && . "$NVM_DIR/nvm.sh" 5 | nvm install node 6 | npm install bower 7 | npm install polymer-cli@1.3.1 8 | ./node_modules/bower/bin/bower install 9 | -------------------------------------------------------------------------------- /fetch_arm_control/launch/arm_interaction_frontend.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /fetch_arm_control/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['fetch_arm_control'], 9 | package_dir={'': 'src'}) 10 | 11 | setup(**setup_args) 12 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['fetch_pbd_interaction'], 9 | package_dir={'': 'src'}) 10 | 11 | setup(**setup_args) 12 | -------------------------------------------------------------------------------- /fetch_arm_control/launch/arm_interaction_backend.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/package.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "fetch-pbd-gui", 3 | "version": "1.0.0", 4 | "description": "", 5 | "main": "index.js", 6 | "scripts": { 7 | "test": "echo \"Error: no test specified\" && exit 1", 8 | "eslint": "eslint" 9 | }, 10 | "author": "", 11 | "license": "ISC", 12 | "dependencies": { 13 | "bower": "^1.8.0", 14 | "polymer-cli": "^1.3.1" 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/msg/RobotSound.msg: -------------------------------------------------------------------------------- 1 | string ALL_POSES_DELETED = ALL_POSES_DELETED 2 | string ERROR = ERROR 3 | string MICROPHONE_WORKING = MICROPHONE_WORKING 4 | string POSE_SAVED = POSE_SAVED 5 | string START_TRAJECTORY = START_TRAJECTORY 6 | string CREATED_ACTION = CREATED_ACTION 7 | string EXECUTION_ENDED = EXECUTION_ENDED 8 | string OTHER = OTHER 9 | string STARTING_EXECUTION = STARTING_EXECUTION 10 | string SUCCESS = SUCCESS 11 | string sound -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/polymer.json: -------------------------------------------------------------------------------- 1 | { 2 | "entrypoint": "index.html", 3 | "shell": "elements/pbd-gui.html", 4 | "fragments": [ 5 | "elements/my-actions.html", 6 | "elements/current-action.html" 7 | ], 8 | "builds": [{ 9 | "name": "bundled-es6", 10 | "bundle": true, 11 | "js": {"minify": true, "compile": false}, 12 | "css": {"minify": true}, 13 | "html": {"minify": true} 14 | }] 15 | } 16 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/msg/SessionState.msg: -------------------------------------------------------------------------------- 1 | uint8 n_actions 2 | int8 current_action 3 | uint8 n_primitives 4 | int8 current_primitive 5 | string[] action_names 6 | string[] ref_frames 7 | string[] primitive_names 8 | bool[] actions_disabled 9 | bool[] marker_visibility 10 | bool[] primitive_pose_editable 11 | std_msgs/ColorRGBA[] primitive_colors 12 | fetch_pbd_interaction/Landmark[] objects 13 | geometry_msgs/Point[] positions 14 | fetch_pbd_interaction/OrientationRPY[] orientations -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/elements/imports.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | language: generic 4 | notifications: 5 | email: 6 | on_success: change 7 | on_failure: always 8 | env: 9 | global: 10 | - ROS_REPO=ros 11 | - NOT_TEST_INSTALL=true 12 | matrix: 13 | - ROS_DISTRO=indigo UPSTREAM_WORKSPACE=file 14 | - ROS_DISTRO=indigo UPSTREAM_WORKSPACE=debian 15 | matrix: 16 | allow_failures: 17 | - env: ROS_DISTRO=indigo UPSTREAM_WORKSPACE=debian 18 | install: 19 | - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config 20 | script: 21 | - .ci_config/travis.sh 22 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/.eslintrc.json: -------------------------------------------------------------------------------- 1 | { 2 | "env": { 3 | "browser": true 4 | }, 5 | "plugins": [ 6 | "html" 7 | ], 8 | "extends": "eslint:recommended", 9 | "rules": { 10 | "indent": [ 11 | "error", 12 | 2 13 | ], 14 | "linebreak-style": [ 15 | "error", 16 | "unix" 17 | ], 18 | "quotes": [ 19 | "error", 20 | "single" 21 | ], 22 | "semi": [ 23 | "error", 24 | "always" 25 | ] 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/compiled_frontend/bundled-es6/index.html: -------------------------------------------------------------------------------- 1 | Fetch Programming by Demonstration -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/compiled_frontend/polymer.json: -------------------------------------------------------------------------------- 1 | { 2 | "entrypoint": "index.html", 3 | "shell": "elements/pbd-gui.html", 4 | "fragments": [ 5 | "elements/my-actions.html", 6 | "elements/current-action.html" 7 | ], 8 | "sources": [ 9 | "src/**/*", 10 | "index.html", 11 | "elements/pbd-gui.html", 12 | "elements/my-actions.html", 13 | "elements/current-action.html" 14 | ], 15 | "extraDependencies": [ 16 | "bower_components/webcomponentsjs/*.js" 17 | ], 18 | "builds": [ 19 | { 20 | "name": "bundled-es6", 21 | "bundle": true, 22 | "js": { 23 | "minify": true, 24 | "compile": false 25 | }, 26 | "css": { 27 | "minify": true 28 | }, 29 | "html": { 30 | "minify": true 31 | } 32 | } 33 | ] 34 | } -------------------------------------------------------------------------------- /fetch_arm_control/nodes/interactive_arm_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | '''Node to control robot's arms using interactive markers''' 3 | 4 | # ###################################################################### 5 | # Imports 6 | # ###################################################################### 7 | 8 | # Core ROS imports come first. 9 | import rospy 10 | 11 | # ROS builtins 12 | from tf import TransformListener 13 | 14 | # Local 15 | from fetch_arm_control.arm import Arm 16 | from fetch_arm_control.arm_control_marker import ArmControlMarker 17 | 18 | if __name__ == '__main__': 19 | 20 | # Register as a ROS node. 21 | rospy.init_node('fetch_arm_interaction', anonymous=True) 22 | 23 | # Run the system 24 | tf_listener = TransformListener() 25 | arm = Arm(tf_listener) 26 | marker = ArmControlMarker(arm) 27 | rospy.spin() -------------------------------------------------------------------------------- /fetch_pbd_interaction/launch/pbd_rviz_viewer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /fetch_social_gaze/package.xml: -------------------------------------------------------------------------------- 1 | 2 | fetch_social_gaze 3 | 0.0.8 4 | The fetch_social_gaze package 5 | 6 | Sarah Elliott 7 | Sarah Elliott 8 | Russell Toris 9 | BSD 10 | 11 | catkin 12 | rospy 13 | std_msgs 14 | actionlib 15 | geometry_msgs 16 | geometry_msgs 17 | actionlib 18 | std_msgs 19 | rospy 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/index.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | Fetch Programming by Demonstration 7 | 8 | 22 | 23 | 24 | 25 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /fetch_arm_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fetch_arm_control 4 | 0.0.8 5 | The fetch_arm_control package 6 | Sarah Elliott 7 | Sarah Elliott 8 | Russell Toris 9 | BSD 10 | 11 | catkin 12 | roscpp 13 | rospy 14 | std_msgs 15 | geometry_msgs 16 | message_generation 17 | actionlib 18 | tf 19 | std_msgs 20 | geometry_msgs 21 | rospy 22 | tf 23 | message_runtime 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/nodes/pbd_arm_control_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | '''Provides a service interface to control the robot's arm 3 | in the context of PbD. 4 | ''' 5 | # ###################################################################### 6 | # Imports 7 | # ###################################################################### 8 | 9 | # Core ROS imports come first. 10 | import rospy 11 | 12 | # Local 13 | from fetch_pbd_interaction import ArmControl 14 | 15 | # ###################################################################### 16 | # Main Function 17 | # ###################################################################### 18 | 19 | if __name__ == '__main__': 20 | 21 | # Register as a ROS node. 22 | rospy.init_node('fetch_arm_control', anonymous=True) 23 | 24 | arm_control = ArmControl() 25 | 26 | while(not rospy.is_shutdown()): 27 | arm_control.update() 28 | # This is the pause between update runs. Note that this doesn't 29 | # guarantee an update rate, only that there is this amount of 30 | # pause between udpates. 31 | rospy.sleep(0.1) 32 | -------------------------------------------------------------------------------- /fetch_social_gaze/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package fetch_social_gaze 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.8 (2017-08-08) 6 | ------------------ 7 | * Do reachability check for ref_type PREVIOUS_TARGET 8 | * Namespace topics and services, some marker fixes 9 | * Contributors: Sarah Elliott 10 | 11 | 0.0.7 (2017-04-20) 12 | ------------------ 13 | 14 | 0.0.6 (2016-10-29) 15 | ------------------ 16 | 17 | 0.0.5 (2016-09-17) 18 | ------------------ 19 | 20 | 0.0.4 (2016-09-09) 21 | ------------------ 22 | 23 | 0.0.3 (2016-09-09) 24 | ------------------ 25 | 26 | 0.0.2 (2016-09-09) 27 | ------------------ 28 | 29 | 0.0.1 (2016-09-09) 30 | ------------------ 31 | * Update package.xml 32 | * License and documentation updates 33 | * Getting ready to be released 34 | * Added markers being relative to each other 35 | * Huge reorganisation. Now working. 36 | * Huge reorganisation. Now working. 37 | * Cleaning up code 38 | * Working, all functionality of PR2 PbD 39 | * First commit of port of PbD to Fetch. Things work. Rough around the edges. 40 | * Contributors: Russell Toris, Sarah Elliott 41 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/launch/pbd_web_prereqs.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /fetch_arm_control/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package fetch_arm_control 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.8 (2017-08-08) 6 | ------------------ 7 | * Namespace topics and services, some marker fixes 8 | * Change World node to C++ and add Grasp suggestion integration 9 | * Contributors: Sarah Elliott 10 | 11 | 0.0.7 (2017-04-20) 12 | ------------------ 13 | * Make the gripper open all the way 14 | * Contributors: Sarah Elliott 15 | 16 | 0.0.6 (2016-10-29) 17 | ------------------ 18 | 19 | 0.0.5 (2016-09-17) 20 | ------------------ 21 | 22 | 0.0.4 (2016-09-09) 23 | ------------------ 24 | * Dependencies again 25 | * Contributors: Sarah Elliott 26 | 27 | 0.0.3 (2016-09-09) 28 | ------------------ 29 | * More dependency fixing 30 | * Contributors: Sarah Elliott 31 | 32 | 0.0.2 (2016-09-09) 33 | ------------------ 34 | * Dependencies 35 | * Contributors: Sarah Elliott 36 | 37 | 0.0.1 (2016-09-09) 38 | ------------------ 39 | * Update package.xml 40 | * Getting ready to be released 41 | * Marker UI fixes 42 | * Fixes based on Russell's comments 43 | * Documentation and code cleanup 44 | * Make my own sortable list 45 | * Add back the standalone arm control node 46 | * Huge reorganisation. Now working. 47 | * Huge reorganisation. Now working. 48 | * Changed ArmControls class to ArmInteraction, fixed bug in PbD 49 | * Make fake Interactive Manipulation work 50 | * Working, all functionality of PR2 PbD 51 | * First commit of port of PbD to Fetch. Things work. Rough around the edges. 52 | * Contributors: Russell Toris, Sarah Elliott 53 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/srv/GuiInput.srv: -------------------------------------------------------------------------------- 1 | # Action Creation/Navigation 2 | string CREATE_ACTION = create-action 3 | string SWITCH_TO_ACTION = switch-to-action 4 | string NEXT_ACTION = next-action 5 | string PREV_ACTION = previous-action 6 | string UPDATE_ACTION_NAME = update-action-name 7 | string DELETE_CURRENT_ACTION = delete-current-action 8 | string DELETE_ACTION = delete-action 9 | string COPY_ACTION = copy-action 10 | 11 | # Primitive Creation/Navigation 12 | string SWITCH_PRIMITIVE_ORDER = switch-primitive-order 13 | string DELETE_PRIMITIVE = delete-primitive 14 | string COPY_PRIMITIVE = copy-primitive 15 | string SELECT_PRIMITIVE = select-primitive 16 | string DELETE_ALL_PRIMITIVES = delete-all-primitives 17 | string DELETE_LAST_PRIMITIVE = delete-last-primitive 18 | string HIDE_PRIMITIVE_MARKER = hide-primitive-marker 19 | string SHOW_PRIMITIVE_MARKER = show-primitive-marker 20 | 21 | # Programming 22 | string OPEN_HAND = open-hand 23 | string CLOSE_HAND = close-hand 24 | 25 | string RECORD_OBJECTS= record-objects 26 | string SAVE_TARGET = save-target 27 | string ADD_GRASP = add-grasp 28 | 29 | string START_RECORDING_TRAJECTORY = start-recording-trajectory 30 | string STOP_RECORDING_TRAJECTORY = stop-recording-trajectory 31 | 32 | string POSE_EDITED = pose-edited 33 | 34 | # Execution 35 | string EXECUTE_ACTION = execute-action 36 | string STOP_EXECUTION = stop-execution 37 | string EXECUTE_PRIMITIVE = execute-primitive 38 | 39 | # Message fields 40 | string command 41 | string param 42 | uint16[] list_params 43 | geometry_msgs/Point position 44 | fetch_pbd_interaction/OrientationRPY orientation 45 | --- -------------------------------------------------------------------------------- /fetch_pbd_interaction/include/fetch_pbd_interaction/world_landmark.h: -------------------------------------------------------------------------------- 1 | #ifndef PBD_WORLD_LANDMARK 2 | #define PBD_WORLD_LANDMARK 3 | 4 | 5 | // System includes 6 | #include 7 | 8 | // ROS builtins 9 | #include "ros/ros.h" 10 | #include 11 | #include 12 | #include "visualization_msgs/InteractiveMarkerFeedback.h" 13 | #include "visualization_msgs/InteractiveMarker.h" 14 | 15 | // Local 16 | #include "fetch_pbd_interaction/Landmark.h" 17 | 18 | namespace fetch_pbd_interaction { 19 | 20 | class World; 21 | 22 | class WorldLandmark { 23 | 24 | private: 25 | //void (World::*_removeObjectCallback)(std::string name); 26 | //void (World::*_generateGraspsCallback)(std::string name); 27 | sensor_msgs::PointCloud2 point_cloud; 28 | int index; 29 | std::string assigned_name; 30 | bool is_removed; 31 | fetch_pbd_interaction::World *world; 32 | 33 | public: 34 | WorldLandmark(geometry_msgs::Pose pose, int ind, geometry_msgs::Vector3 dims, 35 | sensor_msgs::PointCloud2 pc); // World *world, void (World::*removeObject)(std::string name), void (World::*generateGrasps)(std::string name)); 36 | ~WorldLandmark(); 37 | std::string getName(); 38 | void remove(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 39 | void getGrasps(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 40 | visualization_msgs::InteractiveMarker int_marker; 41 | interactive_markers::MenuHandler menu_handler; 42 | 43 | fetch_pbd_interaction::Landmark object; 44 | 45 | 46 | }; 47 | 48 | }; 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, Fetch Robotics, Inc 2 | Copyright (c) 2013-2016, University of Washington 3 | Copyright (c) 2012-2013, Willow Garage, Inc 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/bower.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "fetch-pbd-gui", 3 | "authors": [ 4 | "Sarah Elliott" 5 | ], 6 | "private": true, 7 | "dependencies": { 8 | "hydrolysis": "Polymer/hydrolysis#^1.18", 9 | "iron-a11y-announcer": "PolymerElements/iron-a11y-announcer#^1.0.0", 10 | "iron-ajax": "PolymerElements/iron-ajax#^1.0.0", 11 | "iron-autogrow-textarea": "PolymerElements/iron-autogrow-textarea#^1.0.0", 12 | "iron-component-page": "PolymerElements/iron-component-page#^1.0.0", 13 | "iron-dropdown": "PolymerElements/iron-dropdown#^1.0.0", 14 | "iron-input": "PolymerElements/iron-input#^1.0.0", 15 | "paper-checkbox": "PolymerElements/paper-checkbox#^1.1.3", 16 | "paper-drawer-panel": "PolymerElements/paper-drawer-panel#^1.0.9", 17 | "paper-dropdown-menu": "PolymerElements/paper-dropdown-menu#^1.2.1", 18 | "paper-fab": "PolymerElements/paper-fab#^1.2.0", 19 | "paper-item": "PolymerElements/paper-item#^1.2.1", 20 | "paper-listbox": "polymerelements/paper-listbox#^1.1.2", 21 | "paper-menu": "PolymerElements/paper-menu#^1.2.2", 22 | "paper-radio-button": "PolymerElements/paper-radio-button#^1.2.0", 23 | "paper-radio-group": "PolymerElements/paper-radio-group#^1.2.0", 24 | "paper-tooltip": "PolymerElements/paper-tooltip#^1.1.2", 25 | "app-route": "PolymerElements/app-route#^0.9.2", 26 | "app-layout": "^0.10.2", 27 | "paper-icon-button": "^1.1.2", 28 | "iron-pages": "^1.0.8", 29 | "paper-dialog": "^1.1.0" 30 | }, 31 | "devDependencies": { 32 | "web-component-tester": "^4.0.0" 33 | }, 34 | "resolutions": { 35 | "chai": "^3.2.0" 36 | }, 37 | "description": "", 38 | "main": "", 39 | "license": "MIT", 40 | "homepage": "", 41 | "ignore": [ 42 | "**/.*", 43 | "node_modules", 44 | "bower_components", 45 | "test", 46 | "tests" 47 | ] 48 | } 49 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/nodes/pbd_interaction_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | '''This runs the "core" of the PbD backend. 3 | It handles the interactions between the GUI and the state of the world 4 | and recorded actions. 5 | It coordinates responses from the head and arms of the robot. 6 | ''' 7 | 8 | # ###################################################################### 9 | # Imports 10 | # ###################################################################### 11 | 12 | # Core ROS imports come first. 13 | import rospy 14 | 15 | # Local 16 | from fetch_pbd_interaction import Interaction 17 | 18 | # ###################################################################### 19 | # Main Function 20 | # ###################################################################### 21 | 22 | if __name__ == '__main__': 23 | 24 | # Register as a ROS node. 25 | rospy.init_node('interaction', anonymous=True) 26 | grasp_suggestion_service = rospy.get_param('~grasp_suggestion_service') 27 | external_ee_link = rospy.get_param('~grasp_suggestion_ee_link') 28 | grasp_feedback_topic = rospy.get_param('~grasp_feedback_topic') 29 | to_file = rospy.get_param('~to_file') 30 | from_file = rospy.get_param('~from_file') 31 | play_sound = rospy.get_param('~play_sound') 32 | social_gaze = rospy.get_param('~social_gaze') 33 | 34 | # Run the system 35 | interaction = Interaction(grasp_suggestion_service, 36 | grasp_feedback_topic, 37 | external_ee_link, 38 | to_file, from_file, 39 | play_sound, social_gaze) 40 | 41 | while not rospy.is_shutdown(): 42 | interaction.update() 43 | 44 | # This is the pause between update runs. Note that this doesn't 45 | # guarantee an update rate, only that there is this amount of 46 | # pause between udpates. 47 | rospy.sleep(0.1) 48 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/elements/marker_icons.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | 10 | 12 | 15 | 19 | 20 | 22 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/src/world_landmark.cpp: -------------------------------------------------------------------------------- 1 | #include "fetch_pbd_interaction/world_landmark.h" 2 | #include "fetch_pbd_interaction/world.h" 3 | 4 | namespace fetch_pbd_interaction { 5 | 6 | WorldLandmark::WorldLandmark(geometry_msgs::Pose pose, int ind, geometry_msgs::Vector3 dimensions, 7 | sensor_msgs::PointCloud2 pc){// World *world, void (World::*removeObject)(std::string name), void (World::*generateGrasps)(std::string name)){ 8 | 9 | // _removeObjectCallback = removeObject; 10 | // _generateGraspsCallback = generateGrasps; 11 | point_cloud = pc; 12 | 13 | assigned_name = ""; 14 | 15 | index = ind; 16 | 17 | object = fetch_pbd_interaction::Landmark(); 18 | object.name = getName(); 19 | object.pose = pose; 20 | object.dimensions = dimensions; 21 | object.point_cloud = point_cloud; 22 | // menu_handler = interactive_markers::MenuHandler(); 23 | int_marker = visualization_msgs::InteractiveMarker(); 24 | is_removed = false; 25 | // this->world = world; 26 | // menu_handler.insert("Remove from scene", &WorldLandmark::remove, this); 27 | // menu_handler.insert("Generate grasps for object", &WorldLandmark::getGrasps, this); 28 | // menu_handler.insert("Remove from scene", boost::bind(&WorldLandmark::remove, this, _1)); 29 | // menu_handler.insert("Generate grasps for object", boost::bind(&WorldLandmark::getGrasps, this, _1)); 30 | 31 | } 32 | 33 | WorldLandmark::~WorldLandmark() {} 34 | 35 | std::string WorldLandmark::getName(){ 36 | if (assigned_name == "") { 37 | std::stringstream sstm; 38 | sstm << "thing " << index; 39 | return sstm.str(); 40 | } 41 | else { 42 | return assigned_name; 43 | } 44 | } 45 | void WorldLandmark::remove(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){ 46 | ROS_INFO("Will remove object"); //, getName().c_str()); 47 | // ((world)->*_removeObjectCallback)(object.name); 48 | 49 | } 50 | void WorldLandmark::getGrasps(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){ 51 | ROS_INFO("Generating grasps for object"); //: %s", getName().c_str()); 52 | // ((world)->*_generateGraspsCallback)(object.name); 53 | } 54 | 55 | 56 | } -------------------------------------------------------------------------------- /fetch_pbd_interaction/nodes/pbd_world_node.cpp: -------------------------------------------------------------------------------- 1 | #include "fetch_pbd_interaction/world.h" 2 | 3 | using fetch_pbd_interaction::World; 4 | 5 | int main(int argc, char **argv) 6 | { 7 | ros::init(argc, argv, "pbd_world_node"); 8 | ros::NodeHandle n, pn("~"); 9 | ros::AsyncSpinner spinner(2); 10 | spinner.start(); 11 | std::string grasp_suggestion_service; 12 | std::string segmentation_service_name; 13 | std::string segmented_objects_topic_name; 14 | std::string segmented_table_topic_name; 15 | std::string planning_scene_topic; 16 | float obj_similar_dist_threshold; 17 | float obj_add_dist_threshold; 18 | float obj_nearest_dist_threshold; 19 | float obj_dist_zero_clamp; 20 | float text_height; 21 | float surface_height; 22 | float text_offset; 23 | std::string base_frame; 24 | 25 | pn.getParam("grasp_suggestion_service", grasp_suggestion_service); 26 | pn.getParam("segmentation_service", segmentation_service_name); 27 | pn.getParam("segmented_objects_topic", segmented_objects_topic_name); 28 | pn.getParam("segmented_table_topic", segmented_table_topic_name); 29 | pn.getParam("planning_scene_topic", planning_scene_topic); 30 | pn.getParam("object_similar_distance_threshold", obj_similar_dist_threshold); 31 | pn.getParam("object_add_distance_threshold", obj_add_dist_threshold); 32 | pn.getParam("object_nearest_distance_threshold", obj_nearest_dist_threshold); 33 | pn.getParam("object_distance_zero_clamp", obj_dist_zero_clamp); 34 | pn.getParam("text_height", text_height); 35 | pn.getParam("surface_height", surface_height); 36 | pn.getParam("text_offset", text_offset); 37 | pn.getParam("base_frame", base_frame); 38 | 39 | World world(n, pn, grasp_suggestion_service, 40 | segmentation_service_name, segmented_objects_topic_name, segmented_table_topic_name, 41 | planning_scene_topic, obj_similar_dist_threshold, obj_add_dist_threshold, 42 | obj_nearest_dist_threshold, 43 | obj_dist_zero_clamp, text_height, surface_height, text_offset, base_frame); 44 | ros::Rate r(10); 45 | while (ros::ok()) 46 | { 47 | world.update(); 48 | r.sleep(); 49 | } 50 | 51 | return 0; 52 | } 53 | -------------------------------------------------------------------------------- /fetch_arm_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html 2 | # Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html 3 | cmake_minimum_required(VERSION 2.8.3) 4 | project(fetch_arm_control) 5 | # Load catkin and all dependencies required for this package 6 | # TODO: remove all from COMPONENTS that are not catkin packages. 7 | find_package(catkin REQUIRED COMPONENTS 8 | rospy 9 | actionlib 10 | std_msgs 11 | geometry_msgs 12 | tf 13 | message_generation 14 | ) 15 | 16 | # include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) 17 | # CATKIN_MIGRATION: removed during catkin migration 18 | # cmake_minimum_required(VERSION 2.4.6) 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | catkin_python_setup() 24 | 25 | # Generate messages and services 26 | add_message_files( 27 | FILES 28 | GripperState.msg 29 | ) 30 | 31 | ## Generate added messages and services with any dependencies listed here 32 | generate_messages( 33 | DEPENDENCIES 34 | std_msgs 35 | geometry_msgs 36 | tf 37 | ) 38 | 39 | # catkin_package parameters: http://ros.org/doc/groovy/api/catkin/html/dev_guide/generated_cmake_api.html#catkin-package 40 | # TODO: fill in what other packages will need to use this package 41 | catkin_package( 42 | INCLUDE_DIRS src/fetch_arm_control 43 | #LIBRARIES pr2_arm_control 44 | CATKIN_DEPENDS rospy 45 | #DEPENDS system_lib 46 | ) 47 | 48 | ########### 49 | ## Build ## 50 | ########### 51 | 52 | ## Specify additional locations of header files 53 | ## Your package locations should be listed before other locations 54 | include_directories( 55 | ${catkin_INCLUDE_DIRS} 56 | ) 57 | 58 | ############# 59 | ## Install ## 60 | ############# 61 | 62 | install(PROGRAMS 63 | nodes/interactive_arm_control.py 64 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 65 | ) 66 | 67 | install(FILES 68 | launch/arm_interaction_backend.launch 69 | launch/arm_interaction_frontend.launch 70 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 71 | ) -------------------------------------------------------------------------------- /fetch_pbd_interaction/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fetch_pbd_interaction 4 | 0.0.8 5 | The fetch_pbd_interaction package 6 | 7 | Sarah Elliott 8 | Sarah Elliott 9 | Russell Toris 10 | 11 | BSD 12 | 13 | catkin 14 | git 15 | geometry_msgs 16 | std_msgs 17 | actionlib 18 | message_generation 19 | fetch_arm_control 20 | roscpp 21 | rospy 22 | rosbridge_server 23 | rospy_message_converter 24 | interactive_marker_proxy 25 | interactive_markers 26 | tf2_web_republisher 27 | moveit_commander 28 | python-couchdb 29 | rail_segmentation 30 | rail_manipulation_msgs 31 | moveit_ros_planning 32 | moveit_ros_planning_interface 33 | moveit_msgs 34 | roscpp 35 | pcl_ros 36 | geometry_msgs 37 | message_runtime 38 | fetch_arm_control 39 | rospy 40 | rosbridge_server 41 | rospy_message_converter 42 | std_msgs 43 | tf 44 | interactive_marker_proxy 45 | interactive_markers 46 | tf2_web_republisher 47 | moveit_commander 48 | python-couchdb 49 | couchdb 50 | rail_segmentation 51 | rail_manipulation_msgs 52 | ros_numpy 53 | moveit_ros_planning 54 | moveit_ros_planning 55 | moveit_msgs 56 | roscpp 57 | pcl_ros 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/src/fetch_pbd_interaction/primitive.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | '''Abstract class defining an interface to primitives 3 | ''' 4 | 5 | from abc import ABCMeta, abstractmethod 6 | 7 | class Primitive: 8 | __metaclass__ = ABCMeta 9 | 10 | @abstractmethod 11 | def __init__(self): 12 | pass 13 | 14 | @abstractmethod 15 | def build_from_json(self, json): 16 | pass 17 | 18 | @abstractmethod 19 | def get_json(self): 20 | pass 21 | 22 | @abstractmethod 23 | def check_pre_condition(self): 24 | pass 25 | 26 | @abstractmethod 27 | def check_post_condition(self): 28 | pass 29 | 30 | @abstractmethod 31 | def add_marker_callbacks(self, click_cb, 32 | delete_cb, 33 | pose_change_cb, 34 | action_change_cb): 35 | pass 36 | 37 | @abstractmethod 38 | def show_marker(self): 39 | pass 40 | 41 | @abstractmethod 42 | def hide_marker(self): 43 | pass 44 | 45 | @abstractmethod 46 | def marker_visible(self): 47 | pass 48 | 49 | @abstractmethod 50 | def update_ref_frames(self): 51 | pass 52 | 53 | @abstractmethod 54 | def change_ref_frame(self, ref_type, landmark): 55 | pass 56 | 57 | @abstractmethod 58 | def select(self, is_selected): 59 | pass 60 | 61 | @abstractmethod 62 | def is_selected(self): 63 | pass 64 | 65 | @abstractmethod 66 | def is_control_visible(self): 67 | pass 68 | 69 | @abstractmethod 70 | def set_control_visible(self, visible=True): 71 | pass 72 | 73 | @abstractmethod 74 | def update_viz(self, check_reachable=True): 75 | pass 76 | 77 | @abstractmethod 78 | def get_primitive_number(self): 79 | pass 80 | 81 | @abstractmethod 82 | def set_primitive_number(self, number): 83 | pass 84 | 85 | @abstractmethod 86 | def is_object_required(self): 87 | pass 88 | 89 | @abstractmethod 90 | def execute(self): 91 | pass 92 | 93 | @abstractmethod 94 | def head_busy(self): 95 | pass 96 | 97 | @abstractmethod 98 | def is_reachable(self): 99 | pass 100 | 101 | @abstractmethod 102 | def get_relative_pose(self, use_final=True): 103 | pass 104 | 105 | @abstractmethod 106 | def get_absolute_pose(self): 107 | pass 108 | 109 | @abstractmethod 110 | def get_absolute_marker_pose(self, use_final=True): 111 | pass 112 | 113 | @abstractmethod 114 | def get_absolute_marker_position(self, use_final=True): 115 | pass 116 | 117 | @abstractmethod 118 | def decrease_id(self): 119 | pass 120 | 121 | @abstractmethod 122 | def set_name(self, name): 123 | pass 124 | 125 | @abstractmethod 126 | def get_name(self): 127 | pass 128 | 129 | @abstractmethod 130 | def get_number(self): 131 | pass 132 | 133 | @abstractmethod 134 | def set_pose(self, pose): 135 | pass 136 | 137 | @abstractmethod 138 | def pose_editable(self): 139 | pass 140 | 141 | @abstractmethod 142 | def get_ref_type(self): 143 | pass 144 | 145 | 146 | 147 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/launch/pbd.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/launch/pbd_prereqs.launch: -------------------------------------------------------------------------------- 1 | 2 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/nodes/demo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | '''This runs the "core" of the PbD backend. 3 | It handles the interactions between the GUI and the state of the world 4 | and recorded actions. 5 | It coordinates responses from the head and arms of the robot. 6 | ''' 7 | 8 | # ###################################################################### 9 | # Imports 10 | # ###################################################################### 11 | 12 | # Core ROS imports come first. 13 | import rospy 14 | 15 | # ROS builtins 16 | from visualization_msgs.msg import MarkerArray 17 | from interactive_markers.interactive_marker_server import \ 18 | InteractiveMarkerServer 19 | from tf import TransformListener 20 | import rospkg 21 | 22 | # Local 23 | from fetch_arm_control.msg import GripperState 24 | from fetch_pbd_interaction.session import Session 25 | from fetch_pbd_interaction.msg import ExecutionStatus 26 | from fetch_pbd_interaction.srv import Ping, PingResponse, GetObjectList 27 | from fetch_pbd_interaction.msg import RobotSound, WorldState 28 | from fetch_pbd_interaction.robot import Robot 29 | from std_msgs.msg import String 30 | from std_srvs.srv import Empty 31 | 32 | # ###################################################################### 33 | # Module level constants 34 | # ###################################################################### 35 | 36 | EXECUTION_Z_OFFSET = -0.00 37 | BASE_LINK = 'base_link' 38 | TOPIC_IM_SERVER = 'programmed_actions' 39 | 40 | 41 | # ###################################################################### 42 | # Main Function 43 | # ###################################################################### 44 | 45 | if __name__ == '__main__': 46 | 47 | # Register as a ROS node. 48 | rospy.init_node('fetch_session_interface_demo', anonymous=True) 49 | 50 | # Run the system 51 | tf_listener = TransformListener() 52 | robot = Robot(tf_listener) 53 | im_server = InteractiveMarkerServer(TOPIC_IM_SERVER) 54 | # Get path to example json file 55 | rospack = rospkg.RosPack() 56 | file_path = rospack.get_path('fetch_pbd_interaction') + \ 57 | "/examples/example_actions.json" 58 | session = Session(robot, tf_listener, 59 | im_server, from_file=file_path) 60 | 61 | if session.n_actions() > 0: 62 | rospy.loginfo("There are {} actions.".format(session.n_actions())) 63 | 64 | # Select first action 65 | session.switch_to_action_by_index(0) 66 | # Execute first action 67 | rospy.loginfo("Executing action 0, " + 68 | "with {} primitives.".format(session.n_primitives())) 69 | session.execute_current_action() 70 | 71 | # You can specify pauses between actions 72 | rospy.sleep(3) 73 | 74 | # You can also execute individual primitives 75 | if session.n_primitives() > 0: 76 | rospy.loginfo("Executing first primitive of first action") 77 | session.execute_primitive(0) 78 | else: 79 | rospy.loginfo("Action 0 has no primitives.") 80 | 81 | action_num = raw_input('Which action should we execute next? Enter 1 or 2:') 82 | action_num = int(action_num) 83 | 84 | rospy.loginfo("Executing action {}.".format(action_num)) 85 | session.switch_to_action_by_index(action_num) 86 | session.execute_current_action() 87 | 88 | # You can also summon actions by name 89 | rospy.loginfo("Executing Wave action") 90 | session.switch_to_action_by_name("Wave") 91 | session.execute_current_action() 92 | 93 | else: 94 | rospy.logwarn("No actions available!") 95 | 96 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package fetch_pbd_interaction 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.8 (2017-08-08) 6 | ------------------ 7 | * Update README and CMakeLists 8 | * Do reachability check for ref_type PREVIOUS_TARGET 9 | * Interface improvements: update markers more efficiently, etc 10 | * Implemented pre-conditions for primitives 11 | * Interface improvements: object labels, marker fixes 12 | * Namespace topics and services, some marker fixes 13 | * Install specific version of polymer-cli to not get issues when polymer changes 14 | * Change World node to C++ and add Grasp suggestion integration 15 | * Contributors: Sarah Elliott 16 | 17 | 0.0.7 (2017-04-20) 18 | ------------------ 19 | * Built frontend and now serve pre-built frontend directly 20 | * add couchdb as run_depend, fix `#6 `_ 21 | * Custom ros3d.js to use unsubscribeTf and show mesh colours. Use local copies of Robot Web Tools libraries. 22 | * Contributors: Sarah Elliott 23 | 24 | 0.0.6 (2016-10-29) 25 | ------------------ 26 | * Update install_web_dependencies.sh 27 | * Contributors: Sarah Elliott 28 | 29 | 0.0.5 (2016-09-17) 30 | ------------------ 31 | * Update package.xml 32 | * Contributors: Russell Toris 33 | 34 | 0.0.4 (2016-09-09) 35 | ------------------ 36 | * Dependencies again 37 | * Contributors: Sarah Elliott 38 | 39 | 0.0.3 (2016-09-09) 40 | ------------------ 41 | 42 | 0.0.2 (2016-09-09) 43 | ------------------ 44 | * Dependencies 45 | * Contributors: Sarah Elliott 46 | 47 | 0.0.1 (2016-09-09) 48 | ------------------ 49 | * Update package.xml 50 | * License and documentation updates 51 | * Update examples 52 | * Okay last CMakelists.tx change... 53 | * Fix some frame initialisation issue 54 | * Accidentally deleted line before... 55 | * Switch from GuiInput.msg to GuiInput.srv 56 | * Less gross CMakelist/path stuff 57 | * CMakelists change 58 | * Loading/saving json files, still need to work on location of files 59 | * Small fix for editing actions 60 | * Small fix for deleting objects from world 61 | * Require version of rail_segmentation 62 | * Switched to use rail_segmentation 63 | * Getting ready to be released 64 | * Small fix for deleting actions 65 | * Fix frame issues when reordering primitives 66 | * Fix bug in reordering primitives 67 | * Marker UI fixes 68 | * Small fixes so things appear better on mobile 69 | * Error message popups 70 | * Add table to planning scene 71 | * Add functionality for stopping execution 72 | * Fixes based on Russell's comments 73 | * Change dependencies and action selection 74 | * Misc reorganisation and added to example script 75 | * Fix for trajectory marker 76 | * Coloring for selected primitive 77 | * Changes to selecting a primitive and structure of markers functionality 78 | * More marker fixes. Probably needs a lot of cleanup now. 79 | * Really made the markers work this time... 80 | * Marker and UI fixes 81 | * Fixed the no sound/no head movement behaviour 82 | * Lots of changes but added ability to edit actions numerically 83 | * Documentation and code cleanup 84 | * Make my own sortable list 85 | * Fix object marker location for web viz 86 | * Get rid of arrows pointing to objects 87 | * Added markers being relative to each other 88 | * Show and hide markers, plus some other marker fixes 89 | * Polymer web interface 90 | * Add back the standalone arm control node 91 | * Huge reorganisation. Now working. 92 | * Huge reorganisation. Now working. 93 | * Make continuous trajectories work (although crappily) 94 | * Fixed bug in moving to marker location 95 | * Changes to make 'links' between steps work 96 | * Found small naming bug in unused service 97 | * Cleaning up code 98 | * Replace moveit_python with moveit_commander and use poses instead of joints 99 | * Changed ArmControls class to ArmInteraction, fixed bug in PbD 100 | * Working, all functionality of PR2 PbD 101 | * First commit of port of PbD to Fetch. Things work. Rough around the edges. 102 | * Contributors: Russell Toris, Sarah Elliott 103 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/elements/scripts/STLLoader.min.js: -------------------------------------------------------------------------------- 1 | THREE.STLLoader=function(){};THREE.STLLoader.prototype={constructor:THREE.STLLoader};THREE.STLLoader.prototype.load=function(e,t){function i(r){if(r.target.status===200||r.target.status===0){var i=n.parse(r.target.response||r.target.responseText);n.dispatchEvent({type:"load",content:i});if(t)t(i)}else{n.dispatchEvent({type:"error",message:"Couldn't load URL ["+e+"]",response:r.target.responseText})}}var n=this;var r=new XMLHttpRequest;r.addEventListener("load",i,false);r.addEventListener("progress",function(e){n.dispatchEvent({type:"progress",loaded:e.loaded,total:e.total})},false);r.addEventListener("error",function(){n.dispatchEvent({type:"error",message:"Couldn't load URL ["+e+"]"})},false);r.overrideMimeType("text/plain; charset=x-user-defined");r.open("GET",e,true);r.responseType="arraybuffer";r.send(null)};THREE.STLLoader.prototype.parse=function(e){var t=function(){var e,t,r,i;i=new DataView(n);t=32/8*3+32/8*3*3+16/8;r=i.getUint32(80,true);e=80+32/8+r*t;return e===i.byteLength};var n=this.ensureBinary(e);return t()?this.parseBinary(n):this.parseASCII(this.ensureString(e))};THREE.STLLoader.prototype.parseBinary=function(e){var t,n,r,i,s,o,u,a,f,l,c;i=new DataView(e);r=i.getUint32(80,true);n=new THREE.Geometry;a=84;f=12*4+2;for(t=0;tthis.byteLength){throw new Error("DataView length or (byteOffset+length) value is out of bounds")}if(this.isString){r=this._getCharCodes(this.buffer,t,t+e)}else{r=this.buffer.slice(t,t+e)}if(!n&&e>1){if(!(r instanceof Array)){r=Array.prototype.slice.call(r)}r.reverse()}return r},getFloat64:function(e,t){var n=this._getBytes(8,e,t),r=1-2*(n[7]>>7),i=((n[7]<<1&255)<<3|n[6]>>4)-((1<<10)-1),s=(n[6]&15)*Math.pow(2,48)+n[5]*Math.pow(2,40)+n[4]*Math.pow(2,32)+n[3]*Math.pow(2,24)+n[2]*Math.pow(2,16)+n[1]*Math.pow(2,8)+n[0];if(i===1024){if(s!==0){return NaN}else{return r*Infinity}}if(i===-1023){return r*s*Math.pow(2,-1022-52)}return r*(1+s*Math.pow(2,-52))*Math.pow(2,i)},getFloat32:function(e,t){var n=this._getBytes(4,e,t),r=1-2*(n[3]>>7),i=(n[3]<<1&255|n[2]>>7)-127,s=(n[2]&127)<<16|n[1]<<8|n[0];if(i===128){if(s!==0){return NaN}else{return r*Infinity}}if(i===-127){return r*s*Math.pow(2,-126-23)}return r*(1+s*Math.pow(2,-23))*Math.pow(2,i)},getInt32:function(e,t){var n=this._getBytes(4,e,t);return n[3]<<24|n[2]<<16|n[1]<<8|n[0]},getUint32:function(e,t){return this.getInt32(e,t)>>>0},getInt16:function(e,t){return this.getUint16(e,t)<<16>>16},getUint16:function(e,t){var n=this._getBytes(2,e,t);return n[1]<<8|n[0]},getInt8:function(e){return this.getUint8(e)<<24>>24},getUint8:function(e){return this._getBytes(1,e)[0]}}} 2 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/compiled_frontend/bundled-es6/elements/my-actions.html: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /fetch_social_gaze/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(fetch_social_gaze) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | rospy 9 | actionlib 10 | std_msgs 11 | geometry_msgs 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 34 | ## pulled in transitively but can be declared for certainty nonetheless: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a run_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | # Generate actions in the 'action' folder 62 | add_action_files( 63 | FILES 64 | Gaze.action 65 | ) 66 | 67 | # Generate added messages and services with any dependencies listed here 68 | generate_messages( 69 | DEPENDENCIES 70 | actionlib_msgs 71 | std_msgs 72 | geometry_msgs # Or other packages containing msgs 73 | ) 74 | 75 | ################################### 76 | ## catkin specific configuration ## 77 | ################################### 78 | ## The catkin_package macro generates cmake config files for your package 79 | ## Declare things to be passed to dependent projects 80 | ## INCLUDE_DIRS: uncomment this if you package contains header files 81 | ## LIBRARIES: libraries you create in this project that dependent projects also need 82 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 83 | ## DEPENDS: system dependencies of this project that dependent projects also need 84 | catkin_package( 85 | # INCLUDE_DIRS include 86 | # LIBRARIES pr2_pbd 87 | # CATKIN_DEPENDS rospy 88 | # DEPENDS system_lib 89 | ) 90 | 91 | ########### 92 | ## Build ## 93 | ########### 94 | 95 | ## Specify additional locations of header files 96 | ## Your package locations should be listed before other locations 97 | # include_directories(include) 98 | include_directories( 99 | ${catkin_INCLUDE_DIRS} 100 | ) 101 | 102 | ## Declare a cpp library 103 | # add_library(pr2_pbd 104 | # src/${PROJECT_NAME}/pr2_pbd.cpp 105 | # ) 106 | 107 | ## Declare a cpp executable 108 | # add_executable(pr2_pbd_node src/pr2_pbd_node.cpp) 109 | 110 | ## Add cmake target dependencies of the executable/library 111 | ## as an example, message headers may need to be generated before nodes 112 | # add_dependencies(pr2_pbd_node pr2_pbd_generate_messages_cpp) 113 | 114 | ## Specify libraries to link a library or executable target against 115 | # target_link_libraries(pr2_pbd_node 116 | # ${catkin_LIBRARIES} 117 | # ) 118 | 119 | ############# 120 | ## Install ## 121 | ############# 122 | 123 | # all install targets should use catkin DESTINATION variables 124 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 125 | 126 | ## Mark executable scripts (Python etc.) for installation 127 | ## in contrast to setup.py, you can choose the destination 128 | # install(PROGRAMS 129 | # scripts/my_python_script 130 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 131 | # ) 132 | 133 | ## Mark executables and/or libraries for installation 134 | # install(TARGETS pr2_pbd pr2_pbd_node 135 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 136 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 137 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 138 | # ) 139 | 140 | ## Mark cpp header files for installation 141 | # install(DIRECTORY include/${PROJECT_NAME}/ 142 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 143 | # FILES_MATCHING PATTERN "*.h" 144 | # PATTERN ".svn" EXCLUDE 145 | # ) 146 | 147 | ## Mark other files for installation (e.g. launch and bag files, etc.) 148 | # install(FILES 149 | # # myfile1 150 | # # myfile2 151 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 152 | # ) 153 | 154 | install(PROGRAMS 155 | nodes/social_gaze_server.py 156 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 157 | ) 158 | 159 | install(FILES 160 | launch/gaze.launch 161 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 162 | ) 163 | 164 | ############# 165 | ## Testing ## 166 | ############# 167 | 168 | ## Add gtest based cpp test target and link libraries 169 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_pr2_pbd.cpp) 170 | # if(TARGET ${PROJECT_NAME}-test) 171 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 172 | # endif() 173 | 174 | ## Add folders to be run by python nosetests 175 | # catkin_add_nosetests(test) 176 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/elements/scripts/eventemitter2.min.js: -------------------------------------------------------------------------------- 1 | !function(e){function r(){this._events={};if(this._conf){i.call(this,this._conf)}}function i(e){if(e){this._conf=e;e.delimiter&&(this.delimiter=e.delimiter);e.maxListeners&&(this._events.maxListeners=e.maxListeners);e.wildcard&&(this.wildcard=e.wildcard);e.newListener&&(this.newListener=e.newListener);if(this.wildcard){this.listenerTree={}}}}function s(e){this._events={};this.newListener=false;i.call(this,e)}function o(e,t,n,r){if(!n){return[]}var i=[],s,u,a,f,l,c,h,p=t.length,d=t[r],v=t[r+1];if(r===p&&n._listeners){if(typeof n._listeners==="function"){e&&e.push(n._listeners);return[n]}else{for(s=0,u=n._listeners.length;s0&&o._listeners.length>a){o._listeners.warned=true;console.error("(node) warning: possible EventEmitter memory "+"leak detected. %d listeners added. "+"Use emitter.setMaxListeners() to increase limit.",o._listeners.length);console.trace()}}}return true}u=e.shift()}return true}var t=Array.isArray?Array.isArray:function(t){return Object.prototype.toString.call(t)==="[object Array]"};var n=10;s.prototype.delimiter=".";s.prototype.setMaxListeners=function(e){this._events||r.call(this);this._events.maxListeners=e;if(!this._conf)this._conf={};this._conf.maxListeners=e};s.prototype.event="";s.prototype.once=function(e,t){this.many(e,1,t);return this};s.prototype.many=function(e,t,n){function i(){if(--t===0){r.off(e,i)}n.apply(this,arguments)}var r=this;if(typeof n!=="function"){throw new Error("many only accepts instances of Function")}i._origin=n;this.on(e,i);return r};s.prototype.emit=function(){this._events||r.call(this);var e=arguments[0];if(e==="newListener"&&!this.newListener){if(!this._events.newListener){return false}}if(this._all){var t=arguments.length;var n=new Array(t-1);for(var i=1;i1)switch(arguments.length){case 2:s.call(this,arguments[1]);break;case 3:s.call(this,arguments[1],arguments[2]);break;default:var t=arguments.length;var n=new Array(t-1);for(var i=1;i0||!!this._all}else{return!!this._all}};s.prototype.on=function(e,i){if(typeof e==="function"){this.onAny(e);return this}if(typeof i!=="function"){throw new Error("on only accepts instances of Function")}this._events||r.call(this);this.emit("newListener",e,i);if(this.wildcard){u.call(this,e,i);return this}if(!this._events[e]){this._events[e]=i}else if(typeof this._events[e]==="function"){this._events[e]=[this._events[e],i]}else if(t(this._events[e])){this._events[e].push(i);if(!this._events[e].warned){var s=n;if(typeof this._events.maxListeners!=="undefined"){s=this._events.maxListeners}if(s>0&&this._events[e].length>s){this._events[e].warned=true;console.error("(node) warning: possible EventEmitter memory "+"leak detected. %d listeners added. "+"Use emitter.setMaxListeners() to increase limit.",this._events[e].length);console.trace()}}}return this};s.prototype.onAny=function(e){if(typeof e!=="function"){throw new Error("onAny only accepts instances of Function")}if(!this._all){this._all=[]}this._all.push(e);return this};s.prototype.addListener=s.prototype.on;s.prototype.off=function(e,n){if(typeof n!=="function"){throw new Error("removeListener only takes instances of Function")}var r,i=[];if(this.wildcard){var s=typeof e==="string"?e.split(this.delimiter):e.slice();i=o.call(this,null,s,this.listenerTree,0)}else{if(!this._events[e])return this;r=this._events[e];i.push({_listeners:r})}for(var u=0;u0){r=this._all;for(t=0,n=r.length;t 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 114 | 115 | 195 | 196 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Fetch Programming by Demonstration [![Build Status](https://api.travis-ci.org/fetchrobotics/fetch_pbd.png)](https://travis-ci.org/fetchrobotics/fetch_pbd) 2 | 3 | TODO: Travis build status and coveralls testing? 4 | 5 | This repository is based on PR2 Programming by Demonstration. This version is for the Fetch Robot. 6 | 7 | The original [PR2 Programming by Demonstration](https://github.com/PR2/pr2_pbd) was done by [Maya Cakmak](http://www.mayacakmak.com/) and the [Human-Centered Robotics Lab](https://hcrlab.cs.washington.edu/) at the University of Washington. 8 | 9 | ## System Requirements 10 | This PbD is designed for Ubuntu 14.04 and ROS Indigo. 11 | 12 | ## Installing from Source 13 | Clone this repository and build on the robot: 14 | ```bash 15 | cd ~/catkin_ws/src 16 | git clone https://github.com/fetchrobotics/fetch_pbd.git 17 | cd ~/catkin_ws 18 | catkin_make 19 | ``` 20 | To make sure all dependencies are installed: 21 | ```bash 22 | cd ~/catkin_ws 23 | source ~/devel/setup.bash 24 | rosdep update 25 | rosdep install --from-paths src --ignore-src --rosdistro=indigo -y 26 | ``` 27 | ## Making Changes to the Web Interface 28 | To make changes to the web interface, you will want to install all of the web dependencies: 29 | ```bash 30 | roscd fetch_pbd_interaction 31 | cd web_interface/fetch-pbd-gui 32 | ./install_web_dependencies.sh 33 | ``` 34 | Note that the above adds the following paths to the end of your `~/.bashrc`: 35 | ```bash 36 | export NVM_DIR="/home/USERNAME/.nvm" 37 | [ -s "$NVM_DIR/nvm.sh" ] && . "$NVM_DIR/nvm.sh" # This loads nvm 38 | ``` 39 | You will want to source your `~/.bashrc` file again before running the software. 40 | Now all of the dependencies are installed. After you make changes to the web interface, to build you can run: 41 | ```bash 42 | roscd fetch_pbd_interaction 43 | cd web_interface/fetch-pbd-gui 44 | ./build_frontend.sh 45 | ``` 46 | Now when you roslaunch this software it will run using your newly built changes! 47 | 48 | ## Running 49 | ### Commands on Fetch Robot 50 | #### Terminal #1 51 | ```bash 52 | source ~/catkin_ws/devel/setup.bash 53 | roslaunch fetch_pbd_interaction pbd.launch 54 | ``` 55 | You can run the backend without the "social gaze" head movements or without the sounds by passing arguments to the launch file: 56 | ```bash 57 | source ~/catkin_ws/devel/setup.bash 58 | roslaunch fetch_pbd_interaction pbd.launch social_gaze:=false play_sound:=false 59 | ``` 60 | 61 | You can also pass arguments to the launch file to save your actions to a json file or load them from a json file. 62 | This behaviour is a bit complicated. It is recommended that you specify the full path to files or else it will look in your .ros folder. 63 | If you specify a from_file then actions will be loaded from that file. They will replace the ones in your session database. 64 | Whatever was in your session database will get stored in a timestamped file in your .ros folder (not overwritten). 65 | If you specify a to_file then the session you are starting will be saved to that file. 66 | ```bash 67 | source ~/catkin_ws/devel/setup.bash 68 | roslaunch fetch_pbd_interaction pbd.launch from_file:=/full/path/from.json to_file:=/full/path/to.json 69 | ``` 70 | 71 | ### Using the GUI 72 | In your browser go to ROBOT_HOSTNAME:8080 in your browser to use the GUI. There is a "mobile" interface, which is the same but without the visualization. This can be useful for just saving/deleting primitives. 73 | 74 | The main page lists all the available actions. 75 | ![Main page](https://cloud.githubusercontent.com/assets/1470402/17989388/c71a3da2-6ae1-11e6-9d2f-894a67e508ca.png) 76 | You can directly run/copy/delete actions from the main page. Or hit the "Edit" button to see more information on that action. 77 | 78 | On the "Current Action" screen, most of the buttons are pretty self-explanatory. You can execute the entire action using the "Run" button at the bottom of the screen. This will execute all of the primitives in the order they appear in the Primitive List. You can click on a specific primitive (either the marker or the list item), to highlight the primitive. 79 | ![](https://cloud.githubusercontent.com/assets/1470402/17989398/d0602b2e-6ae1-11e6-8add-edcedf6285b6.png) 80 | 81 | You can show/hide the markers for each primitive by clicking the marker icon for the primitive in the Primitive List. 82 | ![](https://cloud.githubusercontent.com/assets/1470402/17989394/d05bf02c-6ae1-11e6-9446-9847bbd419ea.png) 83 | 84 | You can change the order of the primitives by dragging them to a new position in the list. 85 | ![](https://cloud.githubusercontent.com/assets/1470402/17989397/d0608290-6ae1-11e6-98a4-bbb1049e1185.png) 86 | 87 | You can edit the position and orientation of certain primitives by clicking the edit icon or by moving the interactive marker. 88 | ![](https://cloud.githubusercontent.com/assets/1470402/17989393/d05b87ea-6ae1-11e6-85d7-922c6dc4844a.png) 89 | 90 | You can change the frame that certain primitives are relative to by right-clicking the marker. 91 | ![](https://user-images.githubusercontent.com/1470402/28805009-d7e45398-761b-11e7-88e2-57898dde0a99.png) 92 | 93 | You can also change the name of the action. 94 | ![](https://cloud.githubusercontent.com/assets/1470402/17989396/d05f44d4-6ae1-11e6-9363-f242c5ea15b6.png) 95 | 96 | You can record objects in the environment using the "Record Objects" button. The objects will appear in the viewer. Poses can be relative to these objects in the environment. 97 | ![](https://user-images.githubusercontent.com/1470402/28805010-d7e6513e-761b-11e7-9fc8-b63c0e517571.png) 98 | ![](https://user-images.githubusercontent.com/1470402/28805011-d7edb910-761b-11e7-81a9-6e1f9c2f0be8.png) 99 | 100 | ### Grasping in Fetch PbD 101 | You can now (optionally) run Fetch PbD with a grasp suggestion service. This is a service that takes in a point cloud for an object and returns a PoseArray of possible grasps for that object. Fetch Pbd provides feedback by publishing which grasp was chosen by the user. Any service that adheres to the .srv interface defined by [SuggestGrasps.srv](https://github.com/GT-RAIL/rail_manipulation_msgs/blob/develop/srv/SuggestGrasps.srv) and optionally the feedback .msg interface, [GraspFeedback.msg](https://github.com/GT-RAIL/rail_manipulation_msgs/blob/develop/msg/GraspFeedback.msg), can be used. If you want to start Fetch PbD with a grasp suggestion service: 102 | ```bash 103 | source ~/catkin_ws/devel/setup.bash 104 | roslaunch fetch_pbd_interaction pbd.launch grasp_suggestion_service:=grasp_service_name grasp_feedback_topic:=grasp_feedback 105 | ``` 106 | The grasp_feedback_topic is optional and can be excluded. Then in the interface you can right-click on objects in the scene and add a grasp for that object. Initially a grasp primitive is generated that is just a placeholder and does not specify any poses. To use the grasp suggestion service to generate grasps, you then right click the blue placeholder box and select "Generate grasps". The service will return a list of grasp poses. Fetch PbD sets a pre-grasp that is 15 cm away from the grasp. The first grasp generated is shown and you can switch to other grasp options by right-clicking the grasp marker and selecting one from the list. 107 | 108 | ![](https://user-images.githubusercontent.com/1470402/28805083-48681a96-761c-11e7-9921-2e9b7e07e318.png) 109 | ![](https://user-images.githubusercontent.com/1470402/28805082-4866964e-761c-11e7-80bd-b67ebd9cc4b2.png) 110 | ![](https://user-images.githubusercontent.com/1470402/28805081-48664284-761c-11e7-91b2-89958b14eab3.png) 111 | 112 | Grasps are executed like any other primitve and can be added to actions in combination with other primitives. 113 | 114 | ### Code Interface 115 | You can also access the actions you've programmed through code. You still need to run pbd_backend.launch. 116 | 117 | #### Commands on Fetch 118 | ```bash 119 | source ~/catkin_ws/devel/setup.bash 120 | rosrun fetch_pbd_interaction demo.py 121 | ``` 122 | 123 | ## System Overview 124 | ### Interaction Node 125 | The pbd_interaction_node.py handles the interaction between speech/GUI and the rest of the system. Changes happen through the update loop in interaction.py and also through the callbacks from speech/GUI commands. interaction.py also subscribes to updates from the pbd_world_node.py, which notifies it of changes in objects in the world. Through callbacks and the update loop, interaction.py hooks in to session.py. session.py handles creating actions and primitives and saving them to the database. 126 | 127 | ### Arm Control Node 128 | The pbd_arm_control_node.py is how the robot's arm is controlled to execute actions/primitives. It provides a lower level service interface to move the arm. The interaction node interacts with this through the interface in robot.py. 129 | 130 | ### World Node 131 | The pbd_world_node.py handles the robot's perception of the world. Other nodes ask the world node about the state of the world and can both send and subscribe to updates to the world. Its main function is to provide a list of objects currently in the scene. 132 | 133 | ### Social Gaze Node 134 | The social_gaze_server.py handles the movements of the robot's head. This is also controlled through the robot.py interface. The sounds are also provided through this interface. 135 | 136 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/include/fetch_pbd_interaction/world.h: -------------------------------------------------------------------------------- 1 | #ifndef PBD_WORLD 2 | #define PBD_WORLD 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "ros/ros.h" 10 | #include "sensor_msgs/PointCloud2.h" 11 | #include "geometry_msgs/Vector3.h" 12 | #include "geometry_msgs/PoseStamped.h" 13 | #include "geometry_msgs/TransformStamped.h" 14 | #include "geometry_msgs/Quaternion.h" 15 | #include "geometry_msgs/Point.h" 16 | #include "visualization_msgs/Marker.h" 17 | #include "visualization_msgs/InteractiveMarker.h" 18 | #include "visualization_msgs/InteractiveMarkerControl.h" 19 | #include "visualization_msgs/InteractiveMarkerFeedback.h" 20 | #include 21 | #include "rail_manipulation_msgs/SegmentedObject.h" 22 | #include "rail_manipulation_msgs/SegmentedObjectList.h" 23 | #include 24 | #include 25 | #include "fetch_pbd_interaction/WorldState.h" 26 | #include "fetch_pbd_interaction/Landmark.h" 27 | #include "fetch_pbd_interaction/GetObjectList.h" 28 | #include "fetch_pbd_interaction/GetMostSimilarObject.h" 29 | #include "fetch_pbd_interaction/GetObjectFromName.h" 30 | #include "fetch_pbd_interaction/GetNearestObject.h" 31 | #include "std_msgs/ColorRGBA.h" 32 | #include "std_srvs/Empty.h" 33 | #include "moveit_msgs/CollisionObject.h" 34 | #include "moveit_msgs/PlanningScene.h" 35 | #include "shape_msgs/SolidPrimitive.h" 36 | 37 | // #include "pcl/point_cloud.h" 38 | #include "pcl_conversions/pcl_conversions.h" 39 | #include "pcl/point_types.h" 40 | #include "pcl/filters/filter.h" 41 | #include "pcl/common/centroid.h" 42 | #include "pcl/common/common.h" 43 | #include "pcl/common/distances.h" 44 | #include "pcl/common/pca.h" 45 | #include "pcl/filters/filter.h" 46 | #include "pcl/kdtree/kdtree_flann.h" 47 | #include "pcl/point_types.h" 48 | #include "pcl/search/kdtree.h" 49 | #include "pcl/segmentation/conditional_euclidean_clustering.h" 50 | #include "pcl/segmentation/extract_clusters.h" 51 | #include "pcl/segmentation/region_growing_rgb.h" 52 | #include "pcl_conversions/pcl_conversions.h" 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | #include 70 | #include 71 | #include 72 | #include 73 | #include 74 | #include 75 | #include 76 | #include 77 | 78 | #include 79 | #include 80 | 81 | #include 82 | #include 83 | 84 | #include "fetch_pbd_interaction/world_landmark.h" 85 | 86 | namespace fetch_pbd_interaction { 87 | 88 | class World { 89 | 90 | private: 91 | boost::mutex mutex; 92 | std::vector objects; 93 | interactive_markers::InteractiveMarkerServer im_server; 94 | ros::Publisher add_grasp_pub; 95 | ros::Publisher world_update_pub; 96 | ros::Publisher planning_scene_diff_publisher; 97 | ros::Subscriber table_subscriber; 98 | ros::Subscriber object_subscriber; 99 | ros::ServiceClient segmentation_service_client; 100 | std::string segmented_objects_topic; 101 | bool has_surface; 102 | tf::TransformListener tf_listener; 103 | tf::TransformBroadcaster tf_broadcaster; 104 | ros::ServiceServer nearest_object_service; 105 | ros::ServiceServer object_list_service; 106 | ros::ServiceServer similar_object_service; 107 | ros::ServiceServer object_from_name_service; 108 | ros::ServiceServer clear_world_objects_service; 109 | ros::ServiceServer update_world_service; 110 | interactive_markers::MenuHandler menu_handler; 111 | geometry_msgs::Vector3 scale_text; 112 | // Two objects must be closer than this to be considered 'the same'. 113 | float obj_similar_dist_threshold; 114 | 115 | // When adding objects, if they are closer than this they'll replace one 116 | // another. 117 | float obj_add_dist_threshold; 118 | 119 | // How close to 'nearest' object something must be to be counted as 120 | // 'near' it. 121 | float obj_nearest_dist_threshold; 122 | 123 | // Landmark distances below this will be clamped to zero. 124 | float obj_dist_zero_clamp; 125 | 126 | // Scales 127 | float text_height; 128 | float surface_height; // 0.01 == 1cm (i think) 129 | float text_offset; // how high objects' labels are above them. 130 | 131 | // colors 132 | std_msgs::ColorRGBA color_obj; // not yet a param 133 | std_msgs::ColorRGBA color_surface; // not yet a param 134 | std_msgs::ColorRGBA color_text; // not yet a param 135 | 136 | // frames 137 | std::string base_frame; 138 | ros::Duration marker_duration; 139 | 140 | static void pc2ToMarker(sensor_msgs::PointCloud2 pc2, int index, 141 | ros::Duration duration, std::string output_frame, 142 | visualization_msgs::Marker* pc2_marker_points, visualization_msgs::Marker* pc2_marker_sphere_list); 143 | 144 | static float objectDissimilarity(fetch_pbd_interaction::Landmark obj1, fetch_pbd_interaction::Landmark obj2); 145 | 146 | static float poseDistance(geometry_msgs::Pose pose1, geometry_msgs::Pose pose2, bool is_on_table=true); 147 | 148 | visualization_msgs::InteractiveMarker getSurfaceMarker(geometry_msgs::Pose pose, geometry_msgs::Vector3 dimensions); 149 | 150 | void worldChanged(); 151 | 152 | bool getMostSimilarObjectCallback(fetch_pbd_interaction::GetMostSimilarObject::Request& req, 153 | fetch_pbd_interaction::GetMostSimilarObject::Response& res); 154 | 155 | std::vector getObjectList(); 156 | 157 | bool getObjectListCallback(fetch_pbd_interaction::GetObjectList::Request& req, 158 | fetch_pbd_interaction::GetObjectList::Response& resp); 159 | 160 | bool updateWorldCallback(fetch_pbd_interaction::GetObjectList::Request& req, 161 | fetch_pbd_interaction::GetObjectList::Response& resp); 162 | 163 | bool getObjectFromNameCallback(fetch_pbd_interaction::GetObjectFromName::Request& req, 164 | fetch_pbd_interaction::GetObjectFromName::Response& resp); 165 | 166 | bool hasObjects(); 167 | 168 | void recordObjectPose(); 169 | 170 | void objectsUpdateCallback(const rail_manipulation_msgs::SegmentedObjectListConstPtr& msg); 171 | 172 | void getBoundingBox(sensor_msgs::PointCloud2 pc2, geometry_msgs::Vector3* dimensions, geometry_msgs::Pose* pose); 173 | 174 | 175 | void tablePositionUpdateCallback(const rail_manipulation_msgs::SegmentedObjectPtr& msg); 176 | 177 | bool clearObjectsCallback(std_srvs::Empty::Request& req, 178 | std_srvs::Empty::Response& resp); 179 | 180 | void clearObjects(); 181 | 182 | bool getNearestObjectCallback(fetch_pbd_interaction::GetNearestObject::Request& req, 183 | fetch_pbd_interaction::GetNearestObject::Response& resp); 184 | 185 | void markerFeedbackCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 186 | 187 | void resetObjects(); 188 | 189 | bool addNewObject(geometry_msgs::Pose pose, geometry_msgs::Vector3 dimensions, 190 | sensor_msgs::PointCloud2 point_cloud); 191 | 192 | 193 | void removeSurfaceMarker(); 194 | 195 | visualization_msgs::InteractiveMarker getObjectMarker(int index); 196 | 197 | void addSurfaceToPlanningScene(geometry_msgs::Pose pose, geometry_msgs::Vector3 dimensions); 198 | 199 | void removeSurfaceFromPlanningScene(); 200 | 201 | void publishTfPose(geometry_msgs::Pose pose, std::string name, std::string parent); 202 | 203 | void removeObject(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 204 | 205 | void addGrasp(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 206 | 207 | 208 | public: 209 | World(ros::NodeHandle n, ros::NodeHandle pn, 210 | const std::string grasp_suggestion_service, 211 | const std::string& segmentation_service_name, const std::string& segmented_objects_topic_name, 212 | const std::string& segmented_table_topic_name, const std::string& planning_scene_topic, 213 | const float& obj_similar_distance_threshold, const float& obj_add_distance_threshold, 214 | const float& obj_nearest_distance_threshold, 215 | const float& obj_distance_zero_clamp, const float& text_h, 216 | const float& surface_h, const float& text_off, 217 | const std::string& base_frame_name); 218 | ~World(); 219 | 220 | void update(); 221 | 222 | }; 223 | 224 | }; 225 | 226 | #endif 227 | 228 | -------------------------------------------------------------------------------- /fetch_arm_control/config/fetch_arm_control.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /PointCloud21 9 | Splitter Ratio: 0.5 10 | Tree Height: 607 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.03 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Class: rviz/RobotModel 53 | Collision Enabled: false 54 | Enabled: true 55 | Links: 56 | All Links Enabled: true 57 | Expand Joint Details: false 58 | Expand Link Details: false 59 | Expand Tree: false 60 | Link Tree Style: Links in Alphabetic Order 61 | base_link: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | Value: true 66 | bellows_link: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | Value: true 71 | bellows_link2: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | elbow_flex_link: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | estop_link: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | forearm_roll_link: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | gripper_link: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | Value: true 96 | head_camera_depth_frame: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | head_camera_depth_optical_frame: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | head_camera_link: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | head_camera_rgb_frame: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | head_camera_rgb_optical_frame: 113 | Alpha: 1 114 | Show Axes: false 115 | Show Trail: false 116 | head_pan_link: 117 | Alpha: 1 118 | Show Axes: false 119 | Show Trail: false 120 | Value: true 121 | head_tilt_link: 122 | Alpha: 1 123 | Show Axes: false 124 | Show Trail: false 125 | Value: true 126 | l_gripper_finger_link: 127 | Alpha: 1 128 | Show Axes: false 129 | Show Trail: false 130 | Value: true 131 | l_wheel_link: 132 | Alpha: 1 133 | Show Axes: false 134 | Show Trail: false 135 | Value: true 136 | laser_link: 137 | Alpha: 1 138 | Show Axes: false 139 | Show Trail: false 140 | Value: true 141 | r_gripper_finger_link: 142 | Alpha: 1 143 | Show Axes: false 144 | Show Trail: false 145 | Value: true 146 | r_wheel_link: 147 | Alpha: 1 148 | Show Axes: false 149 | Show Trail: false 150 | Value: true 151 | shoulder_lift_link: 152 | Alpha: 1 153 | Show Axes: false 154 | Show Trail: false 155 | Value: true 156 | shoulder_pan_link: 157 | Alpha: 1 158 | Show Axes: false 159 | Show Trail: false 160 | Value: true 161 | torso_fixed_link: 162 | Alpha: 1 163 | Show Axes: false 164 | Show Trail: false 165 | Value: true 166 | torso_lift_link: 167 | Alpha: 1 168 | Show Axes: false 169 | Show Trail: false 170 | Value: true 171 | upperarm_roll_link: 172 | Alpha: 1 173 | Show Axes: false 174 | Show Trail: false 175 | Value: true 176 | wrist_flex_link: 177 | Alpha: 1 178 | Show Axes: false 179 | Show Trail: false 180 | Value: true 181 | wrist_roll_link: 182 | Alpha: 1 183 | Show Axes: false 184 | Show Trail: false 185 | Value: true 186 | Name: RobotModel 187 | Robot Description: robot_description 188 | TF Prefix: "" 189 | Update Interval: 0 190 | Value: true 191 | Visual Enabled: true 192 | - Alpha: 1 193 | Autocompute Intensity Bounds: true 194 | Autocompute Value Bounds: 195 | Max Value: 10 196 | Min Value: -10 197 | Value: true 198 | Axis: Z 199 | Channel Name: intensity 200 | Class: rviz/PointCloud2 201 | Color: 255; 255; 255 202 | Color Transformer: RGB8 203 | Decay Time: 0 204 | Enabled: true 205 | Invert Rainbow: false 206 | Max Color: 255; 255; 255 207 | Max Intensity: 4096 208 | Min Color: 0; 0; 0 209 | Min Intensity: 0 210 | Name: PointCloud2 211 | Position Transformer: XYZ 212 | Queue Size: 10 213 | Selectable: true 214 | Size (Pixels): 3 215 | Size (m): 0.01 216 | Style: Flat Squares 217 | Topic: /head_camera/depth_registered/points 218 | Unreliable: false 219 | Use Fixed Frame: true 220 | Use rainbow: true 221 | Value: true 222 | - Class: rviz/InteractiveMarkers 223 | Enable Transparency: true 224 | Enabled: true 225 | Name: InteractiveMarkers 226 | Show Axes: false 227 | Show Descriptions: true 228 | Show Visual Aids: false 229 | Update Topic: /interactive_arm_control/update 230 | Value: true 231 | Enabled: true 232 | Global Options: 233 | Background Color: 48; 48; 48 234 | Fixed Frame: base_link 235 | Frame Rate: 30 236 | Name: root 237 | Tools: 238 | - Class: rviz/Interact 239 | Hide Inactive Objects: true 240 | - Class: rviz/MoveCamera 241 | - Class: rviz/Select 242 | - Class: rviz/FocusCamera 243 | - Class: rviz/Measure 244 | - Class: rviz/SetInitialPose 245 | Topic: /initialpose 246 | - Class: rviz/SetGoal 247 | Topic: /move_base_simple/goal 248 | - Class: rviz/PublishPoint 249 | Single click: true 250 | Topic: /clicked_point 251 | Value: true 252 | Views: 253 | Current: 254 | Class: rviz/Orbit 255 | Distance: 2.6186 256 | Enable Stereo Rendering: 257 | Stereo Eye Separation: 0.06 258 | Stereo Focal Distance: 1 259 | Swap Stereo Eyes: false 260 | Value: false 261 | Focal Point: 262 | X: -0.26045 263 | Y: -0.466054 264 | Z: 0.523743 265 | Name: Current View 266 | Near Clip Distance: 0.01 267 | Pitch: 0.314799 268 | Target Frame: 269 | Value: Orbit (rviz) 270 | Yaw: 6.05358 271 | Saved: ~ 272 | Window Geometry: 273 | Displays: 274 | collapsed: false 275 | Height: 888 276 | Hide Left Dock: false 277 | Hide Right Dock: false 278 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000002eefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002ee000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000249fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000249000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004d50000003efc0100000002fb0000000800540069006d00650100000000000004d5000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000365000002ee00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 279 | Selection: 280 | collapsed: false 281 | Time: 282 | collapsed: false 283 | Tool Properties: 284 | collapsed: false 285 | Views: 286 | collapsed: false 287 | Width: 1237 288 | X: 419 289 | Y: 51 290 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/config/fetch_pbd.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /PointCloud21 9 | Splitter Ratio: 0.5 10 | Tree Height: 775 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.03 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Class: rviz/RobotModel 53 | Collision Enabled: false 54 | Enabled: true 55 | Links: 56 | All Links Enabled: true 57 | Expand Joint Details: false 58 | Expand Link Details: false 59 | Expand Tree: false 60 | Link Tree Style: Links in Alphabetic Order 61 | base_link: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | Value: true 66 | bellows_link: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | Value: true 71 | bellows_link2: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | elbow_flex_link: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | estop_link: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | forearm_roll_link: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | gripper_link: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | Value: true 96 | head_camera_depth_frame: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | head_camera_depth_optical_frame: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | head_camera_link: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | head_camera_rgb_frame: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | head_camera_rgb_optical_frame: 113 | Alpha: 1 114 | Show Axes: false 115 | Show Trail: false 116 | head_pan_link: 117 | Alpha: 1 118 | Show Axes: false 119 | Show Trail: false 120 | Value: true 121 | head_tilt_link: 122 | Alpha: 1 123 | Show Axes: false 124 | Show Trail: false 125 | Value: true 126 | l_gripper_finger_link: 127 | Alpha: 1 128 | Show Axes: false 129 | Show Trail: false 130 | Value: true 131 | l_wheel_link: 132 | Alpha: 1 133 | Show Axes: false 134 | Show Trail: false 135 | Value: true 136 | laser_link: 137 | Alpha: 1 138 | Show Axes: false 139 | Show Trail: false 140 | Value: true 141 | r_gripper_finger_link: 142 | Alpha: 1 143 | Show Axes: false 144 | Show Trail: false 145 | Value: true 146 | r_wheel_link: 147 | Alpha: 1 148 | Show Axes: false 149 | Show Trail: false 150 | Value: true 151 | shoulder_lift_link: 152 | Alpha: 1 153 | Show Axes: false 154 | Show Trail: false 155 | Value: true 156 | shoulder_pan_link: 157 | Alpha: 1 158 | Show Axes: false 159 | Show Trail: false 160 | Value: true 161 | torso_fixed_link: 162 | Alpha: 1 163 | Show Axes: false 164 | Show Trail: false 165 | Value: true 166 | torso_lift_link: 167 | Alpha: 1 168 | Show Axes: false 169 | Show Trail: false 170 | Value: true 171 | upperarm_roll_link: 172 | Alpha: 1 173 | Show Axes: false 174 | Show Trail: false 175 | Value: true 176 | wrist_flex_link: 177 | Alpha: 1 178 | Show Axes: false 179 | Show Trail: false 180 | Value: true 181 | wrist_roll_link: 182 | Alpha: 1 183 | Show Axes: false 184 | Show Trail: false 185 | Value: true 186 | Name: RobotModel 187 | Robot Description: robot_description 188 | TF Prefix: "" 189 | Update Interval: 0 190 | Value: true 191 | Visual Enabled: true 192 | - Class: rviz/InteractiveMarkers 193 | Enable Transparency: true 194 | Enabled: true 195 | Name: InteractiveMarkers 196 | Show Axes: false 197 | Show Descriptions: true 198 | Show Visual Aids: false 199 | Update Topic: /world_objects/update 200 | Value: true 201 | - Alpha: 1 202 | Autocompute Intensity Bounds: true 203 | Autocompute Value Bounds: 204 | Max Value: 10 205 | Min Value: -10 206 | Value: true 207 | Axis: Z 208 | Channel Name: intensity 209 | Class: rviz/PointCloud2 210 | Color: 255; 255; 255 211 | Color Transformer: RGB8 212 | Decay Time: 0 213 | Enabled: true 214 | Invert Rainbow: false 215 | Max Color: 255; 255; 255 216 | Max Intensity: 4096 217 | Min Color: 0; 0; 0 218 | Min Intensity: 0 219 | Name: PointCloud2 220 | Position Transformer: XYZ 221 | Queue Size: 10 222 | Selectable: true 223 | Size (Pixels): 3 224 | Size (m): 0.01 225 | Style: Points 226 | Topic: /head_camera/depth_registered/points 227 | Unreliable: false 228 | Use Fixed Frame: true 229 | Use rainbow: true 230 | Value: true 231 | - Class: rviz/InteractiveMarkers 232 | Enable Transparency: true 233 | Enabled: true 234 | Name: InteractiveMarkers 235 | Show Axes: false 236 | Show Descriptions: true 237 | Show Visual Aids: false 238 | Update Topic: /programmed_actions/update 239 | Value: true 240 | - Class: rviz/InteractiveMarkers 241 | Enable Transparency: true 242 | Enabled: true 243 | Name: InteractiveMarkers 244 | Show Axes: false 245 | Show Descriptions: true 246 | Show Visual Aids: false 247 | Update Topic: /interactive_arm_control/update 248 | Value: true 249 | - Class: rviz/MarkerArray 250 | Enabled: true 251 | Marker Topic: visualization_marker_array 252 | Name: MarkerArray 253 | Namespaces: 254 | {} 255 | Queue Size: 100 256 | Value: true 257 | Enabled: true 258 | Global Options: 259 | Background Color: 48; 48; 48 260 | Fixed Frame: base_link 261 | Frame Rate: 30 262 | Name: root 263 | Tools: 264 | - Class: rviz/Interact 265 | Hide Inactive Objects: true 266 | - Class: rviz/MoveCamera 267 | - Class: rviz/Select 268 | - Class: rviz/FocusCamera 269 | - Class: rviz/Measure 270 | - Class: rviz/SetInitialPose 271 | Topic: /initialpose 272 | - Class: rviz/SetGoal 273 | Topic: /move_base_simple/goal 274 | - Class: rviz/PublishPoint 275 | Single click: true 276 | Topic: /clicked_point 277 | Value: true 278 | Views: 279 | Current: 280 | Class: rviz/Orbit 281 | Distance: 2.40034 282 | Enable Stereo Rendering: 283 | Stereo Eye Separation: 0.06 284 | Stereo Focal Distance: 1 285 | Swap Stereo Eyes: false 286 | Value: false 287 | Focal Point: 288 | X: 0.35159 289 | Y: 0.0883872 290 | Z: 0.642831 291 | Name: Current View 292 | Near Clip Distance: 0.01 293 | Pitch: 0.744798 294 | Target Frame: 295 | Value: Orbit (rviz) 296 | Yaw: 1.5054 297 | Saved: ~ 298 | Window Geometry: 299 | Displays: 300 | collapsed: false 301 | Height: 1056 302 | Hide Left Dock: false 303 | Hide Right Dock: false 304 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000249fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000249000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 305 | Selection: 306 | collapsed: false 307 | Time: 308 | collapsed: false 309 | Tool Properties: 310 | collapsed: false 311 | Views: 312 | collapsed: false 313 | Width: 1855 314 | X: 65 315 | Y: 24 316 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/elements/my-actions.html: -------------------------------------------------------------------------------- 1 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 156 | 157 | 359 | 360 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/web_interface/fetch-pbd-gui/compiled_frontend/bundled-es6/elements/current-action.html: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /fetch_social_gaze/nodes/social_gaze_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | '''Handles where to point robot's head''' 4 | 5 | # ###################################################################### 6 | # Imports 7 | # ###################################################################### 8 | 9 | # Core ROS imports come first. 10 | import rospy 11 | 12 | # System builtins 13 | import time 14 | from numpy import array, linalg 15 | 16 | # ROS builtins 17 | # from scipy.ndimage.filters import * 18 | from actionlib_msgs.msg import GoalStatus 19 | from actionlib import SimpleActionClient, SimpleActionServer 20 | from control_msgs.msg import PointHeadAction, PointHeadGoal 21 | from geometry_msgs.msg import Point 22 | from tf import TransformListener 23 | 24 | # Local 25 | from fetch_pbd_interaction.srv import GetGazeGoal, GetGazeGoalResponse 26 | from fetch_social_gaze.msg import GazeGoal, GazeAction 27 | 28 | # ###################################################################### 29 | # Classes 30 | # ###################################################################### 31 | 32 | class SocialGaze: 33 | '''Handles where to point robot's head''' 34 | 35 | def __init__(self): 36 | self._default_focus_point = Point(1, 0, 1.05) 37 | self._down_focus_point = Point(0.5, 0, 0.5) 38 | self._target_focus_point = Point(1, 0, 1.05) 39 | self._current_focus_point = Point(1, 0, 1.05) 40 | 41 | self._current_gaze_action = GazeGoal.LOOK_FORWARD 42 | self._prev_gaze_action = self._current_gaze_action 43 | self._prev_target_focus_point = array(self._default_focus_point) 44 | 45 | # Some constants 46 | self._no_interrupt = [GazeGoal.NOD, 47 | GazeGoal.SHAKE, GazeGoal.LOOK_DOWN] 48 | self._nod_positions = [Point(1, 0, 0.70), Point(1, 0, 1.20)] 49 | self._shake_positions = [Point(1, 0.2, 1.05), Point(1, -0.2, 1.05)] 50 | self._n_nods = 5 51 | self._n_shakes = 5 52 | 53 | self._nod_counter = 5 54 | self._shake_counter = 5 55 | self._face_pos = None 56 | self._glance_counter = 0 57 | 58 | self.gaze_goal_strs = { 59 | GazeGoal.LOOK_FORWARD: 'LOOK_FORWARD', 60 | GazeGoal.FOLLOW_EE: 'FOLLOW_EE', 61 | GazeGoal.GLANCE_EE: 'GLANCE_EE', 62 | GazeGoal.NOD: 'NOD', 63 | GazeGoal.SHAKE: 'SHAKE', 64 | GazeGoal.FOLLOW_FACE: 'FOLLOW_FACE', 65 | GazeGoal.LOOK_AT_POINT: 'LOOK_AT_POINT', 66 | GazeGoal.LOOK_DOWN: 'LOOK_DOWN', 67 | GazeGoal.NONE: 'NONE', 68 | } 69 | 70 | ## Action client for sending commands to the head. 71 | self._head_action_client = SimpleActionClient( 72 | '/head_controller/point_head', PointHeadAction) 73 | self._head_action_client.wait_for_server() 74 | rospy.loginfo('Head action client has responded.') 75 | 76 | self._head_goal = PointHeadGoal() 77 | self._head_goal.target.header.frame_id = 'base_link' 78 | self._head_goal.min_duration = rospy.Duration(1.0) 79 | self._head_goal.target.point = Point(1, 0, 1) 80 | 81 | ## Service client for arm states 82 | self._tf_listener = TransformListener() 83 | 84 | ## Server for gaze requested gaze actions 85 | self._gaze_action_server = SimpleActionServer( 86 | '/fetch_pbd/gaze_action', GazeAction, self._execute_gaze_action, False) 87 | self._gaze_action_server.start() 88 | 89 | self._is_action_complete = True 90 | 91 | rospy.Service('/fetch_pbd/get_current_gaze_goal', GetGazeGoal, 92 | self._get_gaze_goal) 93 | 94 | # ################################################################## 95 | # Instance methods: Public (API) 96 | # ################################################################## 97 | 98 | def update(self): 99 | '''Update goal for head movement''' 100 | is_action_possibly_complete = True 101 | 102 | if self._current_gaze_action == GazeGoal.FOLLOW_EE: 103 | self._target_focus_point = self._get_ee_pos() 104 | 105 | elif self._current_gaze_action == GazeGoal.NOD: 106 | self._target_focus_point = self._get_next_nod_point( 107 | self._current_focus_point, self._target_focus_point) 108 | self._head_goal.min_duration = rospy.Duration(0.5) 109 | is_action_possibly_complete = False 110 | 111 | elif self._current_gaze_action == GazeGoal.SHAKE: 112 | self._target_focus_point = self._get_next_shake_point( 113 | self._current_focus_point, self._target_focus_point) 114 | self._head_goal.min_duration = rospy.Duration(0.5) 115 | is_action_possibly_complete = False 116 | 117 | elif self._current_gaze_action == GazeGoal.GLANCE_EE: 118 | self._target_focus_point = self._get_next_glance_point( 119 | self._current_focus_point, self._target_focus_point) 120 | is_action_possibly_complete = False 121 | 122 | self._current_focus_point = SocialGaze._filter_look_at_position( 123 | self._current_focus_point, self._target_focus_point) 124 | if self._current_gaze_action is GazeGoal.NONE: 125 | pass 126 | elif (SocialGaze._is_the_same( 127 | SocialGaze._point2array(self._head_goal.target.point), 128 | SocialGaze._point2array(self._target_focus_point))): 129 | 130 | if is_action_possibly_complete: 131 | head_state = self._head_action_client.get_state() 132 | if head_state != GoalStatus.ACTIVE: 133 | self._is_action_complete = True 134 | else: 135 | self._head_goal.target.point.x = self._current_focus_point.x 136 | self._head_goal.target.point.y = self._current_focus_point.y 137 | self._head_goal.target.point.z = self._current_focus_point.z 138 | self._head_action_client.send_goal(self._head_goal) 139 | 140 | time.sleep(0.02) 141 | 142 | # ################################################################## 143 | # Static methods: Internal ("private") 144 | # ################################################################## 145 | 146 | @staticmethod 147 | def _is_the_same(current, target): 148 | '''Get 3DOF pose of end effector 149 | 150 | Args: 151 | current (array): (x, y, z) of point 152 | target (array): (x, y, z) of point 153 | 154 | Returns: 155 | bool 156 | ''' 157 | diff = target - current 158 | dist = linalg.norm(diff) 159 | return dist < 0.003 160 | 161 | @staticmethod 162 | def _point2array(p): 163 | '''Make Point msg into array 164 | 165 | Args: 166 | p (Point) 167 | 168 | Returns: 169 | array 170 | ''' 171 | return array((p.x, p.y, p.z)) 172 | 173 | @staticmethod 174 | def _array2point(a): 175 | '''Make array into Point msg 176 | 177 | Args: 178 | a (array) 179 | 180 | Returns: 181 | Point 182 | ''' 183 | return Point(a[0], a[1], a[2]) 184 | 185 | @staticmethod 186 | def _filter_look_at_position(current, target): 187 | '''If head goal is too far away, returns an intermediate position 188 | to limit speed 189 | 190 | Args: 191 | current (Point): current head goal 192 | target (Point): new head goal 193 | 194 | Returns: 195 | Point 196 | ''' 197 | speed = 0.02 198 | diff = (SocialGaze._point2array(target) - 199 | SocialGaze._point2array(current)) 200 | dist = linalg.norm(diff) 201 | if dist > speed: 202 | step = dist / speed 203 | return SocialGaze._array2point(SocialGaze._point2array(current) + 204 | diff / step) 205 | else: 206 | return target 207 | 208 | # ################################################################## 209 | # Instance methods: Internal ("private") 210 | # ################################################################## 211 | 212 | def _get_gaze_goal(self, req): 213 | '''Return current gaze goal 214 | 215 | Args: 216 | req (GetGazeGoalRequest) : Unused 217 | Returns: 218 | GetGazeGoalResponse 219 | ''' 220 | goal = self._current_gaze_action 221 | # rospy.loginfo("Gaze Goal: {}".format(goal)) 222 | return GetGazeGoalResponse(int(goal)) 223 | 224 | 225 | def _get_ee_pos(self): 226 | '''Get 3DOF pose of end effector 227 | 228 | Returns: 229 | Point: location of wrist of end effector 230 | (could change to be finger tips) 231 | ''' 232 | 233 | from_frame = '/base_link' 234 | to_frame = '/wrist_roll_link' 235 | 236 | try: 237 | t = self._tf_listener.getLatestCommonTime(from_frame, to_frame) 238 | (position, rotation) = self._tf_listener.lookupTransform( 239 | from_frame, 240 | to_frame, t) 241 | except: 242 | rospy.logerr('Could not get the end-effector pose.') 243 | 244 | return Point(position[0], position[1], position[2]) 245 | 246 | ## Callback function for receiving gaze commands 247 | def _execute_gaze_action(self, goal): 248 | '''Get 3DOF pose of end effector 249 | 250 | Args: 251 | goal (GazeGoal): what type of action to perform next 252 | ''' 253 | # rospy.loginfo("Got a head goal: {}".format(goal.action)) 254 | command = goal.action 255 | if self._no_interrupt.count(self._current_gaze_action) == 0: 256 | if (self._current_gaze_action != command or 257 | command == GazeGoal.LOOK_AT_POINT): 258 | self._is_action_complete = False 259 | if command == GazeGoal.LOOK_FORWARD: 260 | self._target_focus_point = self._default_focus_point 261 | elif command == GazeGoal.LOOK_DOWN: 262 | self._target_focus_point = self._down_focus_point 263 | elif command == GazeGoal.NOD: 264 | self._n_nods = goal.repeat 265 | self._start_nod() 266 | elif command == GazeGoal.SHAKE: 267 | self._n_shakes = goal.repeat 268 | self._start_shake() 269 | elif command == GazeGoal.GLANCE_EE: 270 | self._start_glance() 271 | elif command == GazeGoal.LOOK_AT_POINT: 272 | self._target_focus_point = goal.point 273 | # rospy.loginfo('\tSetting gaze action to: ' + 274 | # self.gaze_goal_strs[command]) 275 | self._current_gaze_action = command 276 | 277 | while not self._is_action_complete: 278 | time.sleep(0.1) 279 | self._current_gaze_action = GazeGoal.NONE 280 | # Perturb the head goal so it gets updated in the update loop. 281 | self._head_goal.target.point.x += 1 282 | self._gaze_action_server.set_succeeded() 283 | else: 284 | self._gaze_action_server.set_aborted() 285 | 286 | else: 287 | self._gaze_action_server.set_aborted() 288 | 289 | def _start_nod(self): 290 | '''Start nod action''' 291 | self._prev_target_focus_point = self._target_focus_point 292 | self._prev_gaze_action = str(self._current_gaze_action) 293 | self._nod_counter = 0 294 | self._target_focus_point = self._nod_positions[0] 295 | 296 | def _start_glance(self): 297 | '''Start glance action''' 298 | self._prev_target_focus_point = self._target_focus_point 299 | self._prev_gaze_action = str(self._current_gaze_action) 300 | self._glance_counter = 0 301 | self._target_focus_point = self._get_ee_pos() 302 | 303 | def _start_shake(self): 304 | '''Start shake action''' 305 | self._prev_target_focus_point = self._target_focus_point 306 | self._prev_gaze_action = str(self._current_gaze_action) 307 | self._shake_counter = 0 308 | self._target_focus_point = self._shake_positions[0] 309 | 310 | def _get_next_nod_point(self, current, target): 311 | '''Get next point to look at while nodding 312 | 313 | Args: 314 | current (Point): current head goal 315 | target (Point): new head goal 316 | 317 | Returns: 318 | Point 319 | ''' 320 | if (SocialGaze._is_the_same(SocialGaze._point2array(current), 321 | SocialGaze._point2array(target))): 322 | self._nod_counter += 1 323 | if self._nod_counter == self._n_nods: 324 | self._current_gaze_action = self._prev_gaze_action 325 | return self._prev_target_focus_point 326 | else: 327 | return self._nod_positions[self._nod_counter % 2] 328 | else: 329 | return target 330 | 331 | def _get_next_glance_point(self, current, target): 332 | '''Get next point to look at while glancing 333 | 334 | Args: 335 | current (Point): current head goal 336 | target (Point): new head goal 337 | 338 | Returns: 339 | Point 340 | ''' 341 | if (SocialGaze._is_the_same(SocialGaze._point2array(current), 342 | SocialGaze._point2array(target))): 343 | self._glance_counter = 1 344 | self._current_gaze_action = self._prev_gaze_action 345 | return self._prev_target_focus_point 346 | else: 347 | return target 348 | 349 | def _get_next_shake_point(self, current, target): 350 | '''Get next point to look at while shaking 351 | 352 | Args: 353 | current (Point): current head goal 354 | target (Point): new head goal 355 | 356 | Returns: 357 | Point 358 | ''' 359 | if (SocialGaze._is_the_same(SocialGaze._point2array(current), 360 | SocialGaze._point2array(target))): 361 | self._shake_counter += 1 362 | if self._shake_counter == self._n_shakes: 363 | self._current_gaze_action = self._prev_gaze_action 364 | return self._prev_target_focus_point 365 | else: 366 | return self._shake_positions[self._shake_counter % 2] 367 | else: 368 | return target 369 | 370 | if __name__ == '__main__': 371 | rospy.init_node('social_gaze') 372 | gaze = SocialGaze() 373 | while not rospy.is_shutdown(): 374 | gaze.update() 375 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/src/fetch_pbd_interaction/robot.py: -------------------------------------------------------------------------------- 1 | '''Robot class that provides an interface to arm and head (maybe base later)''' 2 | 3 | # ###################################################################### 4 | # Imports 5 | # ###################################################################### 6 | 7 | # Core ROS imports come first. 8 | import rospy 9 | import roslib 10 | 11 | # System builtins 12 | import os 13 | 14 | # ROS builtins 15 | from sound_play.libsoundplay import SoundClient 16 | from std_srvs.srv import Empty 17 | from actionlib import SimpleActionClient 18 | from actionlib_msgs.msg import GoalStatus 19 | 20 | # Local 21 | from fetch_pbd_interaction.srv import MoveArm, GetGripperState, \ 22 | SetGripperState, GetArmMovement, \ 23 | GetEEPose, GetJointStates, GetGazeGoal, \ 24 | GetNearestObject, MoveArmTraj 25 | from fetch_social_gaze.msg import GazeGoal, GazeAction 26 | from fetch_pbd_interaction.msg import ArmState, Landmark 27 | from fetch_arm_control.msg import GripperState 28 | 29 | # ###################################################################### 30 | # Module level constants 31 | # ###################################################################### 32 | 33 | # String for base link name 34 | BASE_LINK = 'base_link' 35 | 36 | PKG = 'fetch_pbd_interaction' 37 | 38 | # Directory sounds are in, within the package. 39 | SOUNDS_DIR = os.path.join(roslib.packages.get_pkg_dir(PKG), 'sounds', '') 40 | 41 | # Common postfix for sound files. 42 | SOUND_FILEFORMAT = '.wav' 43 | 44 | # ###################################################################### 45 | # Classes 46 | # ###################################################################### 47 | 48 | class Robot: 49 | '''Robot class that provides an interface to arm 50 | and head (maybe base later)''' 51 | 52 | def __init__(self, tf_listener, play_sound=True, social_gaze=True): 53 | ''' 54 | Args: 55 | tf_listener (TransformListener) 56 | ''' 57 | self._social_gaze = social_gaze 58 | self._play_sound = play_sound 59 | self._tf_listener = tf_listener 60 | 61 | # arm services 62 | self.move_arm_to_joints_plan_srv = rospy.ServiceProxy( 63 | '/fetch_pbd/move_arm_to_joints_plan', 64 | MoveArm) 65 | rospy.wait_for_service('/fetch_pbd/move_arm_to_joints_plan') 66 | 67 | self.move_arm_to_joints_srv = rospy.ServiceProxy( 68 | '/fetch_pbd/move_arm_to_joints', 69 | MoveArmTraj) 70 | rospy.wait_for_service('/fetch_pbd/move_arm_to_joints') 71 | 72 | self.move_arm_to_pose_srv = rospy.ServiceProxy( 73 | '/fetch_pbd/move_arm_to_pose', 74 | MoveArm) 75 | rospy.wait_for_service('/fetch_pbd/move_arm_to_pose') 76 | 77 | self.start_move_arm_to_pose_srv = rospy.ServiceProxy( 78 | '/fetch_pbd/start_move_arm_to_pose', 79 | MoveArm) 80 | rospy.wait_for_service('/fetch_pbd/start_move_arm_to_pose') 81 | 82 | self.is_reachable_srv = rospy.ServiceProxy('/fetch_pbd/is_reachable', MoveArm) 83 | rospy.wait_for_service('/fetch_pbd/is_reachable') 84 | 85 | self.is_arm_moving_srv = rospy.ServiceProxy( 86 | '/fetch_pbd/is_arm_moving', GetArmMovement) 87 | rospy.wait_for_service('/fetch_pbd/is_arm_moving') 88 | 89 | self.relax_arm_srv = rospy.ServiceProxy('/fetch_pbd/relax_arm', Empty) 90 | rospy.wait_for_service('/fetch_pbd/relax_arm') 91 | 92 | self.reset_arm_movement_history_srv = rospy.ServiceProxy( 93 | '/fetch_pbd/reset_arm_movement_history', 94 | Empty) 95 | rospy.wait_for_service('/fetch_pbd/reset_arm_movement_history') 96 | 97 | self.get_gripper_state_srv = rospy.ServiceProxy( 98 | '/fetch_pbd/get_gripper_state', 99 | GetGripperState) 100 | rospy.wait_for_service('/fetch_pbd/get_gripper_state') 101 | 102 | self.get_joint_states_srv = rospy.ServiceProxy( 103 | '/fetch_pbd/get_joint_states', 104 | GetJointStates) 105 | rospy.wait_for_service('/fetch_pbd/get_joint_states') 106 | 107 | self.get_ee_pose_srv = rospy.ServiceProxy('/fetch_pbd/get_ee_pose', 108 | GetEEPose) 109 | rospy.wait_for_service('/fetch_pbd/get_ee_pose') 110 | 111 | self.set_gripper_state_srv = rospy.ServiceProxy( 112 | '/fetch_pbd/set_gripper_state', 113 | SetGripperState) 114 | rospy.wait_for_service('/fetch_pbd/set_gripper_state') 115 | 116 | rospy.loginfo("Got all arm services") 117 | 118 | # head services 119 | 120 | self.gaze_client = SimpleActionClient('/fetch_pbd/gaze_action', 121 | GazeAction) 122 | 123 | self.gaze_client.wait_for_server(rospy.Duration(5)) 124 | 125 | self.current_gaze_goal_srv = rospy.ServiceProxy( 126 | '/fetch_pbd/get_current_gaze_goal', 127 | GetGazeGoal) 128 | rospy.wait_for_service('/fetch_pbd/get_current_gaze_goal') 129 | 130 | rospy.loginfo("Got all head services") 131 | 132 | # world services 133 | 134 | self._get_nearest_object_srv = rospy.ServiceProxy( 135 | '/fetch_pbd/get_nearest_object', 136 | GetNearestObject) 137 | rospy.wait_for_service('/fetch_pbd/get_nearest_object') 138 | 139 | rospy.loginfo("Got all world services") 140 | 141 | 142 | # sound stuff 143 | self._sound_client = SoundClient() 144 | 145 | # arm stuff 146 | 147 | def move_arm_to_pose(self, arm_state): 148 | '''Move robot's arm to pose 149 | 150 | Args: 151 | arm_state (ArmState) : contains pose info 152 | Return: 153 | bool : success 154 | ''' 155 | try: 156 | result = self.move_arm_to_pose_srv(arm_state).success 157 | except Exception, e: 158 | result = False 159 | return result 160 | 161 | def start_move_arm_to_pose(self, arm_state): 162 | '''Start thread to move robot's arm to pose 163 | 164 | Args: 165 | arm_state (ArmState) : contains pose info 166 | Return: 167 | bool : success 168 | ''' 169 | # TODO(sarah): Is this really necessary? Investigate. 170 | try: 171 | result = self.start_move_arm_to_pose_srv(arm_state).success 172 | except Exception, e: 173 | result = False 174 | return result 175 | 176 | def move_arm_to_joints_plan(self, arm_state): 177 | '''Move robot's arm to joints positions from arm_state 178 | using Moveit to plan 179 | 180 | Args: 181 | arm_state (ArmState) : contains joint position info 182 | Return: 183 | bool : success 184 | ''' 185 | try: 186 | result = self.move_arm_to_joints_plan_srv(arm_state).success 187 | except Exception, e: 188 | result = False 189 | return result 190 | def move_arm_to_joints(self, arm_states, times): 191 | '''Move robot's arm to joints positions from arm_state 192 | without planning, just joint interpolation 193 | 194 | Args: 195 | arm_states ([ArmState]) : contains joint position info 196 | Return: 197 | bool : success 198 | ''' 199 | try: 200 | result = self.move_arm_to_joints_srv(arm_states, times).success 201 | except Exception, e: 202 | result = False 203 | return result 204 | 205 | def can_reach(self, arm_state): 206 | '''Returns whether arm can reach pose 207 | 208 | Args: 209 | arm_state (ArmState) : contains pose info 210 | Return: 211 | bool : success 212 | ''' 213 | try: 214 | result = self.is_reachable_srv(arm_state).success 215 | except Exception, e: 216 | result = False 217 | return result 218 | 219 | def reset_arm_movement_history(self): 220 | '''Resets movement history of arm''' 221 | try: 222 | self.reset_arm_movement_history_srv() 223 | except Exception, e: 224 | pass 225 | return 226 | 227 | def get_gripper_state(self): 228 | '''Returns state of gripper 229 | 230 | Returns: 231 | GripperState.OPEN|GripperState.CLOSED 232 | ''' 233 | try: 234 | result = self.get_gripper_state_srv().gripper_state 235 | except Exception, e: 236 | # defaults to closed, but maybe not a good idea 237 | result = GripperState.CLOSED 238 | return result 239 | 240 | def set_gripper_state(self, gripper_state): 241 | '''Sets state of gripper. Assumed to succeed 242 | 243 | Args: 244 | gripper_state (GripperState.OPEN|GripperState.CLOSED) 245 | ''' 246 | try: 247 | self.set_gripper_state_srv(gripper_state) 248 | except Exception, e: 249 | pass 250 | return 251 | 252 | def get_arm_state(self): 253 | '''Returns current state of arm 254 | 255 | Returns: 256 | ArmState 257 | ''' 258 | abs_ee_pose = self.get_ee_pose_srv().ee_pose # (PoseStamped) 259 | joint_pose = self.get_joint_states_srv().joints # ([float64]) 260 | 261 | state = None 262 | rel_ee_pose = None 263 | try: 264 | resp = self._get_nearest_object_srv( 265 | abs_ee_pose) 266 | has_nearest = resp.has_nearest 267 | except Exception, e: 268 | rospy.logwarn(str(e)) 269 | has_nearest = False 270 | 271 | if not has_nearest: 272 | # Arm state is absolute (relative to robot's base_link). 273 | state = ArmState( 274 | ArmState.ROBOT_BASE, # ref_frame (uint8) 275 | abs_ee_pose, # ee_pose (PoseStamped) 276 | joint_pose, # joint_pose ([float64]) 277 | [], # velocities 278 | Landmark() # ref_frame_landmark (Landmark) 279 | ) 280 | else: 281 | nearest_obj = resp.nearest_object 282 | # Arm state is relative (to some object in the world). 283 | # rospy.loginfo("Relative to: {}".format(nearest_obj.name)) 284 | 285 | rel_ee_pose = self._tf_listener.transformPose( 286 | nearest_obj.name, abs_ee_pose) 287 | 288 | state = ArmState( 289 | ArmState.OBJECT, # ref_frame (uint8) 290 | rel_ee_pose, # ee_pose (PoseStamped) 291 | joint_pose, # joint_pose [float64] 292 | [], # velocities 293 | nearest_obj # ref_frameLandmark (Landmark) 294 | ) 295 | 296 | return state 297 | 298 | def relax_arm(self): 299 | '''Make sure gravity compensation controller is on and other 300 | controllers are off 301 | ''' 302 | try: 303 | self.relax_arm_srv() 304 | except Exception, e: 305 | pass 306 | return 307 | 308 | def is_arm_moving(self): 309 | '''Check if arm is currently moving 310 | 311 | Returns: 312 | bool : True if arm is moving, else False 313 | ''' 314 | try: 315 | result = self.is_arm_moving_srv().moving 316 | except Exception, e: 317 | result = False 318 | return result 319 | 320 | # Head stuff 321 | 322 | def shake_head(self, num=5): 323 | '''Shakes robot's head 324 | 325 | Args: 326 | num (int) : number of times to perform action 327 | ''' 328 | rospy.loginfo("Head shake requested") 329 | if self._social_gaze: 330 | try: 331 | goal = GazeGoal() 332 | goal.action = GazeGoal.SHAKE 333 | goal.repeat = num 334 | current_goal = self.current_gaze_goal_srv().gaze_goal 335 | if goal.action != current_goal: 336 | self.gaze_client.send_goal(goal) 337 | self.gaze_client.wait_for_result() 338 | rospy.loginfo("Gaze result: {}".format(self.gaze_client.get_result())) 339 | else: 340 | rospy.loginfo("Gaze goal is same as previous") 341 | except Exception, e: 342 | rospy.logwarn("Fetch social gaze exception: {}".format(str(e))) 343 | 344 | def nod_head(self, num=5): 345 | '''Nods robot's head 346 | 347 | Args: 348 | num (int) : number of times to perform action 349 | ''' 350 | rospy.loginfo("Head nod requested") 351 | if self._social_gaze: 352 | try: 353 | goal = GazeGoal() 354 | goal.action = GazeGoal.NOD 355 | goal.repeat = num 356 | current_goal = self.current_gaze_goal_srv().gaze_goal 357 | if goal.action != current_goal: 358 | self.gaze_client.send_goal(goal) 359 | self.gaze_client.wait_for_result() 360 | rospy.loginfo("Gaze result: {}".format(self.gaze_client.get_result())) 361 | else: 362 | rospy.loginfo("Gaze goal is same as previous") 363 | except Exception, e: 364 | rospy.logwarn("Fetch social gaze exception: {}".format(str(e))) 365 | 366 | def look_at_point(self, point): 367 | '''Points robot's head at point 368 | 369 | Args: 370 | point (Point) 371 | ''' 372 | if self._social_gaze: 373 | try: 374 | goal = GazeGoal() 375 | goal.action = GazeGoal.LOOK_AT_POINT 376 | goal.point = point 377 | current_goal = self.current_gaze_goal_srv().gaze_goal 378 | if goal.action != current_goal: 379 | self.gaze_client.send_goal(goal) 380 | except Exception, e: 381 | pass 382 | 383 | def look_at_ee(self, follow=True): 384 | '''Makes head look at (or follow) end effector position 385 | 386 | Args: 387 | follow (bool, optional) : If True, follow end effector, 388 | else, just glance at end effector 389 | ''' 390 | # rospy.loginfo("Look at ee") 391 | if self._social_gaze: 392 | try: 393 | goal = GazeGoal() 394 | if follow: 395 | goal.action = GazeGoal.FOLLOW_EE 396 | else: 397 | goal.action = GazeGoal.GLANCE_EE 398 | current_goal = self.current_gaze_goal_srv().gaze_goal 399 | if goal.action != current_goal: 400 | self.gaze_client.send_goal(goal) 401 | except Exception, e: 402 | pass 403 | 404 | def look_forward(self): 405 | '''Point head forward''' 406 | if self._social_gaze: 407 | try: 408 | goal = GazeGoal() 409 | goal.action = GazeGoal.LOOK_FORWARD 410 | current_goal = self.current_gaze_goal_srv().gaze_goal 411 | if goal.action != current_goal: 412 | self.gaze_client.send_goal(goal) 413 | except Exception, e: 414 | pass 415 | 416 | def look_down(self): 417 | '''Point head down at table''' 418 | # TODO(sarah): maybe actually scan for table instead of 419 | # looking at static point 420 | try: 421 | rospy.loginfo("Requesting to look down") 422 | goal = GazeGoal() 423 | goal.action = GazeGoal.LOOK_DOWN 424 | current_goal = self.current_gaze_goal_srv().gaze_goal 425 | if goal.action != current_goal: 426 | self.gaze_client.send_goal(goal) 427 | while (self.gaze_client.get_state() == GoalStatus.PENDING or 428 | self.gaze_client.get_state() == GoalStatus.ACTIVE): 429 | rospy.sleep(0.2) 430 | rospy.sleep(0.5) 431 | except Exception, e: 432 | rospy.logwarn("Gaze error: {}".format(e)) 433 | 434 | # Sound stuff 435 | 436 | def play_sound(self, requested_sound): 437 | '''Play sound that is requested 438 | 439 | Args: 440 | requested_sound (RobotSound.ERROR|etc...) : see RobotSound.msg 441 | ''' 442 | try: 443 | if self._play_sound: 444 | self._sound_client.playWave( 445 | os.path.join(SOUNDS_DIR, requested_sound + SOUND_FILEFORMAT)) 446 | except Exception, e: 447 | rospy.loginfo(e) 448 | -------------------------------------------------------------------------------- /fetch_pbd_interaction/src/fetch_pbd_interaction/arm_control.py: -------------------------------------------------------------------------------- 1 | '''Control of the arm for action execution.''' 2 | 3 | # ###################################################################### 4 | # Imports 5 | # ###################################################################### 6 | 7 | # Core ROS imports come first. 8 | import rospy 9 | 10 | # System builtins 11 | import threading 12 | 13 | # ROS builtins 14 | from std_srvs.srv import Empty, EmptyResponse 15 | from tf import TransformListener 16 | 17 | # Local 18 | from fetch_arm_control import Arm 19 | from fetch_arm_control.msg import GripperState 20 | from fetch_pbd_interaction.msg import ArmState, ExecutionStatus 21 | from fetch_pbd_interaction.srv import MoveArm, MoveArmResponse, \ 22 | GetEEPose, GetEEPoseResponse, \ 23 | GetGripperState, \ 24 | GetGripperStateResponse, \ 25 | SetGripperState, \ 26 | SetGripperStateResponse, \ 27 | GetJointStates, GetJointStatesResponse, \ 28 | GetArmMovement, GetArmMovementResponse, \ 29 | MoveArmTraj, MoveArmTrajResponse 30 | 31 | # ###################################################################### 32 | # Module level constants 33 | # ###################################################################### 34 | 35 | # The minimum arm movement that 'counts' as moved. 36 | # TODO(mbforbes): This is duplicated in arm.py. Use theirs. 37 | ARM_MOVEMENT_THRESHOLD = 0.04 38 | 39 | # How long to sleep between checking whether arms have successfully 40 | # completed their movement. 41 | MOVE_TO_JOINTS_SLEEP_INTERVAL = 0.01 # seconds 42 | 43 | # How long to sleep between checking whether arms have successfully 44 | # completed their trajectory. 45 | TRAJECTORY_COMPLETE_SLEEP_INTERVAL = 0.01 # seconds 46 | 47 | # How long to sleep between checking whether grippers have finished 48 | # opening/closing. 49 | GRIPPER_FINISH_SLEEP_INTERVAL = 0.01 # seconds 50 | 51 | 52 | # ###################################################################### 53 | # Classes 54 | # ###################################################################### 55 | 56 | class ArmControl: 57 | '''Higher-level interface that exposes capabilities from 58 | fetch_arm_control/Arm.py class as services. 59 | ''' 60 | 61 | def __init__(self): 62 | 63 | # Initialize arm state. 64 | self._tf_listener = TransformListener() 65 | self._arm = Arm(self._tf_listener) 66 | self._arm.close_gripper() 67 | self._status = ExecutionStatus.NOT_EXECUTING 68 | 69 | rospy.Service('/fetch_pbd/move_arm_to_joints_plan', MoveArm, 70 | self._move_to_joints_plan) 71 | rospy.Service('/fetch_pbd/move_arm_to_joints', MoveArmTraj, self._move_to_joints) 72 | 73 | rospy.Service('/fetch_pbd/move_arm_to_pose', MoveArm, self._move_to_pose) 74 | rospy.Service('/fetch_pbd/start_move_arm_to_pose', MoveArm, 75 | self._start_move_to_pose) 76 | 77 | rospy.Service('/fetch_pbd/is_reachable', MoveArm, self._is_reachable) 78 | rospy.Service('/fetch_pbd/is_arm_moving', GetArmMovement, self._is_arm_moving) 79 | 80 | 81 | rospy.Service('/fetch_pbd/relax_arm', Empty, self._relax_arm) 82 | 83 | rospy.Service('/fetch_pbd/reset_arm_movement_history', Empty, 84 | self._reset_movement_history) 85 | 86 | rospy.Service('/fetch_pbd/get_gripper_state', GetGripperState, 87 | self._get_gripper_state) 88 | rospy.Service('/fetch_pbd/get_ee_pose', GetEEPose, self._get_ee_pose) 89 | rospy.Service('/fetch_pbd/get_joint_states', GetJointStates, 90 | self._get_joint_states) 91 | 92 | rospy.Service('/fetch_pbd/set_gripper_state', SetGripperState, 93 | self._set_gripper_state) 94 | 95 | rospy.loginfo('Arm initialized.') 96 | 97 | # ################################################################## 98 | # Instance methods: Public (API) 99 | # ################################################################## 100 | 101 | def update(self): 102 | '''Periodic update for the arm.''' 103 | self._arm.update() 104 | 105 | # ################################################################## 106 | # Instance methods: Internal ("private") 107 | # ################################################################## 108 | 109 | def _reset_movement_history(self, req): 110 | ''' 111 | Args: 112 | req (EmptyRequest) 113 | Returns: 114 | EmptyResponse 115 | ''' 116 | 117 | self._arm.reset_movement_history() 118 | return EmptyResponse() 119 | 120 | def _set_gripper_state(self, req): 121 | '''Set gripper to gripper_state 122 | (open or closed). 123 | 124 | Args: 125 | req (SetGripperStateRequest) 126 | 127 | Returns: 128 | SetGripperStateResponse 129 | ''' 130 | gripper_state = req.gripper_state 131 | if gripper_state == self._arm.get_gripper_state(): 132 | # Already in that mode; do nothing and return False. 133 | return SetGripperStateResponse() 134 | else: 135 | # Change gripper mode. 136 | if gripper_state == GripperState.OPEN: 137 | self._arm.open_gripper() 138 | else: 139 | self._arm.close_gripper() 140 | return SetGripperStateResponse() 141 | 142 | def _is_reachable(self, req): 143 | '''. 144 | 145 | Args: 146 | req (MoveArmRequest): The arm's state 147 | 148 | Returns: 149 | MoveArmResponse 150 | ''' 151 | arm_state = req.arm_state 152 | solution, reachable = self._solve_ik_for_arm(arm_state) 153 | 154 | return MoveArmResponse(reachable) 155 | 156 | def _solve_ik_for_arm(self, arm_state, z_offset=0.0): 157 | '''Finds an IK solution for a particular arm pose. 158 | 159 | Args: 160 | arm_state (ArmState): The arm's state, 161 | z_offset (float, optional): Offset to add to z-values of 162 | pose positions. Defaults to 0.0. 163 | 164 | Returns: 165 | (ArmState, bool): Tuple of 166 | 167 | the ArmState, which will be an updated ArmState object 168 | with IK solved if an IK solution was found. If IK was 169 | not found, what is returned will depend on whether the 170 | reference frame is relative (ArmState.OBJECT) or 171 | absolute (ArmState.ROBOT_BASE). If the reference frame 172 | is relative and no IK was found, an empty ArmState is 173 | returned. If the reference frame is absolute and no IK 174 | was found, then the original passed arm_state is 175 | returned; 176 | 177 | the bool, which is whether an IK solution was 178 | successfully found. 179 | ''' 180 | # Ideally we need to find IK only if the frame is relative to an 181 | # object, but users can edit absolute poses in the GUI to make 182 | # them unreachable, so we try IK for absolute poses too. 183 | 184 | if arm_state.ref_type == ArmState.OBJECT: 185 | # Arm is relative. 186 | solution = ArmState() 187 | 188 | target_pose = self._tf_listener.transformPose( 189 | 'base_link', arm_state.ee_pose) 190 | target_pose.pose.position.z = target_pose.pose.position.z + \ 191 | z_offset 192 | 193 | # Try solving IK. 194 | target_joints = self._arm.get_ik_for_ee( 195 | target_pose, arm_state.joint_pose) 196 | 197 | # Check whether solution found. 198 | if target_joints is None: 199 | # No solution: RETURN EMPTY ArmState. 200 | rospy.loginfo('No IK for relative end-effector pose.') 201 | return solution, False 202 | else: 203 | # Found a solution; update the solution arm_state and 204 | # return. 205 | solution.ref_type = ArmState.ROBOT_BASE 206 | solution.ee_pose = target_pose 207 | solution.joint_pose = target_joints 208 | return solution, True 209 | elif arm_state.ref_type == ArmState.ROBOT_BASE: 210 | # Arm is absolute. 211 | 212 | target_pose = arm_state.ee_pose 213 | target_pose.pose.position.z = target_pose.pose.position.z + \ 214 | z_offset 215 | 216 | # Try solving IK. 217 | target_joints = self._arm.get_ik_for_ee( 218 | target_pose, arm_state.joint_pose) 219 | if target_joints is None: 220 | # No IK found; return the original. 221 | rospy.logdebug('No IK for absolute end-effector pose.') 222 | return arm_state, False 223 | else: 224 | # IK found; fill in solution ArmState and return. 225 | solution = ArmState() 226 | solution.ref_type = ArmState.ROBOT_BASE 227 | solution.ee_pose = target_pose 228 | solution.joint_pose = target_joints 229 | return solution, True 230 | elif arm_state.ref_type == ArmState.PREVIOUS_TARGET: 231 | # Arm is relative to previous primitive 232 | 233 | target_pose = arm_state.ee_pose 234 | target_pose.pose.position.z = target_pose.pose.position.z + \ 235 | z_offset 236 | 237 | # Try solving IK. 238 | target_joints = self._arm.get_ik_for_ee( 239 | target_pose, arm_state.joint_pose) 240 | if target_joints is None: 241 | # No IK found; return the original. 242 | rospy.logdebug('No IK for end-effector pose' + 243 | 'relative to previous target.') 244 | return arm_state, False 245 | else: 246 | # IK found; fill in solution ArmState and return. 247 | solution = ArmState() 248 | solution.ref_type = ArmState.PREVIOUS_TARGET 249 | solution.ee_pose = target_pose 250 | solution.joint_pose = target_joints 251 | return solution, True 252 | else: 253 | return arm_state, True 254 | 255 | def _get_joint_states(self, req): 256 | '''Get joint positions. 257 | 258 | Args: 259 | req (GetJointStatesRequest) 260 | 261 | Returns: 262 | GetJointStatesResponse 263 | ''' 264 | 265 | return GetJointStatesResponse(self._arm.get_joint_state()) 266 | 267 | def _get_gripper_state(self, req): 268 | ''' Get gripper status on the indicated side. 269 | 270 | Args: 271 | req (GetGripperStateRequest) 272 | 273 | Returns: 274 | GetGripperStateResponse 275 | ''' 276 | return GetGripperStateResponse(self._arm.get_gripper_state()) 277 | 278 | 279 | def _get_ee_pose(self, req): 280 | ''' Get current pose of the arm's end-effector on the indicated 281 | side. 282 | 283 | Args: 284 | req (GetEEPoseRequest) 285 | 286 | Returns: 287 | GetEEPoseResponse 288 | ''' 289 | return GetEEPoseResponse(self._arm.get_ee_state()) 290 | 291 | def _relax_arm(self, req): 292 | ''' Turns on gravity comp controller and turns off other controllers 293 | 294 | Args: 295 | req (EmptyRequest): Unused 296 | 297 | Returns: 298 | EmptyResponse 299 | ''' 300 | 301 | self._arm.relax_arm() 302 | 303 | return EmptyResponse() 304 | 305 | def _start_move_to_pose(self, req): 306 | '''Creates a thread for moving to a target pose. 307 | 308 | Args: 309 | req (MoveArmRequest): Arm state that contains the pose to 310 | move to. 311 | ''' 312 | thread = threading.Thread( 313 | group=None, 314 | target=self._move_to_pose, 315 | args=(req,), 316 | name='move_to_arm_state_thread' 317 | ) 318 | thread.start() 319 | 320 | # Log 321 | rospy.loginfo('Started thread to move arm.') 322 | 323 | return MoveArmResponse(True) 324 | 325 | def _move_to_pose(self, req): 326 | '''The thread function that makes the arm move to the 327 | target end-effector pose (within arm_state). 328 | 329 | Args: 330 | req (MoveArmRequest): Arm state that contains the pose to 331 | move to. 332 | ''' 333 | arm_state = req.arm_state 334 | rospy.loginfo("Moving to pose: {}".format(arm_state.ee_pose)) 335 | self._status = ExecutionStatus.EXECUTING 336 | 337 | # Should we transform pose to base_link? 338 | if self._arm.move_to_pose(arm_state.ee_pose): 339 | self._status = ExecutionStatus.SUCCEEDED 340 | success = True 341 | else: 342 | self._status = ExecutionStatus.NO_IK 343 | success = False 344 | 345 | self._arm.relax_arm() 346 | return MoveArmResponse(success) 347 | 348 | def _move_to_joints_plan(self, req): 349 | ''' 350 | Makes the arms move to the joint positions contained in the 351 | passed arm states. 352 | 353 | This assumes that the joint positions are valid (i.e. IK has 354 | been called previously to set each ArmState's joints to good 355 | values). 356 | 357 | Args: 358 | req (MoveArmRequest) 359 | 360 | Returns: 361 | MoveArmResponse: Whether the arms successfully moved to the passed 362 | joint positions. 363 | ''' 364 | # Estimate times to get to both poses. 365 | arm_state = req.arm_state 366 | 367 | # Move arms to target. 368 | 369 | self._status = ExecutionStatus.EXECUTING 370 | suc = self._arm.move_to_joints_plan(arm_state.joint_pose, 371 | arm_state.velocities) 372 | 373 | 374 | # Wait until both arms complete the trajectory. 375 | while self._arm.is_executing(): 376 | rospy.sleep(MOVE_TO_JOINTS_SLEEP_INTERVAL) 377 | 378 | rospy.loginfo('\tArms reached target.') 379 | 380 | # Verify that both arms succeeded 381 | # DEBUG: remove 382 | 383 | if not suc: 384 | self._status = ExecutionStatus.NO_IK 385 | success = False 386 | else: 387 | self._status = ExecutionStatus.SUCCEEDED 388 | success = True 389 | 390 | self._arm.relax_arm() 391 | return MoveArmResponse(success) 392 | 393 | def _move_to_joints(self, req): 394 | ''' 395 | Makes the arms move to the joint positions contained in the 396 | passed arm states. 397 | 398 | Note: This moves directly to the joint positions using interpolation 399 | 400 | This assumes that the joint positions are valid (i.e. IK has 401 | been called previously to set each ArmState's joints to good 402 | values). 403 | 404 | Args: 405 | req (MoveArmRequest) 406 | 407 | Returns: 408 | MoveArmResponse: Whether the arms successfully moved to the passed 409 | joint positions. 410 | ''' 411 | # Estimate times to get to both poses. 412 | joints = [] 413 | # times_to_poses = [] 414 | # arm_state = req.arm_state[i] 415 | 416 | # time_to_pose = None 417 | 418 | # if arm_state is not None: 419 | # time_to_pose = self._arm.get_time_to_pose(arm_state.ee_pose) 420 | for arm_state in req.arm_states: 421 | joints.append(arm_state.joint_pose) 422 | 423 | # If both arms are moving, adjust velocities and find most 424 | # moving arm. Look at it. 425 | 426 | # Move arms to target. 427 | suc = False 428 | self._status = ExecutionStatus.EXECUTING 429 | suc = self._arm.move_to_joints( 430 | joints, req.times) 431 | 432 | # Wait until both arms complete the trajectory. 433 | while self._arm.is_executing(): 434 | rospy.sleep(MOVE_TO_JOINTS_SLEEP_INTERVAL) 435 | 436 | rospy.loginfo('\tArms reached target.') 437 | 438 | # Verify that both arms succeeded 439 | # DEBUG: remove 440 | 441 | if not suc: 442 | self._status = ExecutionStatus.NO_IK 443 | success = False 444 | else: 445 | self._status = ExecutionStatus.SUCCEEDED 446 | success = True 447 | 448 | self._arm.relax_arm() 449 | return MoveArmTrajResponse(success) 450 | 451 | 452 | def _is_arm_moving(self, req): 453 | ''' 454 | Returns true is arm is moving 455 | 456 | Args: 457 | req (IsArmMovingRequest) 458 | 459 | Returns: 460 | IsArmMovingResponse: Whether the arm is currently moving 461 | ''' 462 | 463 | if self._arm.get_movement() < ARM_MOVEMENT_THRESHOLD: 464 | return GetArmMovementResponse(False) 465 | else: 466 | return GetArmMovementResponse(True) 467 | --------------------------------------------------------------------------------