Statistics
| Branch: | Tag: | Revision:

hlrc / client / python / hlrc_client / MiddlewareROS.py @ 62d50515

History | View | Annotate | Download (10.339 KB)

1
"""
2
This file is part of hlrc
3

4
Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
5
http://opensource.cit-ec.de/projects/hlrc
6

7
This file may be licensed under the terms of the
8
GNU General Public License Version 3 (the ``GPL''),
9
or (at your option) any later version.
10

11
Software distributed under the License is distributed
12
on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
13
express or implied. See the GPL for the specific language
14
governing rights and limitations.
15

16
You should have received a copy of the GPL along with this
17
program. If not, go to http://www.gnu.org/licenses/gpl.html
18
or write to the Free Software Foundation, Inc.,
19
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
20

21
The development of this software was supported by the
22
Excellence Cluster EXC 277 Cognitive Interaction Technology.
23
The Excellence Cluster EXC 277 is a grant of the Deutsche
24
Forschungsgemeinschaft (DFG) in the context of the German
25
Excellence Initiative.
26
"""
27

    
28
from Middleware import *
29
import errno
30

    
31
try:
32
        import roslib; 
33
        
34
        import rospy 
35
        import actionlib
36
        from hlrc_server.msg import *
37
    
38
except ImportError as exception:
39
        sys.stderr.write("ImportError: {}\n> HINT: try to export PYTHONPATH=$PYTHONPATH:$YOUR_PREFIX/lib/python2.7/site-packages/\n\n".format(exception))
40
        sys.exit(errno.ENOPKG)
41

    
42
class MiddlewareROS(Middleware):
43
        #default timeout to wait in case of blocking calls
44
        ROS_ACTION_CALL_TIMEOUT = 30.0
45
        
46
        #######################################################################
47
        def __init__(self, scope, loglevel=logging.WARNING):
48
                """initialise
49
                :param scope: base scope we want to listen on 
50
                """
51
                #init base settings
52
                Middleware.__init__(self,scope,loglevel)
53
                #call mw init
54
                self.init_middleware()
55
                
56
        #######################################################################
57
        def init_middleware(self):
58
                """initialise middleware
59
                """
60
                self.logger.info("initialising ROS middleware connection on scope %s" % (self.base_scope))
61
                
62
                rospy.init_node('hlrc_client', anonymous=True)
63
                
64
                self.logger.info("creating action clients")
65
                
66
                self.logger.debug("creating speech action client")
67
                self.speech_client = actionlib.SimpleActionClient(self.base_scope + "/set/speech", speechAction)
68
                self.speech_client.wait_for_server()
69
                
70
                self.logger.debug("creating default emotion action client")
71
                self.default_emotion_client = actionlib.SimpleActionClient(self.base_scope + "/set/defaultEmotion", emotionstateAction)
72
                self.default_emotion_client.wait_for_server()
73
                
74
                self.logger.debug("creating current emotion action client")
75
                self.current_emotion_client = actionlib.SimpleActionClient(self.base_scope + "/set/currentEmotion", emotionstateAction)
76
                self.current_emotion_client.wait_for_server()
77
                
78
                self.logger.debug("creating animation action client")
79
                self.animation_client = actionlib.SimpleActionClient(self.base_scope + "/set/animation", animationAction)
80
                self.animation_client.wait_for_server()
81
                
82
                self.logger.debug("creating gazetarget action client")
83
                self.gazetarget_client = actionlib.SimpleActionClient(self.base_scope + "/set/gaze", gazetargetAction)
84
                self.gazetarget_client.wait_for_server()
85
                
86
                self.logger.debug("creating mouthtarget action client")
87
                self.mouthtarget_client = actionlib.SimpleActionClient(self.base_scope + "/set/mouth", mouthtargetAction)
88
                self.mouthtarget_client.wait_for_server()
89
                
90
                self.logger.info("MiddlewareROS initialised")
91
                
92

    
93
        #######################################################################
94
        def publish_emotion(self, em_type, emotion, blocking):
95
                """publish an emotion via mw
96
                :param em_type: type of emotion (RobotEmotion::TYPE_DEFAULT or RobotEmotion::TYPE_CURRENT)
97
                :param emotion: emotion to set
98
                :param blocking: True if this call should block until execution finished on robot
99
                """
100
                self.logger.debug("calling the emotion rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
101
                
102
                #create a goal to send to the action server.
103
                goal = emotionstateGoal()
104
                goal.value    = self.convert_emotiontype_to_rosval(emotion.value)
105
                goal.duration =  int(emotion.time_ms)
106
                
107
                #which client do we use?
108
                if (em_type == RobotEmotion.TYPE_DEFAULT):
109
                        client = self.default_emotion_client
110
                else:
111
                        client = self.current_emotion_client
112
                
113
                #send the goal to the server
114
                client.send_goal(goal)
115
                
116
                #wait for the server to finish 
117
                if (blocking):
118
                        timed_out = client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
119
                        if (timed_out):
120
                                self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))
121
                
122
        def publish_default_emotion(self, emotion, blocking):
123
                self.publish_emotion(RobotEmotion.TYPE_DEFAULT, emotion, blocking)
124
        
125
        def publish_current_emotion(self, emotion, blocking):
126
                self.publish_emotion(RobotEmotion.TYPE_CURRENT, emotion, blocking)
127
        
128
        def publish_head_animation(self, animation, blocking):
129
                """publish an head animation via mw
130
                :param animation: animation to set
131
                :param blocking: True if this call should block until execution finished on robot
132
                """
133
                self.logger.debug("calling the animation rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
134
                
135
                #create a goal to send to the action server.
136
                goal = animationGoal()
137
                goal.target = self.convert_animationtype_to_rosval(animation.value)
138
                goal.repetitions = animation.repetitions
139
                goal.duration_each =  int(animation.time_ms)
140
                goal.scale = animation.scale
141
                
142
                #send the goal to the server
143
                self.animation_client.send_goal(goal)
144
                
145
                #wait for the server to finish 
146
                if (blocking):
147
                        timed_out = self.animation_client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
148
                        if (timed_out):
149
                                self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))
150
                
151
                self.logger.debug("animation rpc done")
152
        
153
        def publish_gaze_target(self, gaze, blocking):
154
                """publish a gaze target via mw
155
                :param gaze: gaze to set
156
                :param blocking: True if this call should block until execution finished on robot
157
                """
158
                self.logger.debug("calling the gaze rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
159
                
160
                #create a goal to send to the action server.
161
                goal = gazetargetGoal()
162
                goal.pan  = gaze.pan
163
                goal.tilt = gaze.tilt
164
                goal.roll = gaze.roll
165
                goal.vergence = gaze.vergence
166
                goal.pan_offset  = gaze.pan_offset
167
                goal.tilt_offset = gaze.tilt_offset
168
                
169
                #send the goal to the server
170
                self.gazetarget_client.send_goal(goal)
171
                
172
                #wait for the server to finish 
173
                if (blocking):
174
                        timed_out = self.gazetarget_client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
175
                        if (timed_out):
176
                                self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))
177
                
178
                
179
                self.logger.debug("gaze rpc done")
180
        
181
        def publish_mouth_target(self, mouth, blocking):
182
                """publish a mouth target via mw
183
                :param mouth: mouth value to set
184
                :param blocking: True if this call should block until execution finished on robot
185
                """
186
                self.logger.debug("calling the mouth rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
187
                
188
                #create a goal to send to the action server.
189
                goal = mouthtargetGoal()
190
                goal.opening_left  = mouth.opening_left
191
                goal.opening_center = mouth.opening_center
192
                goal.opening_right = mouth.opening_right
193
                goal.position_left = mouth.position_left
194
                goal.position_center  = mouth.position_center
195
                goal.position_right = mouth.position_right
196
                
197
                #send the goal to the server
198
                self.mouthtarget_client.send_goal(goal)
199
                
200
                #wait for the server to finish 
201
                if (blocking):
202
                        timed_out = self.mouthtarget_client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
203
                        if (timed_out):
204
                                self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))
205
                
206
                self.logger.debug("mouth rpc done")
207
        
208
        def publish_speech(self, text_, blocking):
209
                """publish a tts request via mw
210
                :param text_: text to synthesize and speak
211
                :param blocking: True if this call should block until execution finished on robot
212
                """
213
                self.logger.debug("calling the speech rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
214
                
215
                #create a goal to send to the action server.
216
                goal = speechGoal(text=text_)
217
                
218
                #send the goal to the server
219
                self.speech_client.send_goal(goal)
220
                
221
                #wait for the server to finish 
222
                if (blocking):
223
                        timed_out = self.speech_client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
224
                        if (timed_out):
225
                                self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))
226
                                
227
                self.logger.debug("speech rpc done")
228
        
229
        #######################################################################
230
        # some helpers
231
        def convert_animationtype_to_rosval(self, value):
232
                """convert RobotAnimation.value to ROS animation value
233
                :param value: RobotAnimation.* id to convert to rsb id
234
                """
235
                #NOTE: this convertion is important as the actual integer values of
236
                #      thy python api and the protobuf might be different
237
                if (value == RobotAnimation.IDLE):
238
                        return animationGoal.IDLE
239
                elif (value == RobotAnimation.HEAD_NOD):
240
                        return animationGoal.HEAD_NOD
241
                elif (value == RobotAnimation.HEAD_SHAKE):
242
                        return animationGoal.HEAD_SHAKE
243
                elif (value == RobotAnimation.EYEBLINK_L):
244
                        return animationGoal.EYEBLINK_L
245
                elif (value == RobotAnimation.EYEBLINK_R):
246
                        return  animationGoal.EYEBLINK_R
247
                elif (value == RobotAnimation.EYEBLINK_BOTH):
248
                        return  animationGoal.EYEBLINK_BOTH
249
                elif (value == RobotAnimation.EYEBROWS_RAISE):
250
                        return  animationGoal.EYEBROWS_RAISE
251
                elif (value == RobotAnimation.EYEBROWS_LOWER):
252
                        return  animationGoal.EYEBROWS_LOWER
253
                else:
254
                        self.logger.error("invalid animation type %d\n" % (value))
255
                        return  animationGoal.NEUTRAL
256
                
257
        def convert_emotiontype_to_rosval(self, value):
258
                """convert RobotEmotion.value to ROS animation value
259
                :param value: RobotEmotion.* id to convert to ros id
260
                """
261
                #NOTE: this convertion is important as the actual integer values of
262
                #      thy python api and the protobuf might be different
263

    
264
                if (value == RobotEmotion.NEUTRAL):
265
                        return emotionstateGoal.NEUTRAL
266
                elif (value == RobotEmotion.HAPPY):
267
                        return emotionstateGoal.HAPPY
268
                elif (value == RobotEmotion.SAD):
269
                        return emotionstateGoal.SAD
270
                elif (value == RobotEmotion.ANGRY):
271
                        return emotionstateGoal.ANGRY
272
                elif (value == RobotEmotion.SURPRISED):
273
                        return  emotionstateGoal.SURPRISED
274
                elif (value == RobotEmotion.FEAR):
275
                        return emotionstateGoal.FEAR
276
                else:
277
                        self.logger.error("invalid emotion type %d\n" % (value))
278
                        return  emotionstateGoal.NEUTRAL