Statistics
| Branch: | Tag: | Revision:

hlrc / client / python / hlrc_client / MiddlewareROS.py @ c7de5860

History | View | Annotate | Download (12.434 KB)

1 0c286af0 Simon Schulz
"""
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 70c54617 Simon Schulz
import roslib;
32
import rospy
33 64f5c90e Simon Schulz
import actionlib
34
from hlrc_server.msg import *
35 2126781b Simon Schulz
from actionlib_msgs.msg import GoalStatus
36 0c286af0 Simon Schulz
37
class MiddlewareROS(Middleware):
38 3877047d Simon Schulz
    #default timeout to wait in case of blocking calls
39
    ROS_ACTION_CALL_TIMEOUT = 30.0
40
41
    #######################################################################
42
    def __init__(self, scope, loglevel=logging.WARNING):
43 70c54617 Simon Schulz
        """initialise
44
        :param scope: base scope we want to listen on
45
        """
46
        #init base settings
47
        Middleware.__init__(self,scope,loglevel)
48
        #call mw init
49
        self.init_middleware()
50
51
    def __del__(self):
52
        """destructor
53
        """
54
        self.logger.debug("destructor of MiddlewareROS called")
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 02fc76ae Simon Schulz
    #######################################################################
93
    def is_running(self):
94
        return not rospy.is_shutdown()
95 70c54617 Simon Schulz
96
    #######################################################################
97
    def publish_emotion(self, em_type, emotion, blocking):
98
        """publish an emotion via mw
99
        :param em_type: type of emotion (RobotEmotion::TYPE_DEFAULT or RobotEmotion::TYPE_CURRENT)
100
        :param emotion: emotion to set
101
        :param blocking: True if this call should block until execution finished on robot
102
        """
103
        self.logger.debug("calling the emotion rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
104
105
        #create a goal to send to the action server.
106
        goal = emotionstateGoal()
107
        goal.value    = self.convert_emotiontype_to_rosval(emotion.value)
108
        goal.duration =  int(emotion.time_ms)
109
110
        #which client do we use?
111
        if (em_type == RobotEmotion.TYPE_DEFAULT):
112
            client = self.default_emotion_client
113
        else:
114
            client = self.current_emotion_client
115
116
        #send the goal to the server
117
        if (blocking):
118 2126781b Simon Schulz
            #send and wait:
119
            state = client.send_goal_and_wait(goal, execute_timeout = rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
120
            if (state != GoalStatus.SUCCEEDED):
121
                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))
122
        else:
123
            #non blocking, fire & forget
124
            client.send_goal(goal)
125
126
        self.logger.debug("emotion rpc done")
127 70c54617 Simon Schulz
128
    def publish_default_emotion(self, emotion, blocking):
129
        self.publish_emotion(RobotEmotion.TYPE_DEFAULT, emotion, blocking)
130
131
    def publish_current_emotion(self, emotion, blocking):
132
        self.publish_emotion(RobotEmotion.TYPE_CURRENT, emotion, blocking)
133
134
    def publish_head_animation(self, animation, blocking):
135
        """publish an head animation via mw
136
        :param animation: animation to set
137
        :param blocking: True if this call should block until execution finished on robot
138
        """
139
        self.logger.debug("calling the animation rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
140
141
        #create a goal to send to the action server.
142
        goal = animationGoal()
143
        goal.target = self.convert_animationtype_to_rosval(animation.value)
144
        goal.repetitions = animation.repetitions
145
        goal.duration_each =  int(animation.time_ms)
146
        goal.scale = animation.scale
147
148
        #send the goal to the server
149
        if (blocking):
150 2126781b Simon Schulz
            #send and wait:
151
            state = self.animation_client.send_goal_and_wait(goal, execute_timeout = rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
152
            if (state != GoalStatus.SUCCEEDED):
153
                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))
154
        else:
155
            #non blocking, fire & forget
156
            self.animation_client.send_goal(goal)
157 70c54617 Simon Schulz
158
        self.logger.debug("animation rpc done")
159
160
    def publish_gaze_target(self, gaze, blocking):
161
        """publish a gaze target via mw
162
        :param gaze: gaze to set
163
        :param blocking: True if this call should block until execution finished on robot
164
        """
165
        self.logger.debug("calling the gaze rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
166
167
        #create a goal to send to the action server.
168
        goal = gazetargetGoal()
169
        goal.pan  = gaze.pan
170
        goal.tilt = gaze.tilt
171
        goal.roll = gaze.roll
172
        goal.vergence = gaze.vergence
173
        goal.pan_offset  = gaze.pan_offset
174
        goal.tilt_offset = gaze.tilt_offset
175
176
        #type
177
        if (gaze.gaze_type == RobotGaze.GAZETARGET_ABSOLUTE):
178
            goal.gaze_type = gazetargetGoal.GAZETARGET_ABSOLUTE
179
        else:
180
            goal.gaze_type = gazetargetGoal.GAZETARGET_RELATIVE
181
182 0e7e9a0e Simon Schulz
        goal.gaze_timestamp = rospy.Time.from_sec(gaze.timestamp)
183 70c54617 Simon Schulz
184
        #send the goal to the server
185
        if (blocking):
186 2126781b Simon Schulz
            #send and wait:
187
            state = self.gazetarget_client.send_goal_and_wait(goal, execute_timeout = rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
188
            if (state != GoalStatus.SUCCEEDED):
189
                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))
190
        else:
191
            #non blocking, fire & forget
192
            self.gazetarget_client.send_goal(goal)
193 70c54617 Simon Schulz
194
        self.logger.debug("gaze rpc done")
195
196
    def publish_mouth_target(self, mouth, blocking):
197
        """publish a mouth target via mw
198
        :param mouth: mouth value to set
199
        :param blocking: True if this call should block until execution finished on robot
200
        """
201
        self.logger.debug("calling the mouth rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
202
203
        #create a goal to send to the action server.
204
        goal = mouthtargetGoal()
205
        goal.opening_left  = mouth.opening_left
206
        goal.opening_center = mouth.opening_center
207
        goal.opening_right = mouth.opening_right
208
        goal.position_left = mouth.position_left
209
        goal.position_center  = mouth.position_center
210
        goal.position_right = mouth.position_right
211
212
        #send the goal to the server
213
        if (blocking):
214 2126781b Simon Schulz
            #send and wait:
215
            state = self.mouthtarget_client.send_goal_and_wait(goal, execute_timeout = rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
216
            if (state != GoalStatus.SUCCEEDED):
217
                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))
218
        else:
219
            #non blocking, fire & forget
220
            self.mouthtarget_client.send_goal(goal)
221 70c54617 Simon Schulz
222
        self.logger.debug("mouth rpc done")
223
224
    def publish_speech(self, text_, blocking):
225
        """publish a tts request via mw
226
        :param text_: text to synthesize and speak
227
        :param blocking: True if this call should block until execution finished on robot
228
        """
229
        self.logger.debug("calling the speech rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
230
231
        #create a goal to send to the action server.
232
        goal = speechGoal(text=text_)
233
234
        #send the goal to the server
235
        if (blocking):
236 2126781b Simon Schulz
            #send and wait:
237
            state = self.speech_client.send_goal_and_wait(goal, execute_timeout = rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
238
            if (state != GoalStatus.SUCCEEDED):
239
                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))
240
        else:
241
            #non blocking, fire & forget
242
            self.speech_client.send_goal(goal)
243 70c54617 Simon Schulz
244
        self.logger.debug("speech rpc done")
245
246
    #######################################################################
247
    # some helpers
248
    def convert_animationtype_to_rosval(self, value):
249
        """convert RobotAnimation.value to ROS animation value
250
        :param value: RobotAnimation.* id to convert to rsb id
251
        """
252
        #NOTE: this convertion is important as the actual integer values of
253
        #      thy python api and the protobuf might be different
254
        if (value == RobotAnimation.IDLE):
255
            return animationGoal.IDLE
256
        elif (value == RobotAnimation.HEAD_NOD):
257
            return animationGoal.HEAD_NOD
258
        elif (value == RobotAnimation.HEAD_SHAKE):
259
            return animationGoal.HEAD_SHAKE
260
        elif (value == RobotAnimation.EYEBLINK_L):
261
            return animationGoal.EYEBLINK_L
262
        elif (value == RobotAnimation.EYEBLINK_R):
263
            return  animationGoal.EYEBLINK_R
264
        elif (value == RobotAnimation.EYEBLINK_BOTH):
265
            return  animationGoal.EYEBLINK_BOTH
266
        elif (value == RobotAnimation.EYEBROWS_RAISE):
267
            return  animationGoal.EYEBROWS_RAISE
268
        elif (value == RobotAnimation.EYEBROWS_LOWER):
269
            return  animationGoal.EYEBROWS_LOWER
270
        else:
271
            self.logger.error("invalid animation type %d\n" % (value))
272
            return  animationGoal.NEUTRAL
273
274
    def convert_emotiontype_to_rosval(self, value):
275
        """convert RobotEmotion.value to ROS animation value
276
        :param value: RobotEmotion.* id to convert to ros id
277
        """
278
        #NOTE: this convertion is important as the actual integer values of
279
        #      thy python api and the protobuf might be different
280
281
        if (value == RobotEmotion.NEUTRAL):
282
            return emotionstateGoal.NEUTRAL
283
        elif (value == RobotEmotion.HAPPY):
284
            return emotionstateGoal.HAPPY
285
        elif (value == RobotEmotion.SAD):
286
            return emotionstateGoal.SAD
287
        elif (value == RobotEmotion.ANGRY):
288
            return emotionstateGoal.ANGRY
289
        elif (value == RobotEmotion.SURPRISED):
290
            return  emotionstateGoal.SURPRISED
291
        elif (value == RobotEmotion.FEAR):
292
            return emotionstateGoal.FEAR
293
        else:
294
            self.logger.error("invalid emotion type %d\n" % (value))
295
            return  emotionstateGoal.NEUTRAL