Merge pull request #931 from tue-robotics/rwc2019_challenge_take_out_…

…the_garbage

Rwc2019 challenge take out the garbage

challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/control_to_trash_bin.py

@@ -0,0 +1,59 @@
+import math
+import os
+
+import rospy
+from geometry_msgs.msg import Quaternion, Pose, Point, PoseStamped
+from robot_skills import Hero
+from robot_smach_states.navigation.control_to_pose import ControlToPose, ControlParameters
+from smach import cb_interface, StateMachine, CBState
+from std_msgs.msg import Header
+from tf.transformations import quaternion_from_euler
+
+
+class ControlToTrashBin(StateMachine):
+    def __init__(self, robot, trashbin_id, radius, yaw_offset):
+        StateMachine.__init__(self, outcomes=['done'])
+
+        @cb_interface(outcomes=['done'])
+        def control_to_pose(_):
+            trash_bin_frame = robot.ed.get_entity(trashbin_id).pose.frame
+            trash_bin_position = trash_bin_frame.p
+            base_frame = robot.base.get_location().frame
+            base_position = base_frame.p
+
+            trash_bin_to_base_vector = base_position - trash_bin_position
+            trash_bin_to_base_vector.Normalize()
+
+            desired_base_position = trash_bin_position + radius * trash_bin_to_base_vector
+
+            goal_pose = PoseStamped(
+                header=Header(
+                    stamp=rospy.Time.now(),
+                    frame_id="map"
+                ),
+                pose=Pose(
+                    position=Point(
+                        x=desired_base_position.x(),
+                        y=desired_base_position.y()
+                    ),
+                    orientation=Quaternion(
+                        *quaternion_from_euler(
+                            0, 0, math.atan2(trash_bin_to_base_vector.y(),
+                                             trash_bin_to_base_vector.x()) - math.pi + yaw_offset
+                        )
+                    )
+                )
+            )
+            ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02, 0.2)).execute({})
+            return 'done'
+
+        with self:
+            self.add('CONTROL_TO_TRASH_BIN', CBState(control_to_pose), transitions={'done': 'done'})
+
+
+if __name__ == '__main__':
+    rospy.init_node(os.path.splitext("test_" + os.path.basename(__file__))[0])
+    hero = Hero()
+    hero.reset()
+    ControlToTrashBin(hero, 'trash_bin', 0.45, -0.2).execute()
+    hero.leftArm().send_joint_goal('grab_trash_bag')

challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/drop_down.py

@@ -1,73 +1,134 @@
 # ROS
 import smach
+import rospy
 
 # TU/e
 import robot_smach_states as states
 import robot_smach_states.util.designators as ds
 from robot_skills.util.kdl_conversions import FrameStamped
+from robocup_knowledge import load_knowledge
+CHALLENGE_KNOWLEDGE = load_knowledge('challenge_take_out_the_garbage')
 
 
-class dropPoseDesignator(ds.Designator):
+class DropTrash(smach.StateMachine):
     """
-    Designator to resolver the drop pose
+    State that moves the robot to the drop bag position and opens the gripper
     """
-    def __init__(self, robot, drop_height, name):
+    def __init__(self, robot, arm_designator):
         """
+
         :param robot: robot object
-        :param drop_height: height to drop the object from
-        :param name: name of pose
         """
-        super(dropPoseDesignator, self).__init__(resolve_type=FrameStamped, name=name)
 
+        smach.State.__init__(self, outcomes=['succeeded', 'failed'])
         self._robot = robot
-        self._drop_height = drop_height
+        self._arm_designator = arm_designator
+
+    def execute(self, userdata=None):
+        arm = self._arm_designator.resolve()
+        if not arm:
+            rospy.logerr("Could not resolve arm")
+            return "failed"
 
-    def _resolve(self):
-        frame = None
+        # Torso up (non-blocking)
+        self._robot.torso.reset()
 
-        # Query ed
-        try:
-            frame = self._robot.ed.get_entity(id="drop_area")._pose
-        except:
-            return None
+        # Arm to position in a safe way
+        arm.send_joint_goal('handover')
+        arm.wait_for_motion_done()
+        arm.send_gripper_goal('open')
+        arm.wait_for_motion_done()
+        arm._arm._send_joint_trajectory(
+            [[0.4, -1.0, 0.0, -1.0, 0.0],[0.4, -1.0, 0.0, -1.57, 0.0], [0.4, -1.0, 0.0, -1.0, 0.0],
+             [0.4, -1.0, 0.0, -1.57, 0.0]])
+        arm.wait_for_motion_done()
+        arm.send_joint_goal('reset')
+        arm.wait_for_motion_done()
+        # arm.send_gripper_goal('close')
+        # arm.wait_for_motion_done()
+        return "succeeded"
 
-        frame.p.z(self._drop_height)
 
-        return FrameStamped(frame, "/map")
+# class DropPoseDesignator(ds.Designator):
+#     """
+#     Designator to resolver the drop pose
+#     """
+#     def __init__(self, robot, drop_height, name):
+#         """
+#         :param robot: robot object
+#         :param drop_height: height to drop the object from
+#         :param name: name of pose
+#         """
+#         super(DropPoseDesignator, self).__init__(resolve_type=FrameStamped, name=name)
+#
+#         self._robot = robot
+#         self._drop_height = drop_height
+#         self._name = name
+#
+#     def _resolve(self):
+#         frame = None
+#
+#         # Query ed
+#         try:
+#             frame = self._robot.ed.get_entity(id=self._name)._pose
+#         except:
+#             rospy.logwarn("The provided entity has no _pose")
+#             return None
+#
+#         frame.p.z(self._drop_height)
+#
+#         return FrameStamped(frame, "/map")
 
 
 class DropDownTrash(smach.StateMachine):
     """
     State that makes the robot go to the drop zone and drop the trash bag there
     """
-    def __init__(self, robot, trash_designator, drop_designator):
+    def __init__(self, robot, drop_zone_id):
         """
         :param robot: robot object
-        :param trash_designator: EdEntityDesignator designating the trash
         :param drop_designator: EdEntityDesignator designating the collection zone
         """
         smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
 
-        drop_area_pose = dropPoseDesignator(robot, 0.6, "drop_pose")
         arm_designator = ds.OccupiedArmDesignator(robot=robot, arm_properties={})
-        self._trash_designator = trash_designator
-        self._drop_designator = drop_designator
 
         with self:
             smach.StateMachine.add("GO_TO_COLLECTION_ZONE",
-                                   states.NavigateToObserve(robot, self._drop_designator),
-                                   transitions={"arrived": "PLACE_TRASH",
+                                   states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=drop_zone_id),
+                                                             radius=0.5),
+
+                                   transitions={"arrived": "DROP_TRASH",
+                                                "goal_not_defined": "aborted",
+                                                "unreachable": "OPEN_DOOR_PLEASE"})
+
+            smach.StateMachine.add("OPEN_DOOR_PLEASE",
+                                   states.Say(robot, "Can you please open the door for me? It seems blocked!"),
+                                   transitions={"spoken": "WAIT_FOR_DOOR_OPEN"})
+
+            smach.StateMachine.add("WAIT_FOR_DOOR_OPEN",
+                                   states.WaitTime(robot=robot, waittime=5),
+                                   transitions={"waited": "GO_TO_COLLECTION_ZONE2",
+                                                "preempted": "GO_TO_COLLECTION_ZONE2"})
+
+            smach.StateMachine.add("GO_TO_COLLECTION_ZONE2",
+                                   states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=drop_zone_id),
+                                                             radius=0.5),
+
+                                   transitions={"arrived": "DROP_TRASH",
                                                 "goal_not_defined": "aborted",
                                                 "unreachable": "failed"})
 
-            smach.StateMachine.add("PLACE_TRASH",
-                                   states.Place(robot=robot, item_to_place=trash_designator,
-                                                place_pose=drop_area_pose, place_volume="on_top_of",
-                                                arm=arm_designator),
-                                   transitions={"done": "succeeded",
+            smach.StateMachine.add("DROP_TRASH", DropTrash(robot=robot, arm_designator=arm_designator),
+                                   transitions={"succeeded": "succeeded",
                                                 "failed": "HANDOVER"})
 
             smach.StateMachine.add("HANDOVER",
                                    states.HandoverToHuman(robot=robot, arm_designator=arm_designator),
                                    transitions={"succeeded": "succeeded",
                                                 "failed": "failed"})
+
+
+#states.NavigateToObserve(robot, drop_designator),
+#states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=INTERMEDIATE_1),
+#                                                         radius=0.5),