Merge pull request #960 from tue-robotics/rwc2019_challenge_set_the_t…

…able

[RWC2019] challenge set the table

challenge_set_the_table/CMakeLists.txt

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(challenge_set_the_table)
+
+find_package(catkin REQUIRED)
+
+catkin_python_setup()
+
+catkin_package()
+
+if (CATKIN_ENABLE_TESTING)
+  catkin_add_nosetests(test)
+endif()

challenge_set_the_table/README.md

@@ -0,0 +1,36 @@
+# Challenge set the table
+
+_Responsible: Rein_
+
+## Scenario (RWC2019)
+
+1. Start:
+
+```
+Robot starts at the entrance door
+```
+
+2. Opening the cupboard drawer:
+
+```
+Robot drives towards the cupboard, positiones itself and tries to open the cupboard drawer.
+```
+
+3. Main loop
+
+```
+Robot picks an item from the cupboard drawer. Robot drives to the table. Robot places item at the correct position on the table.
+```
+
+4. Outro
+
+```
+Robo closes the cupboard drawer and leaves the arena.
+```
+
+## Testing / Running
+
+```
+robot-start
+robot-challenge-set-the-table
+```

challenge_set_the_table/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>challenge_set_the_table</name>
+  <version>0.0.0</version>
+  <description>The robot has to set the table (presumably for dinner) for one person.</description>
+
+  <maintainer email="reinzor@gmail.com">Rein Appeldoorn</maintainer>
+
+  <license>TODO</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>robot_skills</exec_depend>
+  <exec_depend>robot_smach_states</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>smach</exec_depend>
+  <exec_depend>tf</exec_depend>
+  <exec_depend>tf_conversions</exec_depend>
+  <exec_depend>tf_conversions</exec_depend>
+</package>

challenge_set_the_table/scripts/challenge_set_the_table

@@ -0,0 +1,21 @@
+#!/usr/bin/env python
+#
+# Copyright (c) 2019, TU/e Robotics, Netherlands
+# All rights reserved.
+#
+# \author Rein Appeldoorn
+
+from __future__ import print_function
+
+import os
+
+import rospy
+
+from challenge_set_the_table.set_the_table import setup_statemachine
+from robot_smach_states.util import startup
+
+
+if __name__ == '__main__':
+    challenge_name = os.path.basename(__file__)
+    rospy.init_node(challenge_name)
+    startup(setup_statemachine, challenge_name=challenge_name)

challenge_set_the_table/setup.py

@@ -0,0 +1,16 @@
+#!/usr/bin/env python
+#
+# Copyright (c) 2019, TU/e Robotics, Netherlands
+# All rights reserved.
+#
+# \author Rein Appeldoorn
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['challenge_set_the_table'],
+    package_dir={'': 'src'}
+)
+
+setup(**d)

challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_close_cupboard_drawer.py

@@ -0,0 +1,112 @@
+#
+# Copyright (c) 2019, TU/e Robotics, Netherlands
+# All rights reserved.
+#
+# \author Rein Appeldoorn
+
+import math
+import os
+
+import rospy
+from geometry_msgs.msg import PoseStamped, Quaternion
+from robot_skills import Hero
+from robot_smach_states import NavigateToSymbolic
+from robot_smach_states.navigation.control_to_pose import ControlParameters, ControlToPose
+from robot_smach_states.util.designators import EdEntityDesignator
+from smach import StateMachine, cb_interface, CBState
+from tf.transformations import quaternion_from_euler
+
+
+class CloseCupboard(StateMachine):
+    def __init__(self, robot, cupboard_id):
+        StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
+        arm = robot.get_arm()._arm
+
+        def send_joint_goal(position_array, wait_for_motion_done=True):
+            arm._send_joint_trajectory([position_array], timeout=rospy.Duration(0))
+            if wait_for_motion_done:
+                arm.wait_for_motion_done()
+
+        def send_gripper_goal(open_close_string):
+            arm.send_gripper_goal(open_close_string)
+            rospy.sleep(1.0)  # Does not work with motion_done apparently
+
+        @cb_interface(outcomes=['done'])
+        def _pre_grab_handle(_):
+            arm.send_gripper_goal("open", timeout=0)
+            send_joint_goal([0, -0, 0, -1.57, 1.57])
+            return 'done'
+
+        @cb_interface(outcomes=['done'])
+        def _align_with_cupboard(_):
+            goal_pose = PoseStamped()
+            goal_pose.header.stamp = rospy.Time.now()
+            goal_pose.header.frame_id = cupboard_id
+            goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, math.pi))
+            goal_pose.pose.position.x = 1.1
+            goal_pose.pose.position.y = 0
+            ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02, 0.1)).execute({})
+            return 'done'
+
+        @cb_interface(outcomes=['done'])
+        def _grab_handle(_):
+            robot.speech.speak('I hope this goes right!', block=False)
+            send_joint_goal([0, -0.5, 0, -1.07, 1.57])
+            send_gripper_goal("close")
+            return 'done'
+
+        @cb_interface(outcomes=['done'])
+        def _drive_to_close_cupboard(_):
+            goal_pose = PoseStamped()
+            goal_pose.header.stamp = rospy.Time.now()
+            goal_pose.header.frame_id = cupboard_id
+            goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, math.pi))
+            goal_pose.pose.position.x = 0.8
+            goal_pose.pose.position.y = 0
+            ControlToPose(robot, goal_pose, ControlParameters(0.8, 1.0, 0.15, 0.1, 0.1, 0.05, 0.5)).execute({})
+            return 'done'
+
+        @cb_interface(outcomes=['done'])
+        def _retract(_):
+            send_gripper_goal("open")
+            goal_pose = PoseStamped()
+            goal_pose.header.stamp = rospy.Time.now()
+            goal_pose.header.frame_id = cupboard_id
+            goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, math.pi))
+            goal_pose.pose.position.x = 1.0
+            goal_pose.pose.position.y = 0
+            ControlToPose(robot, goal_pose, ControlParameters(0.8, 1.0, 0.15, 0.1, 0.1, 0.05, 0.5)).execute({})
+            arm.send_joint_goal("carrying_pose")
+            return "done"
+
+        with self:
+            self.add_auto('PRE_GRAB_HANDLE', CBState(_pre_grab_handle), ['done'])
+            self.add_auto('ALIGN_WITH_CUPBOARD', CBState(_align_with_cupboard), ['done'])
+            self.add_auto('GRAB_HANDLE', CBState(_grab_handle), ['done'])
+            self.add_auto('DRIVE_TO_CLOSE_CUPBOARD', CBState(_drive_to_close_cupboard), ['done'])
+            self.add('RETRACT', CBState(_retract), transitions={'done': 'succeeded'})
+
+
+class NavigateToAndCloseCupboard(StateMachine):
+    def __init__(self, robot, cupboard_id, cupboard_navigation_area):
+        StateMachine.__init__(self, outcomes=["succeeded", "failed"])
+
+        cupboard = EdEntityDesignator(robot=robot, id=cupboard_id)
+
+        with self:
+            StateMachine.add("NAVIGATE_TO_CUPBOARD",
+                             NavigateToSymbolic(robot, {cupboard: cupboard_navigation_area}, cupboard),
+                             transitions={'arrived': 'CLOSE_CUPBOARD',
+                                          'unreachable': 'failed',
+                                          'goal_not_defined': 'failed'})
+
+            StateMachine.add("CLOSE_CUPBOARD", CloseCupboard(robot, cupboard_id),
+                             transitions={'succeeded': 'succeeded',
+                                          'failed': 'failed'})
+
+
+if __name__ == '__main__':
+    rospy.init_node(os.path.splitext("test_" + os.path.basename(__file__))[0])
+    hero = Hero()
+    hero.reset()
+    NavigateToAndCloseCupboard(hero, 'cabinet', 'in_front_of').execute()

challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_open_cupboard_drawer.py

@@ -0,0 +1,101 @@
+#
+# Copyright (c) 2019, TU/e Robotics, Netherlands
+# All rights reserved.
+#
+# \author Rein Appeldoorn
+
+import math
+import os
+
+import rospy
+from robot_smach_states.navigation.control_to_pose import ControlParameters, ControlToPose
+from geometry_msgs.msg import PoseStamped, Quaternion
+from robot_skills import Hero
+from robot_smach_states import NavigateToSymbolic
+from robot_smach_states.util.designators import EdEntityDesignator
+from smach import StateMachine, cb_interface, CBState
+from tf.transformations import quaternion_from_euler
+
+
+class OpenCupboard(StateMachine):
+    def __init__(self, robot, cupboard_id):
+        StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
+        arm = robot.get_arm()._arm
+
+        def send_joint_goal(position_array, wait_for_motion_done=True):
+            arm._send_joint_trajectory([position_array], timeout=rospy.Duration(0))
+            if wait_for_motion_done:
+                arm.wait_for_motion_done()
+
+        def send_gripper_goal(open_close_string):
+            arm.send_gripper_goal(open_close_string)
+            rospy.sleep(1.0)  # Does not work with motion_done apparently
+
+        @cb_interface(outcomes=['done'])
+        def _pre_grab_handle(_):
+            arm.send_gripper_goal("open", timeout=0)
+            send_joint_goal([0, -0, 0, -1.57, 1.57])
+            return 'done'
+
+        @cb_interface(outcomes=['done'])
+        def _align_with_cupboard(_):
+            goal_pose = PoseStamped()
+            goal_pose.header.stamp = rospy.Time.now()
+            goal_pose.header.frame_id = cupboard_id
+            goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, math.pi))
+            goal_pose.pose.position.x = 0.85
+            goal_pose.pose.position.y = 0
+            ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02, 0.1)).execute({})
+            return 'done'
+
+        @cb_interface(outcomes=['done'])
+        def _grab_handle(_):
+            robot.speech.speak('I hope this goes right!', block=False)
+            send_joint_goal([0, -0.5, 0, -1.07, 1.57])
+            send_gripper_goal("close")
+            return 'done'
+
+        @cb_interface(outcomes=['done'])
+        def _drive_to_open_cupboard(_):
+            goal_pose = PoseStamped()
+            goal_pose.header.stamp = rospy.Time.now()
+            goal_pose.header.frame_id = cupboard_id
+            goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, math.pi))
+            goal_pose.pose.position.x = 1.4
+            goal_pose.pose.position.y = 0
+            ControlToPose(robot, goal_pose, ControlParameters(0.8, 1.0, 0.15, 0.1, 0.1, 0.05, 0.5)).execute({})
+
+            arm.send_joint_goal("carrying_pose")
+
+            return 'done'
+
+        with self:
+            self.add_auto('PRE_GRAB_HANDLE', CBState(_pre_grab_handle), ['done'])
+            self.add_auto('ALIGN_WITH_CUPBOARD', CBState(_align_with_cupboard), ['done'])
+            self.add_auto('GRAB_HANDLE', CBState(_grab_handle), ['done'])
+            self.add('DRIVE_TO_OPEN_CUPBOARD', CBState(_drive_to_open_cupboard), transitions={'done': 'succeeded'})
+
+
+class NavigateToAndOpenCupboard(StateMachine):
+    def __init__(self, robot, cupboard_id, cupboard_navigation_area):
+        StateMachine.__init__(self, outcomes=["succeeded", "failed"])
+
+        cupboard = EdEntityDesignator(robot=robot, id=cupboard_id)
+
+        with self:
+            StateMachine.add("NAVIGATE_TO_CUPBOARD",
+                             NavigateToSymbolic(robot, {cupboard: cupboard_navigation_area}, cupboard),
+                             transitions={'arrived': 'failed',
+                                          'unreachable': 'failed',
+                                          'goal_not_defined': 'failed'})
+
+            StateMachine.add("OPEN_CUPBOARD", OpenCupboard(robot, cupboard_id),
+                             transitions={'succeeded': 'succeeded',
+                                          'failed': 'failed'})
+
+
+if __name__ == '__main__':
+    rospy.init_node(os.path.splitext("test_" + os.path.basename(__file__))[0])
+    hero = Hero()
+    hero.reset()
+    NavigateToAndOpenCupboard(hero, 'cabinet', 'in_front_of').execute()