Merge branch 'master' into azure_ssh

Author: MatthijsBurgh

challenge_gpsr/src/gpsr.py

@@ -224,15 +224,9 @@ def main():
     skip        = rospy.get_param('~skip', False)
     restart     = rospy.get_param('~restart', False)
     robot_name  = rospy.get_param('~robot_name')
-    no_of_tasks = rospy.get_param('~number_of_tasks', 0)
+    no_of_tasks = rospy.get_param('~number_of_tasks', 999)
     test        = rospy.get_param('~test_mode', False)
-    eegpsr      = rospy.get_param('~eegpsr', False)
-    time_limit  = rospy.get_param('~time_limit', 0)
-    if no_of_tasks == 0:
-        no_of_tasks = 999
-
-    if time_limit == 0:
-        time_limit = 999
+    time_limit  = rospy.get_param('~time_limit', 999)
 
     rospy.loginfo("[GPSR] Parameters:")
     rospy.loginfo("[GPSR] robot_name = {}".format(robot_name))
@@ -246,14 +240,9 @@ def main():
         rospy.loginfo("[GPSR] running in test mode")
     rospy.loginfo("[GPSR] time_limit = {}".format(time_limit))
 
-    # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
 
     robot = get_robot(robot_name)
-
-    if eegpsr:
-        knowledge = load_knowledge('challenge_eegpsr')
-    else:
-        knowledge = load_knowledge('challenge_gpsr')
+    knowledge = load_knowledge('challenge_gpsr')
 
     conversation_engine = ConversationEngineWithHmi(robot, knowledge.grammar, knowledge.grammar_target, knowledge)
     conversation_engine.test = test

challenge_serve_breakfast/images/spoon.jpg

@@ -8,10 +8,15 @@
 import rospkg
 import rospy
 
-from challenge_serve_breakfast.tuning import REQUIRED_ITEMS, JOINTS_HANDOVER, PICK_ROTATION
+from challenge_serve_breakfast.tuning import REQUIRED_ITEMS, JOINTS_HANDOVER
 from robot_skills import get_robot
+from robot_skills.arm.arms import GripperTypes
+# ROS
+from pykdl_ros import VectorStamped
+from robot_smach_states.human_interaction import Say
+from robot_smach_states.manipulation.active_grasp_detector import ActiveGraspDetector
 from robot_smach_states.navigation import NavigateToSymbolic
-from robot_smach_states.util.designators import EdEntityDesignator
+from robot_smach_states.util.designators import EdEntityDesignator, ArmDesignator
 from smach import StateMachine, cb_interface, CBState
 
 item_img_dict = {
@@ -28,10 +33,11 @@ def __init__(self, robot):
         # noinspection PyProtectedMember
         arm = robot.get_arm()._arm
         picked_items = []
+        armdes = ArmDesignator(robot, {"required_gripper_types": [GripperTypes.GRASPING]})
 
         def send_joint_goal(position_array, wait_for_motion_done=True):
             # noinspection PyProtectedMember
-            arm._send_joint_trajectory([position_array], timeout=rospy.Duration(0))
+            arm._send_joint_trajectory([position_array], timeout=0.0)
             if wait_for_motion_done:
                 arm.wait_for_motion_done()
 
@@ -52,10 +58,17 @@ def show_image(package_name, path_to_image_in_package):
 
         @cb_interface(outcomes=["done"])
         def _rotate(_):
-            arm.gripper.send_goal("close", timeout=0.)
-            robot.head.look_up()
-            vyaw = 0.5
-            robot.base.force_drive(0, 0, vyaw, PICK_ROTATION / vyaw)
+            arm.gripper.send_goal("close", timeout=0.0)
+
+            # Look to the operator
+            robot.head.look_at_point(VectorStamped.from_xyz(0.2, -0.2, 1.75, stamp=rospy.Time.now(),
+                                                            frame_id=robot.base_link_frame))
+            robot.head.wait_for_motion_done()
+            return "done"
+
+        @cb_interface(outcomes=["done"])
+        def _handover_pose(user_data):
+            send_joint_goal(JOINTS_HANDOVER, wait_for_motion_done=False)
             return "done"
 
         @cb_interface(outcomes=["succeeded", "failed"], output_keys=["item_picked"])
@@ -67,29 +80,46 @@ def _ask_user(user_data):
 
             item_name = leftover_items[0]
 
-            send_joint_goal(JOINTS_HANDOVER, wait_for_motion_done=False)
-
-            picked_items.append(item_name)
-
             robot.speech.speak("Please put the {} in my gripper, like this".format(item_name), block=False)
             show_image("challenge_serve_breakfast", item_img_dict[item_name])
 
             send_gripper_goal("open")
-            rospy.sleep(10.0)
-            robot.speech.speak("Thanks for that!", block=False)
+            rospy.sleep(7.0)
             send_gripper_goal("close", max_torque=0.6)
             robot.head.reset()
 
             # Set output data
             user_data["item_picked"] = item_name
+            return "succeeded"
 
+        @cb_interface(outcomes=["done"])
+        def _carrying_pose(user_data):
             arm.send_joint_goal("carrying_pose", timeout=0.)
+            robot.speech.speak("Thanks for that!", block=False)
+            return "done"
 
-            return "succeeded"
+        @cb_interface(outcomes=["done"], input_keys=["item_picked"])
+        def _remember_item(user_data):
+            picked_items.append(user_data["item_picked"])
+            return "done"
 
         with self:
-            self.add("ROTATE", CBState(_rotate), transitions={"done": "ASK_USER"})
-            self.add("ASK_USER", CBState(_ask_user), transitions={"succeeded": "succeeded", "failed": "failed"})
+            self.add("ROTATE", CBState(_rotate), transitions={"done": "HANDOVER_POSE"})
+            self.add("HANDOVER_POSE", CBState(_handover_pose), transitions={"done": "ASK_USER"})
+            self.add("ASK_USER", CBState(_ask_user),
+                     transitions={"succeeded": "CHECK_PICK_SUCCESSFUL", "failed": "failed"})
+            self.add("CHECK_PICK_SUCCESSFUL",
+                     ActiveGraspDetector(robot, armdes),
+                     transitions={'true': "ADD_ITEM_TO_LIST",
+                                  'false': "SAY_SOMETHING_WENT_WRONG",
+                                  'failed': "failed",
+                                  'cannot_determine': "SAY_SOMETHING_WENT_WRONG"}
+                     )
+            self.add("SAY_SOMETHING_WENT_WRONG", Say(robot, "Oops, it seems I missed it. Lets try again"),
+                     transitions={"spoken": "ASK_USER2"})
+            self.add("ASK_USER2", CBState(_ask_user), transitions={"succeeded": "ADD_ITEM_TO_LIST", "failed": "failed"})
+            self.add("ADD_ITEM_TO_LIST", CBState(_remember_item), transitions={"done": "CARRYING_POSE"})
+            self.add("CARRYING_POSE", CBState(_carrying_pose), transitions={"done": "succeeded"})
 
 
 class NavigateToAndPickItem(StateMachine):
@@ -101,7 +131,7 @@ def __init__(self, robot, pick_spot_id, pick_spot_navigation_area):
         with self:
             StateMachine.add(
                 "NAVIGATE_TO_PICK_SPOT",
-                NavigateToSymbolic(robot, {pick_spot: pick_spot_navigation_area}, pick_spot),
+                NavigateToSymbolic(robot, {pick_spot: pick_spot_navigation_area},pick_spot, speak=False),
                 transitions={"arrived": "PICK_ITEM", "unreachable": "failed", "goal_not_defined": "failed"},
             )
 

challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_pick_item.py

@@ -11,19 +11,15 @@
 import rospy
 import visualization_msgs.msg
 from geometry_msgs.msg import PoseStamped, Vector3
+from pykdl_ros import FrameStamped
 
 from challenge_serve_breakfast.tuning import (
-    JOINTS_PRE_PRE_PLACE,
-    JOINTS_PRE_PLACE_HORIZONTAL,
-    JOINTS_PRE_PLACE_VERTICAL,
+    get_item_place_pose,
     ITEM_VECTOR_DICT,
     item_vector_to_item_frame,
     item_frame_to_pose,
-    JOINTS_PLACE_HORIZONTAL,
-    JOINTS_PLACE_HORIZONTAL_MILK,
-    JOINTS_PLACE_VERTICAL,
-    JOINTS_RETRACT,
-    COLOR_DICT, REQUIRED_ITEMS,
+    COLOR_DICT,
+    REQUIRED_ITEMS,
 )
 from robot_skills import get_robot
 from robot_smach_states.human_interaction import Say
@@ -35,22 +31,22 @@
 
 
 class PlaceItemOnTable(StateMachine):
-    def __init__(self, robot, table_id):
+    def __init__(self, robot, table_id, retract = True):
         StateMachine.__init__(self, outcomes=["succeeded", "failed"], input_keys=["item_picked"])
         # noinspection PyProtectedMember
         arm = robot.get_arm()._arm
+        self.retract = retract
 
-        def send_joint_goal(position_array, wait_for_motion_done=True):
-            # noinspection PyProtectedMember
-            arm._send_joint_trajectory([position_array], timeout=rospy.Duration(0))
+        def send_goal(pose_goal, wait_for_motion_done=True):
+            arm.send_goal(pose_goal, timeout=0.0)
             if wait_for_motion_done:
                 arm.wait_for_motion_done()
 
         def send_gripper_goal(open_close_string, wait_for_motion_done=True):
             arm.gripper.send_goal(open_close_string)
 
         @cb_interface(outcomes=["done"], input_keys=["item_picked"])
-        def _pre_place(user_data):
+        def _place(user_data):
             item_name = user_data["item_picked"]
             rospy.loginfo(f"Preplacing {item_name}...")
 
@@ -59,57 +55,52 @@ def _pre_place(user_data):
             robot.head.look_up()
             robot.head.wait_for_motion_done()
 
-            send_joint_goal(JOINTS_PRE_PRE_PLACE)
+            item_place_pose = get_item_place_pose(user_data["item_picked"])
+            place_fs = FrameStamped(item_place_pose, rospy.Time(0), table_id)
 
-            if item_name in ["milk_carton", "cereal_box"]:
-                send_joint_goal(JOINTS_PRE_PLACE_HORIZONTAL)
-            else:
-                send_joint_goal(JOINTS_PRE_PLACE_VERTICAL)
+            pre_place_fs = copy.deepcopy(place_fs)
+            pre_place_fs.frame.p.z(place_fs.frame.p.z() + 0.1)
 
-            return "done"
-
-        @cb_interface(outcomes=["done"], input_keys=["item_picked"])
-        def _align_with_table(user_data):
-            item_placement_vector = ITEM_VECTOR_DICT[user_data["item_picked"]]
-            item_frame = item_vector_to_item_frame(item_placement_vector)
+            post_place_fs = copy.deepcopy(place_fs)
+            post_place_fs.frame.p.z(place_fs.frame.p.z() + 0.2)
 
-            goal_pose = item_frame_to_pose(item_frame, table_id)
-            rospy.loginfo("Placing {} at {}".format(user_data["item_picked"], goal_pose))
+            rospy.loginfo("Pre Placing...")
+            send_goal(pre_place_fs)
             robot.head.look_down()
-            ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02, 0.1)).execute({})
-            return "done"
+            robot.head.look_up()
+            robot.head.wait_for_motion_done()
 
-        @cb_interface(outcomes=["done"], input_keys=["item_picked"])
-        def _place_and_retract(user_data):
             rospy.loginfo("Placing...")
-            item_name = user_data["item_picked"]
-            if item_name in ["milk_carton"]:
-                send_joint_goal(JOINTS_PLACE_HORIZONTAL_MILK)
-            elif item_name in ["milk_carton", "cereal_box"]:
-                send_joint_goal(JOINTS_PLACE_HORIZONTAL)
-            else:
-                send_joint_goal(JOINTS_PLACE_VERTICAL)
-
-            rospy.loginfo("Dropping...")
+            send_goal(place_fs)
             send_gripper_goal("open")
-            robot.head.look_up()
-            robot.head.wait_for_motion_done()
 
-            if item_name != "cereal_box":
-                rospy.loginfo("Retract...")
-                send_joint_goal(JOINTS_RETRACT, wait_for_motion_done=False)
+            if item_name != "cereal_box" or self.retract:
+                rospy.loginfo("Retracting...")
+                send_goal(post_place_fs)
                 robot.base.force_drive(-0.1, 0, 0, 3)  # Drive backwards at 0.1m/s for 3s, so 30cm
                 send_gripper_goal("close", wait_for_motion_done=False)
                 arm.send_joint_goal("carrying_pose")
 
+            #if item_name in ["milk_carton", "cereal_box"]:
+            #    send_joint_goal(JOINTS_PRE_PLACE_HORIZONTAL)
+            #else:
+            #    send_joint_goal(JOINTS_PRE_PLACE_VERTICAL)
+
+            #rospy.loginfo("Placing...")
+            #item_name = user_data["item_picked"]
+            #if item_name in ["milk_carton"]:
+            #    send_joint_goal(JOINTS_PLACE_HORIZONTAL_MILK)
+            #elif item_name in ["milk_carton", "cereal_box"]:
+            #    send_joint_goal(JOINTS_PLACE_HORIZONTAL)
+            #else:
+            #    send_joint_goal(JOINTS_PLACE_VERTICAL)
+
             robot.head.reset()
 
             return "done"
 
         with self:
-            self.add_auto("PRE_PLACE", CBState(_pre_place), ["done"])
-            self.add_auto("ALIGN_WITH_TABLE", CBState(_align_with_table), ["done"])
-            self.add("PLACE_AND_RETRACT", CBState(_place_and_retract), transitions={"done": "succeeded"})
+            self.add("PLACE", CBState(_place), {"done": "succeeded"})
 
 
 class NavigateToAndPlaceItemOnTable(StateMachine):
@@ -129,7 +120,7 @@ def __init__(self, robot, table_id, table_close_navigation_area):
             )
             StateMachine.add(
                 "NAVIGATE_TO_TABLE_CLOSE",
-                NavigateToSymbolic(robot, {table: table_close_navigation_area}, table),
+                NavigateToSymbolic(robot, {table: table_close_navigation_area}, table, speak=False),
                 transitions={
                     "arrived": "PLACE_ITEM_ON_TABLE",
                     "unreachable": "SAY_PICK_AWAY_THE_CHAIR",
@@ -159,12 +150,12 @@ def __init__(self, robot, table_id, table_close_navigation_area):
 
             StateMachine.add(
                 "PLACE_ITEM_ON_TABLE",
-                PlaceItemOnTable(robot, table_id),
+                PlaceItemOnTable(robot, table_id, retract=False),
                 transitions={"succeeded": "succeeded", "failed": "failed"},
             )
 
 @cb_interface(outcomes=["done"])
-def _publish_item_poses(marker_array_pub, items):
+def _publish_item_poses(user_data, marker_array_pub, items):
     """
     Publishes item poses as a visualization marker array
 
@@ -184,7 +175,6 @@ def _publish_item_poses(marker_array_pub, items):
         marker_msg.type = visualization_msgs.msg.Marker.SPHERE
         marker_msg.action = 0
         marker_msg.pose = posestamped.pose
-        marker_msg.pose.position.z += 1.0
         marker_msg.scale = Vector3(0.05, 0.05, 0.05)
         marker_msg.color = COLOR_DICT[k]
         array_msg.markers.append(marker_msg)