| 26 |
26 |
"""
|
| 27 |
27 |
|
| 28 |
28 |
from Middleware import *
|
| 29 |
|
import errno
|
| 30 |
29 |
|
| 31 |
30 |
import rospy
|
| 32 |
31 |
import actionlib
|
| 33 |
32 |
from hlrc_server.msg import *
|
| 34 |
|
from geometry_msgs.msg import PointStamped
|
| 35 |
33 |
from actionlib_msgs.msg import GoalStatus
|
| 36 |
34 |
|
| 37 |
35 |
class MiddlewareROS(Middleware):
|
| ... | ... | |
| 39 |
37 |
ROS_ACTION_CALL_TIMEOUT = 30.0
|
| 40 |
38 |
|
| 41 |
39 |
#######################################################################
|
| 42 |
|
def __init__(self, scope, loglevel=logging.WARNING):
|
|
40 |
def __init__(self, scope, loglevel=logging.WARNING, timeout=None):
|
| 43 |
41 |
"""initialise
|
| 44 |
42 |
:param scope: base scope we want to listen on
|
| 45 |
43 |
"""
|
| 46 |
44 |
#init base settings
|
| 47 |
45 |
Middleware.__init__(self,scope,loglevel)
|
| 48 |
|
#call mw init
|
| 49 |
|
self.init_middleware()
|
|
46 |
#ros-specific initialization
|
|
47 |
if timeout is None: timeout = rospy.Duration()
|
|
48 |
self.init_middleware(timeout)
|
| 50 |
49 |
|
| 51 |
50 |
def __del__(self):
|
| 52 |
51 |
"""destructor
|
| ... | ... | |
| 54 |
53 |
self.logger.debug("destructor of MiddlewareROS called")
|
| 55 |
54 |
|
| 56 |
55 |
#######################################################################
|
| 57 |
|
def init_middleware(self):
|
|
56 |
def init_middleware(self, timeout):
|
| 58 |
57 |
"""initialise middleware
|
| 59 |
58 |
"""
|
| 60 |
59 |
self.logger.info("initialising ROS middleware connection on scope %s" % (self.base_scope))
|
| ... | ... | |
| 62 |
61 |
try:
|
| 63 |
62 |
rospy.init_node('hlrc_client', anonymous=True)
|
| 64 |
63 |
except rospy.exceptions.ROSException as e:
|
| 65 |
|
print e
|
|
64 |
self.logger.info(str(e))
|
| 66 |
65 |
|
| 67 |
66 |
self.logger.info("creating action clients")
|
| 68 |
67 |
|
| 69 |
|
self.logger.debug("creating speech action client")
|
| 70 |
|
self.speech_client = actionlib.SimpleActionClient(self.base_scope + "/set/speech", speechAction)
|
| 71 |
|
self.speech_client.wait_for_server()
|
| 72 |
|
|
| 73 |
|
self.logger.debug("creating default emotion action client")
|
| 74 |
|
self.default_emotion_client = actionlib.SimpleActionClient(self.base_scope + "/set/defaultEmotion", emotionstateAction)
|
| 75 |
|
self.default_emotion_client.wait_for_server()
|
| 76 |
|
|
| 77 |
|
self.logger.debug("creating current emotion action client")
|
| 78 |
|
self.current_emotion_client = actionlib.SimpleActionClient(self.base_scope + "/set/currentEmotion", emotionstateAction)
|
| 79 |
|
self.current_emotion_client.wait_for_server()
|
| 80 |
|
|
| 81 |
|
self.logger.debug("creating animation action client")
|
| 82 |
|
self.animation_client = actionlib.SimpleActionClient(self.base_scope + "/set/animation", animationAction)
|
| 83 |
|
self.animation_client.wait_for_server()
|
| 84 |
|
|
| 85 |
|
self.logger.debug("creating gazetarget action client")
|
| 86 |
|
self.gazetarget_client = actionlib.SimpleActionClient(self.base_scope + "/set/gaze", gazetargetAction)
|
| 87 |
|
self.gazetarget_client.wait_for_server()
|
| 88 |
|
|
| 89 |
|
self.logger.debug("creating lookattarget action client")
|
| 90 |
|
self.lookattarget_client = actionlib.SimpleActionClient(self.base_scope + "/set/lookat", lookattargetAction)
|
| 91 |
|
self.lookattarget_client.wait_for_server()
|
| 92 |
|
|
| 93 |
|
self.logger.debug("creating mouthtarget action client")
|
| 94 |
|
self.mouthtarget_client = actionlib.SimpleActionClient(self.base_scope + "/set/mouth", mouthtargetAction)
|
| 95 |
|
self.mouthtarget_client.wait_for_server()
|
|
68 |
start = time.time()
|
|
69 |
def create_client(sub_scope, action, name):
|
|
70 |
if not timeout.is_zero():
|
|
71 |
t = rospy.Duration.from_sec(timeout.to_sec() - (time.time() - start)) # time left
|
|
72 |
if t <= rospy.Duration():
|
|
73 |
raise Exception("timeout while connecting to actionlib servers")
|
|
74 |
else:
|
|
75 |
t = timeout
|
|
76 |
|
|
77 |
self.logger.debug("creating %s client" % name)
|
|
78 |
|
|
79 |
client = actionlib.SimpleActionClient(self.base_scope + sub_scope, action)
|
|
80 |
if not client.wait_for_server(t):
|
|
81 |
raise Exception("timeout while creating %s client" % name)
|
|
82 |
return client
|
|
83 |
|
|
84 |
self.speech_client = create_client("/set/speech", speechAction, "speech action")
|
|
85 |
self.default_emotion_client = create_client("/set/defaultEmotion", emotionstateAction, "default emotion")
|
|
86 |
self.current_emotion_client = create_client("/set/currentEmotion", emotionstateAction, "current emotion")
|
|
87 |
self.animation_client = create_client("/set/animation", animationAction, "animation")
|
|
88 |
self.gazetarget_client = create_client("/set/gaze", gazetargetAction, "gazetarget")
|
|
89 |
self.lookattarget_client = create_client("/set/lookat", lookattargetAction, "lookattarget")
|
|
90 |
self.mouthtarget_client = create_client("/set/mouth", mouthtargetAction, "mouthtarget")
|
| 96 |
91 |
|
| 97 |
92 |
self.logger.info("MiddlewareROS initialised")
|
| 98 |
93 |
|