Statistics
| Branch: | Tag: | Revision:

hlrc / client / python / hlrc_client / MiddlewareROS.py @ 482adb6d

History | View | Annotate | Download (14.023 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
30 70c54617 Simon Schulz
import rospy
31 64f5c90e Simon Schulz
import actionlib
32
from hlrc_server.msg import *
33 2126781b Simon Schulz
from actionlib_msgs.msg import GoalStatus
34 0c286af0 Simon Schulz
35
class MiddlewareROS(Middleware):
36 3877047d Simon Schulz
    #default timeout to wait in case of blocking calls
37
    ROS_ACTION_CALL_TIMEOUT = 30.0
38
39
    #######################################################################
40 6304bfc1 Robert Haschke
    def __init__(self, scope, loglevel=logging.WARNING, timeout=None):
41 70c54617 Simon Schulz
        """initialise
42
        :param scope: base scope we want to listen on
43
        """
44
        #init base settings
45
        Middleware.__init__(self,scope,loglevel)
46 6304bfc1 Robert Haschke
        #ros-specific initialization
47 b39258f1 Robert Haschke
        if timeout is None:
48
            timeout = rospy.Duration()
49
        if not isinstance(timeout, rospy.Duration):
50
            timeout = rospy.Duration.from_sec(timeout)
51 6304bfc1 Robert Haschke
        self.init_middleware(timeout)
52 70c54617 Simon Schulz
53
    def __del__(self):
54
        """destructor
55
        """
56
        self.logger.debug("destructor of MiddlewareROS called")
57
58
    #######################################################################
59 6304bfc1 Robert Haschke
    def init_middleware(self, timeout):
60 70c54617 Simon Schulz
        """initialise middleware
61
        """
62
        self.logger.info("initialising ROS middleware connection on scope %s" % (self.base_scope))
63
64 67f533d7 Robert Haschke
        try:
65
            rospy.init_node('hlrc_client', anonymous=True)
66
        except rospy.exceptions.ROSException as e:
67 6304bfc1 Robert Haschke
            self.logger.info(str(e))
68 70c54617 Simon Schulz
69
        self.logger.info("creating action clients")
70
71 6304bfc1 Robert Haschke
        start = time.time()
72
        def create_client(sub_scope, action, name):
73
            if not timeout.is_zero():
74
                t = rospy.Duration.from_sec(timeout.to_sec() - (time.time() - start)) # time left
75
                if t <= rospy.Duration():
76
                    raise Exception("timeout while connecting to actionlib servers")
77
            else:
78
                t = timeout
79
80
            self.logger.debug("creating %s client" % name)
81
82
            client = actionlib.SimpleActionClient(self.base_scope + sub_scope, action)
83
            if not client.wait_for_server(t):
84
                raise Exception("timeout while creating %s client" % name)
85
            return client
86
87
        self.speech_client = create_client("/set/speech", speechAction, "speech action")
88
        self.default_emotion_client = create_client("/set/defaultEmotion", emotionstateAction, "default emotion")
89
        self.current_emotion_client = create_client("/set/currentEmotion", emotionstateAction, "current emotion")
90
        self.animation_client = create_client("/set/animation", animationAction, "animation")
91
        self.gazetarget_client = create_client("/set/gaze", gazetargetAction, "gazetarget")
92
        self.lookattarget_client = create_client("/set/lookat", lookattargetAction, "lookattarget")
93
        self.mouthtarget_client = create_client("/set/mouth", mouthtargetAction, "mouthtarget")
94 70c54617 Simon Schulz
95
        self.logger.info("MiddlewareROS initialised")
96
97 02fc76ae Simon Schulz
    #######################################################################
98
    def is_running(self):
99
        return not rospy.is_shutdown()
100 70c54617 Simon Schulz
101
    #######################################################################
102
    def publish_emotion(self, em_type, emotion, blocking):
103
        """publish an emotion via mw
104
        :param em_type: type of emotion (RobotEmotion::TYPE_DEFAULT or RobotEmotion::TYPE_CURRENT)
105
        :param emotion: emotion to set
106
        :param blocking: True if this call should block until execution finished on robot
107
        """
108
        self.logger.debug("calling the emotion rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
109
110
        #create a goal to send to the action server.
111
        goal = emotionstateGoal()
112
        goal.value    = self.convert_emotiontype_to_rosval(emotion.value)
113
        goal.duration =  int(emotion.time_ms)
114
115
        #which client do we use?
116
        if (em_type == RobotEmotion.TYPE_DEFAULT):
117
            client = self.default_emotion_client
118
        else:
119
            client = self.current_emotion_client
120
121
        #send the goal to the server
122
        if (blocking):
123 2126781b Simon Schulz
            #send and wait:
124
            state = client.send_goal_and_wait(goal, execute_timeout = rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
125
            if (state != GoalStatus.SUCCEEDED):
126
                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))
127
        else:
128
            #non blocking, fire & forget
129
            client.send_goal(goal)
130
131
        self.logger.debug("emotion rpc done")
132 70c54617 Simon Schulz
133
    def publish_default_emotion(self, emotion, blocking):
134
        self.publish_emotion(RobotEmotion.TYPE_DEFAULT, emotion, blocking)
135
136
    def publish_current_emotion(self, emotion, blocking):
137
        self.publish_emotion(RobotEmotion.TYPE_CURRENT, emotion, blocking)
138
139
    def publish_head_animation(self, animation, blocking):
140
        """publish an head animation via mw
141
        :param animation: animation to set
142
        :param blocking: True if this call should block until execution finished on robot
143
        """
144
        self.logger.debug("calling the animation rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
145
146
        #create a goal to send to the action server.
147
        goal = animationGoal()
148
        goal.target = self.convert_animationtype_to_rosval(animation.value)
149
        goal.repetitions = animation.repetitions
150
        goal.duration_each =  int(animation.time_ms)
151
        goal.scale = animation.scale
152
153
        #send the goal to the server
154
        if (blocking):
155 2126781b Simon Schulz
            #send and wait:
156
            state = self.animation_client.send_goal_and_wait(goal, execute_timeout = rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
157
            if (state != GoalStatus.SUCCEEDED):
158
                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))
159
        else:
160
            #non blocking, fire & forget
161
            self.animation_client.send_goal(goal)
162 70c54617 Simon Schulz
163
        self.logger.debug("animation rpc done")
164
165
    def publish_gaze_target(self, gaze, blocking):
166
        """publish a gaze target via mw
167
        :param gaze: gaze to set
168
        :param blocking: True if this call should block until execution finished on robot
169
        """
170
        self.logger.debug("calling the gaze rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
171
172
        #create a goal to send to the action server.
173
        goal = gazetargetGoal()
174
        goal.pan  = gaze.pan
175
        goal.tilt = gaze.tilt
176
        goal.roll = gaze.roll
177
        goal.vergence = gaze.vergence
178
        goal.pan_offset  = gaze.pan_offset
179
        goal.tilt_offset = gaze.tilt_offset
180
181
        #type
182
        if (gaze.gaze_type == RobotGaze.GAZETARGET_ABSOLUTE):
183
            goal.gaze_type = gazetargetGoal.GAZETARGET_ABSOLUTE
184
        else:
185
            goal.gaze_type = gazetargetGoal.GAZETARGET_RELATIVE
186
187 04cf2b6f Sebastian Meyer zu Borgsen
        goal.gaze_timestamp = rospy.Time.from_sec(gaze.gaze_timestamp.to_seconds())
188 70c54617 Simon Schulz
189
        #send the goal to the server
190
        if (blocking):
191 2126781b Simon Schulz
            #send and wait:
192
            state = self.gazetarget_client.send_goal_and_wait(goal, execute_timeout = rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
193
            if (state != GoalStatus.SUCCEEDED):
194
                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))
195
        else:
196
            #non blocking, fire & forget
197
            self.gazetarget_client.send_goal(goal)
198 70c54617 Simon Schulz
199
        self.logger.debug("gaze rpc done")
200
201 465c7ad7 Florian Lier
    def publish_lookat_target(self, x, y, z, blocking, frame_id='', roll=0.0):
202
        """publish a gaze target via mw
203
        :param x,y,z: position to look at (in eyes frame or frame_id)
204
        :param roll: side-ways motion of head
205
        :param blocking: True if this call should block until execution finished on robot
206
        """
207
        self.logger.debug("calling the gaze rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
208
209
        # create a goal to send to the action server.
210
        goal = lookattargetGoal()
211 60b91de0 Robert Haschke
        p = goal.point
212
        p.header.frame_id = frame_id
213
        p.header.stamp = rospy.Time.now()
214
        p.point.x = float(x)
215
        p.point.y = float(y)
216
        p.point.z = float(z)
217 465c7ad7 Florian Lier
        goal.roll = roll
218
219
        # send the goal to the server
220
        if blocking:
221
            # send and wait:
222
            state = self.lookattarget_client.send_goal_and_wait(goal, execute_timeout=rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
223
            if state != GoalStatus.SUCCEEDED:
224
                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))
225
        else:
226
            # non blocking, fire & forget
227
            self.lookattarget_client.send_goal(goal)
228
229
        self.logger.debug("lookat rpc done")
230
231 70c54617 Simon Schulz
    def publish_mouth_target(self, mouth, blocking):
232
        """publish a mouth target via mw
233
        :param mouth: mouth value to set
234
        :param blocking: True if this call should block until execution finished on robot
235
        """
236
        self.logger.debug("calling the mouth rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
237
238
        #create a goal to send to the action server.
239
        goal = mouthtargetGoal()
240
        goal.opening_left  = mouth.opening_left
241
        goal.opening_center = mouth.opening_center
242
        goal.opening_right = mouth.opening_right
243
        goal.position_left = mouth.position_left
244
        goal.position_center  = mouth.position_center
245
        goal.position_right = mouth.position_right
246
247
        #send the goal to the server
248
        if (blocking):
249 2126781b Simon Schulz
            #send and wait:
250
            state = self.mouthtarget_client.send_goal_and_wait(goal, execute_timeout = rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
251
            if (state != GoalStatus.SUCCEEDED):
252
                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))
253
        else:
254
            #non blocking, fire & forget
255
            self.mouthtarget_client.send_goal(goal)
256 70c54617 Simon Schulz
257
        self.logger.debug("mouth rpc done")
258
259
    def publish_speech(self, text_, blocking):
260
        """publish a tts request via mw
261
        :param text_: text to synthesize and speak
262
        :param blocking: True if this call should block until execution finished on robot
263
        """
264
        self.logger.debug("calling the speech rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
265
266
        #create a goal to send to the action server.
267
        goal = speechGoal(text=text_)
268
269
        #send the goal to the server
270
        if (blocking):
271 2126781b Simon Schulz
            #send and wait:
272
            state = self.speech_client.send_goal_and_wait(goal, execute_timeout = rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
273
            if (state != GoalStatus.SUCCEEDED):
274
                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))
275
        else:
276
            #non blocking, fire & forget
277
            self.speech_client.send_goal(goal)
278 70c54617 Simon Schulz
279
        self.logger.debug("speech rpc done")
280
281
    #######################################################################
282
    # some helpers
283
    def convert_animationtype_to_rosval(self, value):
284
        """convert RobotAnimation.value to ROS animation value
285
        :param value: RobotAnimation.* id to convert to rsb id
286
        """
287
        #NOTE: this convertion is important as the actual integer values of
288
        #      thy python api and the protobuf might be different
289
        if (value == RobotAnimation.IDLE):
290
            return animationGoal.IDLE
291
        elif (value == RobotAnimation.HEAD_NOD):
292
            return animationGoal.HEAD_NOD
293
        elif (value == RobotAnimation.HEAD_SHAKE):
294
            return animationGoal.HEAD_SHAKE
295
        elif (value == RobotAnimation.EYEBLINK_L):
296
            return animationGoal.EYEBLINK_L
297
        elif (value == RobotAnimation.EYEBLINK_R):
298
            return  animationGoal.EYEBLINK_R
299
        elif (value == RobotAnimation.EYEBLINK_BOTH):
300
            return  animationGoal.EYEBLINK_BOTH
301
        elif (value == RobotAnimation.EYEBROWS_RAISE):
302
            return  animationGoal.EYEBROWS_RAISE
303
        elif (value == RobotAnimation.EYEBROWS_LOWER):
304
            return  animationGoal.EYEBROWS_LOWER
305
        else:
306
            self.logger.error("invalid animation type %d\n" % (value))
307
            return  animationGoal.NEUTRAL
308
309
    def convert_emotiontype_to_rosval(self, value):
310
        """convert RobotEmotion.value to ROS animation value
311
        :param value: RobotEmotion.* id to convert to ros id
312
        """
313
        #NOTE: this convertion is important as the actual integer values of
314
        #      thy python api and the protobuf might be different
315
316
        if (value == RobotEmotion.NEUTRAL):
317
            return emotionstateGoal.NEUTRAL
318
        elif (value == RobotEmotion.HAPPY):
319
            return emotionstateGoal.HAPPY
320
        elif (value == RobotEmotion.SAD):
321
            return emotionstateGoal.SAD
322
        elif (value == RobotEmotion.ANGRY):
323
            return emotionstateGoal.ANGRY
324
        elif (value == RobotEmotion.SURPRISED):
325
            return  emotionstateGoal.SURPRISED
326
        elif (value == RobotEmotion.FEAR):
327
            return emotionstateGoal.FEAR
328
        else:
329
            self.logger.error("invalid emotion type %d\n" % (value))
330
            return  emotionstateGoal.NEUTRAL