Merge pull request #889 from tue-robotics/rwc2019_robot_skills

Rwc2019 robot skills

robot_skills/examples/example_force_sensor_edge_up.py

@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+
+import rospy
+from robot_skills.force_sensor import ForceSensor
+
+rospy.init_node('test_force_sensor_edge_up', anonymous=True)
+rospy.sleep(1.0)  # In simulation time is not yet available ...
+ForceSensor('/hero/wrist_wrench/raw').wait_for_edge_up()
+rospy.loginfo("Edge up detected")

robot_skills/examples/example_raytrace.py

@@ -0,0 +1,34 @@
+#!/usr/bin/env python
+from ed_sensor_integration.srv import RayTrace
+from geometry_msgs.msg import PoseStamped
+from robot_skills import perception
+import rospy
+from std_msgs.msg import Header
+from tf import TransformListener
+
+rospy.init_node('ray_trace_example')
+
+p = perception.Perception("hero", None, None, camera_base_ns="hero/head_rgbd_sensor")
+
+srv_proxy = rospy.ServiceProxy("/hero/ed/ray_trace", RayTrace)
+
+tf_listener = TransformListener()
+
+while not rospy.is_shutdown():
+    rgb, depth, depth_info = p.get_rgb_depth_caminfo()
+    if rgb:
+        persons = p.detect_person_3d(*p.get_rgb_depth_caminfo())
+        for person in persons:
+            if "is_pointing" in person.tags:
+                try:
+                    map_pose = tf_listener.transformPose("map", PoseStamped(
+                        header=Header(
+                            frame_id="head_rgbd_sensor_rgb_optical_frame",
+                            stamp=rospy.Time.now() - rospy.Duration.from_sec(0.5)
+                        ),
+                        pose=person.pointing_pose
+                    ))
+                    srv_proxy(raytrace_pose=map_pose)
+
+                except Exception as e:
+                    rospy.logerr("Could not get ray trace from closest person: {}".format(e))

robot_skills/hardware_tests/test_move_down_until_force_sensor_edge_up.py

@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+import rospy
+from robot_skills.util.robot_constructor import robot_constructor
+
+rospy.init_node('test_mode_down_until_force_sensor_edge_up', anonymous=True)
+robot = robot_constructor("hero")
+arm = robot.get_arm()
+
+arm.move_down_until_force_sensor_edge_up()

robot_skills/package.xml

@@ -10,6 +10,8 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <run_depend>cv_bridge</run_depend>
+
   <build_depend>rospy</build_depend>
   <run_depend>rospy</run_depend>
 

robot_skills/src/robot_skills/amigo.py

@@ -1,5 +1,6 @@
-from robot_skills import robot, api, arms, base, ebutton, head, ears, lights, perception, speech, ssl, torso,\
-    world_model_ed
+from robot_skills import robot, api, arms, base, ebutton, head, ears, lights, perception, speech, \
+    sound_source_localisation, torso, world_model_ed
+from .simulation import is_sim_mode, SimEButton
 
 
 class Amigo(robot.Robot):
@@ -10,14 +11,14 @@ def __init__(self, dontInclude=None, wait_services=False):
         super(Amigo, self).__init__(robot_name="amigo", wait_services=wait_services)
 
         self.add_body_part('base', base.Base(self.robot_name, self.tf_listener))
-        self.add_body_part('torso', torso.Torso(self.robot_name, self.tf_listener))
+        self.add_body_part('torso', torso.Torso(self.robot_name, self.tf_listener, self.get_joint_states))
 
-        self.add_arm_part('leftArm', arms.Arm(self.robot_name, self.tf_listener, side="left"))
-        self.add_arm_part('rightArm', arms.Arm(self.robot_name, self.tf_listener, side="right"))
+        self.add_arm_part('leftArm', arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, side="left"))
+        self.add_arm_part('rightArm', arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, side="right"))
 
         self.add_body_part('head', head.Head(self.robot_name, self.tf_listener))
         self.add_body_part('perception', perception.Perception(self.robot_name, self.tf_listener))
-        self.add_body_part('ssl', ssl.SSL(self.robot_name, self.tf_listener))
+        self.add_body_part('ssl', sound_source_localisation.SSL(self.robot_name, self.tf_listener))
 
         # Human Robot Interaction
         self.add_body_part('lights', lights.Lights(self.robot_name, self.tf_listener))
@@ -31,7 +32,8 @@ def __init__(self, dontInclude=None, wait_services=False):
                                              lambda: self.lights.set_color_colorRGBA(lights.LISTENING),
                                              lambda: self.lights.set_color_colorRGBA(lights.RESET)))
 
-        self.add_body_part('ebutton', ebutton.EButton(self.robot_name, self.tf_listener))
+        ebutton_class = SimEButton if is_sim_mode() else ebutton.EButton
+        self.add_body_part('ebutton', ebutton_class(self.robot_name, self.tf_listener))
 
         # Reasoning/world modeling
         self.add_body_part('ed', world_model_ed.ED(self.robot_name, self.tf_listener))

robot_skills/src/robot_skills/api.py

@@ -1,5 +1,8 @@
 # ROS
+import cv2
 import rospy
+from cv_bridge import CvBridge
+from sensor_msgs.msg import CompressedImage
 from std_srvs.srv import Empty
 
 # TU/e Robotics
@@ -21,10 +24,13 @@ def __init__(self, robot_name, tf_listener, pre_hook=None, post_hook=None):
         self._pre_hook = pre_hook
         self._post_hook = post_hook
 
+        self.cv_bridge = CvBridge()
+
         client = self.create_simple_action_client('/' + robot_name + '/hmi', QueryAction)
         self._client = Client(simple_action_client=client)
 
         self.restart_srv = self.create_service_client('/' + robot_name + '/hmi/dragonfly_speech_recognition/restart_node', Empty)
+        self._image_from_ros_publisher = rospy.Publisher('/' + robot_name + '/hmi/image', CompressedImage, queue_size=1)
 
     def query(self, description, grammar, target, timeout=10):
         """
@@ -39,6 +45,7 @@ def query(self, description, grammar, target, timeout=10):
 
         try:
             answer = self._client.query(description, grammar, target, timeout)
+            self.restart_dragonfly()
         except TimeoutException as e:
             if callable(self._post_hook):
                 self._post_hook()
@@ -50,6 +57,33 @@ def query(self, description, grammar, target, timeout=10):
 
         return answer
 
+    def _show_image(self, msg, seconds=5.0):
+        """
+        Show an image on the HMI display interface
+        :param msg: CompressedImage to display
+        :param seconds: How many seconds you would like to display the image on the screen
+        """
+        msg.header.stamp = rospy.Time.from_sec(seconds)
+        self._image_from_ros_publisher.publish(msg)
+
+    def show_image(self, path_to_image, seconds=5.0):
+        """
+        Show an image on the HMI display interface
+        :param path_to_image: Absolute path to image file
+        :param seconds: How many seconds you would like to display the image on the screen
+        """
+        compressed_image_msg = self.cv_bridge.cv2_to_compressed_imgmsg(cv2.imread(path_to_image))
+        self._show_image(compressed_image_msg, seconds)
+
+    def show_image_from_msg(self, msg, seconds=5.0):
+        """
+        Show an image on the HMI display interface
+        :param msg: rgb msg
+        :param seconds: How many seconds you would like to display the image on the screen
+        """
+        compressed_image_msg = self.cv_bridge.cv2_to_compressed_imgmsg(self.cv_bridge.imgmsg_to_cv2(msg, "bgr8"))
+        self._show_image(compressed_image_msg, seconds)
+
     @property
     def last_talker_id(self):
         return self._client.last_talker_id