(speech_recognition) apply pykdl_ros

Author: MatthijsBurgh

challenge_speech_recognition/package.xml

@@ -13,16 +13,9 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>robot_skills</build_depend>
-  <build_depend>robot_smach_states</build_depend>
-  <build_depend>rospy</build_depend>
-  <build_depend>tf</build_depend>
-
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>robot_skills</exec_depend>
-  <exec_depend>robot_smach_states</exec_depend>
-  <exec_depend>rospy</exec_depend>
-  <exec_depend>tf</exec_depend>
+  <depend>pykdl_ros</depend>
+  <depend>robot_skills</depend>
+  <depend>robot_smach_states</depend>
+  <depend>rospy</depend>
 
 </package>

challenge_speech_recognition/src/indirect_speech_recognition.py

@@ -2,13 +2,14 @@
 
 from __future__ import print_function
 
-import rospy
-import smach
 import sys
 import random
 import math
 import time
 
+from pykdl_ros import VectorStamped
+import rospy
+import smach
 
 import robot_smach_states as states
 
@@ -53,7 +54,7 @@ def _turn_to_closest_entity(self):
         operator = None
         while not operator:
             operator = self.robot.ed.get_closest_entity(self, radius=1.9,
-                                                        center_point=self.robot.base.get_location().extractVectorStamped())
+                                                        center_point=VectorStamped.from_framestamped(self.robot.base.get_location()))
             print(operator)
             if not operator:
                 vth = 0.5
@@ -66,8 +67,8 @@ def _turn_to_closest_entity(self):
         # Turn towards the operator
         current = self.robot.base.get_location()
         robot_th = current.frame.M.GetRPY()[2]  # Get the Yaw, rotation around Z
-        desired_th = math.atan2(operator._pose.p.y() - current.frame.p.y(),
-                                operator._pose.p.x() - current.frame.p.x())
+        desired_th = math.atan2(operator.pose.frame.p.y() - current.frame.p.y(),
+                                operator.pose.frame.p.x() - current.frame.p.x())
 
         # Calculate params
         th = desired_th - robot_th