Merge pull request #826 from tue-robotics/rgo2019_challenge_take_out_…

…the_garbage

Rgo2019 challenge take out the garbage

challenge_take_out_the_garbage/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(challenge_take_out_the_garbage)
+
+find_package(catkin REQUIRED COMPONENTS
+  robot_skills
+  robot_smach_states
+)
+
+catkin_python_setup()
+
+###################################
+## catkin specific configuration ##
+###################################
+
+catkin_package()
+
+###########
+## Build ##
+###########
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)

challenge_take_out_the_garbage/README.md

@@ -0,0 +1,19 @@
+Before Testing:
+1: Be on the proper branches:
+	- on ed_object_models -> branch: rgo2019_challenge_take_out_the_garbage
+	- on challange_take_out_the_garbage -> branch: rgo2019_challenge_take_out_the_garbage
+2: The drop area where the robot is going to drop the bag is the left side of the dinner table.
+   So make sure that area is clear of any objects!
+
+Instructions for testing:
+1: Set the robot in the initial position, so in front of the imaginary door (in front of the sofa block)
+2: Run challenge
+3: Press emergency button when it is going to grab the trash bag! or remove bin.
+   The distance is not tuned yet for this trash bin, so it will probably go wrong!!
+
+ToDo's:
+- add second trash bin, if we have a second trash bin.
+  For now the second trash bin is the same as the first one.
+- tune the correct distance for this trash bin
+
+

challenge_take_out_the_garbage/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>challenge_take_out_the_garbage</name>
+  <version>0.0.0</version>
+  <description>The challenge_take_out_the_garbage package</description>
+
+  <maintainer email="kevin.acnowlogia@gmail.com">Kevin Dang</maintainer>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>robot_skills</depend>
+  <depend>robot_smach_states</depend>
+
+</package>

challenge_take_out_the_garbage/scripts/challenge_take_out_the_garbage

@@ -0,0 +1,13 @@
+#!/usr/bin/python
+
+# ROS
+import rospy
+
+# TU/e Robotics
+from robot_smach_states.util.startup import startup
+# Challenge example
+from challenge_take_out_the_garbage.take_out_the_garbage import TakeOutGarbage
+
+if __name__ == "__main__":
+    rospy.init_node('take_out_the_garbage')
+    startup(TakeOutGarbage)

challenge_take_out_the_garbage/setup.py

@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    # #  don't do this unless you want a globally visible script
+    # scripts=['bin/myscript'],
+    packages=['challenge_take_out_the_garbage'],
+    package_dir={'': 'src'}
+)
+
+setup(**d)

challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/drop_down.py

@@ -0,0 +1,73 @@
+# ROS
+import smach
+
+# TU/e
+import robot_smach_states as states
+import robot_smach_states.util.designators as ds
+from robot_skills.util.kdl_conversions import FrameStamped
+
+
+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
+
+    def _resolve(self):
+        frame = None
+
+        # Query ed
+        try:
+            frame = self._robot.ed.get_entity(id="drop_area")._pose
+        except:
+            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):
+        """
+        :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",
+                                                "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",
+                                                "failed": "HANDOVER"})
+
+            smach.StateMachine.add("HANDOVER",
+                                   states.HandoverToHuman(robot=robot, arm_designator=arm_designator),
+                                   transitions={"succeeded": "succeeded",
+                                                "failed": "failed"})

challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/measure_garbage.py

@@ -0,0 +1,51 @@
+# ROS
+import rospy
+import smach
+
+# TU/e
+import robot_smach_states as states
+import robot_smach_states.util.designators as ds
+import robot_smach_states.manipulation as manipulation
+from robot_skills.arms import PublicArm
+
+from geometry_msgs.msg import WrenchStamped
+import math
+
+
+class MeasureForce(object):
+    """
+    Measure the three forces in the arm
+    """
+    def __init__(self, robot):
+        """
+
+        """
+        smach.State.__init__(self, outcomes=["succeeded", "failed"])
+        self._robot = robot
+
+    def get_force(self):
+        ft_sensor_topic = '/'+self._robot.robot_name+'/wrist_wrench/raw'
+
+        force_grab = rospy.wait_for_message(ft_sensor_topic, WrenchStamped)
+
+        force_data_x = force_grab.wrench.force.x
+        force_data_y = force_grab.wrench.force.y
+        force_data_z = force_grab.wrench.force.z
+
+        return [force_data_x, force_data_y, force_data_z]
+
+
+class MeasureGarbage(smach.StateMachine):
+    """
+    Measure the weight of the garbage
+    """
+    def __init__(self, robot):
+        """
+
+        """
+        smach.State.__init__(self, outcomes=["succeeded", "failed"])
+        self._robot = robot
+
+    def execute(self):
+
+        return "succeeded"