Revision 6304bfc1 client/python/hlrc_client/MiddlewareROS.py
| client/python/hlrc_client/MiddlewareROS.py | ||
|---|---|---|
| 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 |
|
Also available in: Unified diff