diff --git a/challenge_serve_breakfast/images/spoon.jpg b/challenge_serve_breakfast/images/spoon.jpg old mode 100755 new mode 100644 index 55e08eae1d..05cea6b7b5 Binary files a/challenge_serve_breakfast/images/spoon.jpg and b/challenge_serve_breakfast/images/spoon.jpg differ diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_pick_item.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_pick_item.py index 6e24981f5d..039f8940df 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_pick_item.py +++ b/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_pick_item.py @@ -10,6 +10,10 @@ from challenge_serve_breakfast.tuning import REQUIRED_ITEMS, JOINTS_HANDOVER, PICK_ROTATION 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.navigation import NavigateToSymbolic from robot_smach_states.util.designators import EdEntityDesignator from smach import StateMachine, cb_interface, CBState @@ -31,7 +35,7 @@ def __init__(self, robot): 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 +56,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 +78,33 @@ 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.) - - return "succeeded" + robot.speech.speak("Thanks for that!", block=False) + 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": "CARRYING_POSE", "failed": "failed"}) + + self.add("CARRYING_POSE", CBState(_carrying_pose), transitions={"done": "succeeded"}) class NavigateToAndPickItem(StateMachine): @@ -101,7 +116,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"}, ) diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_place_item_on_table.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_place_item_on_table.py index af9d0413e7..4d523df030 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_place_item_on_table.py +++ b/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_place_item_on_table.py @@ -42,7 +42,7 @@ def __init__(self, robot, table_id): 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() @@ -83,10 +83,8 @@ def _align_with_table(user_data): def _place_and_retract(user_data): rospy.loginfo("Placing...") item_name = user_data["item_picked"] - if item_name in ["milk_carton"]: + if item_name in ["milk_carton", "cereal_box"]: 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) @@ -129,7 +127,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", @@ -164,7 +162,7 @@ def __init__(self, robot, table_id, table_close_navigation_area): ) @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 +182,7 @@ 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.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) diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/pick_pour_place_cereal.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/pick_pour_place_cereal.py index 2df5eecca4..f8fa53780c 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/pick_pour_place_cereal.py +++ b/challenge_serve_breakfast/src/challenge_serve_breakfast/pick_pour_place_cereal.py @@ -25,7 +25,7 @@ def __init__(self, robot, table_id): 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() @@ -54,6 +54,7 @@ def _align_pour(_): def _pour(_): robot.speech.speak("Hope this goes well", block=False) send_joint_goal(JOINTS_PRE_POUR) + rospy.sleep(0.5) send_joint_goal(JOINTS_POUR) send_joint_goal(JOINTS_PRE_POUR) send_joint_goal(JOINTS_POST_PICK) diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/serve_breakfast.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/serve_breakfast.py index 0663351258..bea8d7a9ef 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/serve_breakfast.py +++ b/challenge_serve_breakfast/src/challenge_serve_breakfast/serve_breakfast.py @@ -9,9 +9,10 @@ from robot_smach_states.navigation import NavigateToWaypoint from robot_smach_states.startup import StartChallengeRobust from robot_smach_states.util.designators import EdEntityDesignator +from robot_smach_states.world_model import UpdateEntityPose from robot_smach_states.utility import WaitTime from smach import State, StateMachine -from .navigate_to_and_pick_item import NavigateToAndPickItem +from .navigate_to_and_pick_item import NavigateToAndPickItem, NavigateToSymbolic from .navigate_to_and_place_item_on_table import NavigateToAndPlaceItemOnTable from .pick_pour_place_cereal import PickPourPlaceCereal from .tuning import REQUIRED_ITEMS @@ -51,11 +52,12 @@ def execute(self, userdata): def setup_statemachine(robot): state_machine = StateMachine(outcomes=["done"]) state_machine.userdata["item_picked"] = None - pick_id = "dinner_table" + pick_id = "closet" pick_area_id = "in_front_of" place_id = "dinner_table" place_area_id = "in_front_of" exit_id = "starting_pose" + table_des = EdEntityDesignator(robot=robot, uuid=place_id) with state_machine: # Intro @@ -63,22 +65,40 @@ def setup_statemachine(robot): StateMachine.add( "START_CHALLENGE_ROBUST", StartChallengeRobust(robot, "initial_pose"), - transitions={"Done": "SAY_START", "Aborted": "done", "Failed": "SAY_START"}, + transitions={"Done": "NAVIGATE_TO_TABLE", "Aborted": "GOODBYE", "Failed": "NAVIGATE_TO_TABLE"}, + ) + #Main loop + StateMachine.add( + "NAVIGATE_TO_TABLE", + NavigateToSymbolic(robot, {table_des:place_area_id}, table_des, speak=True), + transitions={"arrived": "UPDATE_TABLE_POSE", "unreachable": "SAY_OPERATOR_WHERE_TO_STAND", "goal_not_defined": "SAY_OPERATOR_WHERE_TO_STAND"}, + ) + + StateMachine.add( + "UPDATE_TABLE_POSE", + UpdateEntityPose(robot=robot, entity_designator=table_des), + transitions={"done": "SAY_OPERATOR_WHERE_TO_STAND"}, + ) + + StateMachine.add( + "SAY_OPERATOR_WHERE_TO_STAND", + Say( + robot, + "Operator, please stand at the RIGHT of the dishwasher", + block=False, + ), + transitions={"spoken": "SAY_START"}, ) StateMachine.add( "SAY_START", Say( robot, - #f"Lets serve some breakfast baby! If there are any chairs near the {place_id}, please remove them", "Lets serve some breakfast baby! I will be asking for some fast handovers.", block=False, ), transitions={"spoken": "NAVIGATE_AND_PICK_ITEM"}, ) - - # Main loop - StateMachine.add( "NAVIGATE_AND_PICK_ITEM", NavigateToAndPickItem(robot, pick_id, pick_area_id), @@ -87,7 +107,7 @@ def setup_statemachine(robot): StateMachine.add( "NAVIGATE_AND_PICK_ITEM_FAILED", WaitTime(robot, 2), - transitions={"waited": "NAVIGATE_AND_PICK_ITEM", "preempted": "done"} + transitions={"waited": "NAVIGATE_AND_PICK_ITEM", "preempted": "GOODBYE"} ) StateMachine.add( @@ -97,7 +117,7 @@ def setup_statemachine(robot): ) StateMachine.add( - "WAIT", WaitTime(robot, 2), transitions={"waited": "CHECK_IF_WE_HAVE_IT_ALL", "preempted": "done"} + "WAIT", WaitTime(robot, 2), transitions={"waited": "CHECK_IF_WE_HAVE_IT_ALL", "preempted": "GOODBYE"} ) StateMachine.add( diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py index 1a771f2383..97493d5444 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py +++ b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py @@ -15,10 +15,10 @@ REQUIRED_ITEMS = ["spoon", "bowl", "milk_carton", "cereal_box"] ITEM_VECTOR_DICT = { - "spoon": PyKDL.Vector(0.03, -0.15, 0), - "bowl": PyKDL.Vector(0.1, -0.04, 0), - "milk_carton": PyKDL.Vector(-0.1, 0.3, 0), - "cereal_box": PyKDL.Vector(-0.1, -0.3, 0), + "spoon": PyKDL.Vector(0.0, -0.15, 0), + "bowl": PyKDL.Vector(0.0, 0.0, 0), + "milk_carton": PyKDL.Vector(-0.05, 0.15, 0), + "cereal_box": PyKDL.Vector(-0.05, -0.2, 0), } COLOR_DICT = { @@ -28,7 +28,7 @@ "cereal_box": ColorRGBA(1, 1, 0, 1), } -PICK_ROTATION = 2. +PICK_ROTATION = 0 JOINTS_HANDOVER = [0.4, -0.2, 0.0, -1.37, 0] @@ -45,16 +45,16 @@ JOINTS_POST_PICK = [0.7, -1.2, 0, 0, 0] -JOINTS_PRE_POUR = [0.5, -1.2, -1.5, 0, 0] +JOINTS_PRE_POUR = [0.35, -1.2, 0, 0, 0] -JOINTS_POUR = [0.5, -1.2, -2.5, 0, 0] +JOINTS_POUR = [0.4, -1.2, -2.5, 0, 0] -POUR_OFFSET_X = 0.03 +POUR_OFFSET_X = -0.15 POUR_OFFSET_Y = 0.15 def item_vector_to_item_frame(item_vector): - frame = PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, -math.pi / 2), PyKDL.Vector(-0.05, 0.75, 0)) + frame = PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, math.pi), PyKDL.Vector(0.7, 0, 0)) item_placement_vector = item_vector item_frame = frame