Revision 2126781b

View differences:

client/python/hlrc_client/MiddlewareROS.py
32 32
import rospy
33 33
import actionlib
34 34
from hlrc_server.msg import *
35
from actionlib_msgs.msg import GoalStatus
35 36

  
36 37
class MiddlewareROS(Middleware):
37 38
    #default timeout to wait in case of blocking calls
......
113 114
            client = self.current_emotion_client
114 115

  
115 116
        #send the goal to the server
116
        client.send_goal(goal)
117

  
118
        #wait for the server to finish
119 117
        if (blocking):
120
            timed_out = client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
121
            if (timed_out):
122
                self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))
118
            #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")
123 127

  
124 128
    def publish_default_emotion(self, emotion, blocking):
125 129
        self.publish_emotion(RobotEmotion.TYPE_DEFAULT, emotion, blocking)
......
142 146
        goal.scale = animation.scale
143 147

  
144 148
        #send the goal to the server
145
        self.animation_client.send_goal(goal)
146

  
147
        #wait for the server to finish
148 149
        if (blocking):
149
            timed_out = self.animation_client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
150
            if (timed_out):
151
                self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))
150
            #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)
152 157

  
153 158
        self.logger.debug("animation rpc done")
154 159

  
......
177 182
        goal.gaze_timestamp = rospy.Time.from_sec(gaze.timestamp)
178 183

  
179 184
        #send the goal to the server
180
        self.gazetarget_client.send_goal(goal)
181

  
182
        #wait for the server to finish
183 185
        if (blocking):
184
            timed_out = self.gazetarget_client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
185
            if (timed_out):
186
                self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))
187

  
186
            #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)
188 193

  
189 194
        self.logger.debug("gaze rpc done")
190 195

  
......
205 210
        goal.position_right = mouth.position_right
206 211

  
207 212
        #send the goal to the server
208
        self.mouthtarget_client.send_goal(goal)
209

  
210
        #wait for the server to finish
211 213
        if (blocking):
212
            timed_out = self.mouthtarget_client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
213
            if (timed_out):
214
                self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))
214
            #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)
215 221

  
216 222
        self.logger.debug("mouth rpc done")
217 223

  
......
226 232
        goal = speechGoal(text=text_)
227 233

  
228 234
        #send the goal to the server
229
        self.speech_client.send_goal(goal)
230

  
231
        #wait for the server to finish
232 235
        if (blocking):
233
            timed_out = self.speech_client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
234
            if (timed_out):
235
                self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))
236
            #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)
236 243

  
237 244
        self.logger.debug("speech rpc done")
238 245

  
client/python/test_relative_gaze.py
49 49

  
50 50

  
51 51
    while(r.is_running()):
52
        r.set_gaze_target(g)
53
        time.sleep(1.0 / 50.0)
52
        r.set_gaze_target(g, True)
53
        time.sleep(1.0) # / 50.0)
server/include/ROS/GazeCallbackWrapperROS.h
73 73

  
74 74
        //default:
75 75
        gaze_state.vergence  = request->vergence;
76

  
77 76
        gaze_state.pan_offset = request->pan_offset;
78 77
        gaze_state.tilt_offset = request->tilt_offset;
79 78

  

Also available in: Unified diff