Merge pull request #295 from tue-robotics/fix/help_me_carry_5

Fix/help me carry 5

challenge_help_me_carry/src/challenge_help_me_carry.py

@@ -4,15 +4,20 @@
 import smach
 import sys
 import time
+import datetime
 from hmi import TimeoutException
 
 from cb_planner_msgs_srvs.msg import PositionConstraint
 
 import robot_smach_states.util.designators as ds
+from robot_smach_states.util.designators import check_type
+from robot_skills.arms import Arm
+
 import robot_smach_states as states
 
 from robocup_knowledge import load_knowledge
 from robot_skills.util import kdl_conversions
+from robot_skills.util.entity import Entity
 
 challenge_knowledge = load_knowledge('challenge_help_me_carry')
 
@@ -42,7 +47,7 @@ def __init__(self, robot, possible_commands, commands_as_outcomes=False, command
 
         smach.State.__init__(self, outcomes=_outcomes,  output_keys=_output_keys)
 
-    def _listen_for_commands(self, tries=3, time_out = rospy.Duration(30)):
+    def _listen_for_commands(self, tries=3, time_out=30):
         for i in range(0, tries):
             try:
                 result = self._robot.hmi.query('What command?', 'T -> ' + ' | '.join(self._possible_commands), 'T', timeout=time_out)
@@ -109,7 +114,8 @@ def execute(self, userdata):
         handOverHuman = states.HandoverFromHuman(self._robot,
                                                  self._empty_arm_designator,
                                                  "current_item",
-                                                 self._current_item)
+                                                 self._current_item,
+                                                 arm_configuration="carrying_bag_pose")
 
         userdata.target_room_out = userdata.target_room_in
 
@@ -135,6 +141,34 @@ def execute(self, userdata):
 
         return navigateToWaypoint.execute()
 
+class DropBagOnGround(smach.StateMachine):
+    """
+    Put a bag in the robot's gripper on the ground
+    """
+    def __init__(self, robot, arm_designator):
+        """
+        :param robot: the robot with which to execute this state machine
+        :param arm_designator: ArmDesignator resolving to Arm holding the bag to drop
+
+        """
+        smach.StateMachine.__init__(self, outcomes=['succeeded','failed'])
+
+        check_type(arm_designator, Arm)
+
+        with self:
+            smach.StateMachine.add( 'OPEN_BEFORE_DROP', states.SetGripper(robot, arm_designator, gripperstate='open'),
+                                transitions={'succeeded'    :   'DROP_POSE',
+                                             'failed'       :   'DROP_POSE'})
+
+            smach.StateMachine.add("DROP_POSE", states.ArmToJointConfig(robot, arm_designator, "drop_bag_pose"),
+                            transitions={'succeeded'        :'RESET_ARM_OK',
+                                         'failed'           :'RESET_ARM_FAIL'})
+
+            smach.StateMachine.add( 'RESET_ARM_OK', states.ResetArms(robot),
+                                transitions={'done'         :   'succeeded'})
+
+            smach.StateMachine.add( 'RESET_ARM_FAIL', states.ResetArms(robot),
+                                transitions={'done'         :   'failed'})
 
 def setup_statemachine(robot):
 
@@ -147,12 +181,23 @@ def setup_statemachine(robot):
     empty_arm_designator = ds.UnoccupiedArmDesignator(robot.arms,
                                                       robot.rightArm,
                                                       name="empty_arm_designator")
-    current_item = ds.EntityByIdDesignator(robot,
-                                           id=challenge_knowledge.default_item,
-                                           name="current_item")
-    arm_with_item_designator = ds.ArmHoldingEntityDesignator(robot.arms,
-                                                             current_item,
-                                                             name="arm_with_item_designator")
+
+
+    # With the empty_arm_designator locked, it will ALWAYS resolve to the same arm (first resolve is cached), unless it is unlocked
+    # For this challenge, unlocking is not needed
+    bag_arm_designator = empty_arm_designator.lockable()
+    bag_arm_designator.lock()
+
+    # We don't actually grab something, so there is no need for an actual thing to grab
+    current_item = ds.VariableDesignator(Entity("dummy",
+                                                "dummy",
+                                                "/{}/base_link".format(robot.robot_name),
+                                                kdl_conversions.kdlFrameFromXYZRPY(0.6, 0, 0.5),
+                                                None,
+                                                {},
+                                                [],
+                                                datetime.datetime.now()),
+                                         name="current_item")
 
     sm = smach.StateMachine(outcomes=['Done','Aborted'])
 
@@ -217,7 +262,7 @@ def setup_statemachine(robot):
 
         # Grab the item (bag) the operator hands to the robot, when they are at the "car".
         smach.StateMachine.add('GRAB_ITEM',
-                               GrabItem(robot, empty_arm_designator, current_item),
+                               GrabItem(robot, bag_arm_designator, current_item),
                                transitions={'succeeded':        'SAY_GOING_TO_ROOM',
                                             'timeout':          'SAY_GOING_TO_ROOM', # For now in simulation timeout is considered a succes.
                                             'failed':           'Aborted'},
@@ -237,9 +282,9 @@ def setup_statemachine(robot):
 
         # Put the item (bag) down when the robot has arrived at the "drop-off" location (house).
         smach.StateMachine.add('PUTDOWN_ITEM',
-                               states.Place(robot, current_item, place_position, arm_with_item_designator),
-                               transitions={'done':             'ASKING_FOR_HELP',
-                                            'failed':           'Aborted'})
+                               DropBagOnGround(robot, bag_arm_designator),
+                               transitions={'succeeded':        'ASKING_FOR_HELP',
+                                            'failed':           'ASKING_FOR_HELP'})
 
         smach.StateMachine.add('ASKING_FOR_HELP',
                                #TODO: look and then face new operator

robot_skills/src/robot_skills/arms.py

@@ -216,12 +216,12 @@ def send_goal(self, frameStamped,
                 execute_timeout=rospy.Duration(timeout)
             )
             if result == GoalStatus.SUCCEEDED:
-                
+
                 result_pose = self.tf_listener.lookupTransform("amigo/base_link", "amigo/grippoint_{}".format(self.side))
                 dx = grasp_precompute_goal.goal.x - result_pose[0][0]
                 dy = grasp_precompute_goal.goal.y - result_pose[0][1]
                 dz = grasp_precompute_goal.goal.z - result_pose[0][2]
-                
+
                 if abs(dx) > 0.005 or abs(dy) > 0.005 or abs(dz) > 0.005:
                     rospy.logwarn("Grasp-precompute error too large: [{}, {}, {}]".format(
                                   dx, dy, dz))
@@ -454,6 +454,9 @@ def _publish_marker(self, goal, color, ns = ""):
 
         self._marker_publisher.publish(marker)
 
+    def __repr__(self):
+        return "Arm(side='{side}')".format(side=self.side)
+
 
 if __name__ == "__main__":
     rospy.init_node('amigo_arms_executioner', anonymous=True)

robot_skills/src/robot_skills/world_model_ed.py

@@ -127,9 +127,15 @@ def get_closest_room(self, center_point=None, radius=0):
 
         return self.get_closest_entity(type="room", center_point=center_point, radius=radius)
 
-    def get_closest_laser_entity(self, type="", center_point=kdl.Vector(), radius=0):
-        if isinstance(center_point, PointStamped):
-            center_point = self._transform_center_point_to_map(center_point)
+    def get_closest_laser_entity(self, type="", center_point=VectorStamped(), radius=0):
+        """
+        Get the closest entity detected by the laser. The ID's of such entities are postfixed with '-laser'
+        For the rest, this works exactly like get_closest_entity
+        :param type: What type of entities to filter on
+        :param center_point: combined with radius. Around which point to search for entities
+        :param radius: how far from the center_point to look (in meters)
+        :return: list of Entity
+        """
 
         entities = self.get_entities(type="", center_point=center_point, radius=radius)
 

robot_smach_states/src/robot_smach_states/manipulation/manipulation.py

@@ -62,15 +62,24 @@ class HandoverFromHuman(smach.StateMachine):
     CloseGripperOnHandoverToRobot state and given the grabbed_entity_label
     as id.
     '''
-    def __init__(self, robot, arm_designator, grabbed_entity_label="", grabbed_entity_designator=None, timeout=10):
+    def __init__(self, robot, arm_designator, grabbed_entity_label="", grabbed_entity_designator=None, timeout=10, arm_configuration="handover_to_human"):
+        """
+        Hold up hand to accept an object and close hand once something is inserted
+        :param robot: Robot with which to execute this behavior
+        :param arm_designator: ArmDesignator resolving to arm accept item into
+        :param grabbed_entity_label: What ID to give a dummy item in case no grabbed_entity_designator is supplied
+        :param grabbed_entity_designator: EntityDesignator resolving to the accepted item. Can be a dummy
+        :param timeout: How long to hold hand over before closing without anything
+        :param arm_configuration: Which pose to put arm in when holding hand up for the item.
+        """
         smach.StateMachine.__init__(self, outcomes=['succeeded','failed','timeout'])
 
         check_type(arm_designator, Arm)
         if not grabbed_entity_designator and grabbed_entity_label == "":
             rospy.logerr("No grabbed entity label or grabbed entity designator given")
 
         with self:
-            smach.StateMachine.add("POSE", ArmToJointConfig(robot, arm_designator, "handover_to_human"),
+            smach.StateMachine.add("POSE", ArmToJointConfig(robot, arm_designator, arm_configuration),
                             transitions={'succeeded':'OPEN_BEFORE_INSERT','failed':'OPEN_BEFORE_INSERT'})
 
             smach.StateMachine.add( 'OPEN_BEFORE_INSERT', SetGripper(robot, arm_designator, gripperstate='open'),
@@ -187,7 +196,7 @@ def execute(self, userdata):
             return "failed"
 
         if self.item_designator:
-            arm.occupied_by = self.item_designator
+            arm.occupied_by = self.item_designator.resolve()
         else:
             if self.item_label != "":
                 handed_entity = EntityInfo(id=self.item_label)

robot_smach_states/src/robot_smach_states/util/designators/arm.py

@@ -52,7 +52,9 @@ def _resolve(self):
             rospy.loginfo("Found {} operational arms: {}".format(len(operational_arms), [arm2name[arm] for arm in operational_arms]))
 
             if any(operational_arms):
-                return operational_arms[0]
+                selected_arm = operational_arms[0]
+                rospy.logdebug("Selected arm: {arm}".format(arm=selected_arm))
+                return selected_arm
             else:
                 rospy.logerr("ArmDesignator {0} could not resolve to an arm".format(self))
                 return None
@@ -145,8 +147,16 @@ def __init__(self, all_arms, entity_designator, name=None):
         self.entity_designator = entity_designator
 
     def available(self, arm):
-        """Check that the arm is occupied by the entity refered to by the entity_designator"""
-        return arm.occupied_by == self.entity_designator.resolve()
+        """Check that the arm is occupied by the entity referred to by the entity_designator"""
+        # Check that the designator actually resolved to anything.
+        # Otherwise, the check could become None == None, which is also True
+        entity = self.entity_designator.resolve()
+        if entity:
+            rospy.logdebug("{arm} is occupied by entity we're looking for: {ent}".format(arm=arm, ent=entity))
+            return arm.occupied_by == entity
+        else:
+            rospy.logwarn("Entity is None and {arm} is {un}occupied".format(arm=arm, un="un" if arm.occupied_by == None else ""))
+            return False
 
 if __name__ == "__main__":
     import doctest

robot_smach_states/src/robot_smach_states/util/designators/utility.py

@@ -80,6 +80,9 @@ def resolve(self):
     def __repr__(self):
         return "LockingDesignator({})".format(str(self.to_be_locked.name))
 
+    def _get_name(self):
+        return "lockable({})".format(self.to_be_locked.name)
+
 
 class AttrDesignator(Designator):