Revision 3877047d

View differences:

client/cpp/examples/random_gaze/main.cpp
58 58
			gaze.vergence = 0.0;
59 59

  
60 60
            //absolute mode
61
            gaze.gaze_type = RobotGaze::ABSOLUTE;
62
            gaze.timestamp = RobotGaze::now();
61
            gaze.gaze_type      = RobotGaze::ABSOLUTE;
62
            gaze.gaze_timestamp = RobotTimestamp::now();
63 63

  
64 64
            //robot_controller->set_current_emotion(RobotEmotion(RobotEmotion::ANGRY));
65 65

  
client/cpp/include/RobotGaze.h
28 28

  
29 29
#pragma once
30 30
#include <time.h>
31
#include "RobotTimestamp.h"
31 32

  
32 33
class RobotGaze{
33 34
public:
34
    RobotGaze(){
35
    RobotGaze() : gaze_timestamp() {
35 36
        pan = 0.0;
36 37
        tilt = 0.0;
37 38
        roll = 0.0;
......
41 42
        gaze_type = ABSOLUTE;
42 43
    }
43 44

  
44
    static double now(){
45
    /*static double now(){
45 46
        //fetch time
46 47
        struct timespec tp;
47 48
        clock_gettime(CLOCK_REALTIME, &tp);
48 49
        return tp.tv_sec+tp.tv_nsec/1000000000.0;
49
    };
50
    };*/
50 51

  
51 52
    enum {
52 53
        RELATIVE = 0,
53 54
        ABSOLUTE = 1
54 55
    };
55 56

  
56
    double timestamp;
57

  
57
    RobotTimestamp gaze_timestamp;
58 58
    int gaze_type;
59 59

  
60 60
    float pan, tilt, roll, vergence;
client/cpp/src/MiddlewareROS.cpp
154 154
    goal.pan_offset = target.pan_offset;
155 155

  
156 156
    //timestamp:
157
    goal.timestamp = ros::Time(target.timestamp);
157
    goal.gaze_timestamp.sec  = target.gaze_timestamp.sec;
158
    goal.gaze_timestamp.nsec = target.gaze_timestamp.nsec;
158 159

  
159 160
    if (target.gaze_type == RobotGaze::ABSOLUTE){
160 161
        goal.gaze_type = hlrc_server::gazetargetGoal::GAZETARGET_ABSOLUTE;
client/cpp/src/MiddlewareRSB.cpp
40 40
#include <boost/algorithm/string.hpp>
41 41
#include <boost/range/algorithm_ext/erase.hpp>
42 42

  
43
WARNING: RSB interface might be deprtecated and needs some backporting
44
from the ROS code. [todo]
45

  
43 46
using namespace std;
44 47
using namespace rsb;
45 48
using namespace rsb::patterns;
client/python/CMakeLists.txt
6 6

  
7 7
find_program(PYTHON "python")
8 8

  
9
#HACK for qtcreator ide integration. do not remove!
10
file(GLOB_RECURSE HEADER_LIST RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.py)
11
add_custom_target(python_sources SOURCES ${HEADER_LIST})
12

  
9 13
if (PYTHON)
10 14
    set(SETUP_PY    "${CMAKE_CURRENT_SOURCE_DIR}/setup.py")
11 15
    set(OUTPUT      "${CMAKE_CURRENT_SOURCE_DIR}/build")
client/python/hlrc_client/Middleware.py
33 33
from RobotAnimation import *
34 34

  
35 35
class Middleware:
36
	#######################################################################
37
	def __init__(self, scope, loglevel=logging.WARNING):
38
		"""initialise
39
		:param scope: base scope we want to listen on 
40
		"""
41
		self.base_scope = scope
42
		
43
		self.logger = logging.getLogger(__name__)
44
		
45
		# create nice and actually usable formatter and add it to the handler
46
		self.config_logger(loglevel)
47
		
48
		#initialise defaults
49
		self.default_emotion = RobotEmotion()
50
		self.current_emotion = RobotEmotion()
51
		self.gaze_target = RobotGaze()
52
		self.mouth_target = RobotMouth()
53
		self.robot_animation = RobotAnimation()
54
			
36
    #######################################################################
37
    def __init__(self, scope, loglevel=logging.WARNING):
38
	"""initialise
39
	:param scope: base scope we want to listen on
40
	"""
41
	self.base_scope = scope
42

  
43
	self.logger = logging.getLogger(__name__)
44

  
45
	# create nice and actually usable formatter and add it to the handler
46
	self.config_logger(loglevel)
47

  
48
	#initialise defaults
49
	self.default_emotion = RobotEmotion()
50
	self.current_emotion = RobotEmotion()
51
	self.gaze_target = RobotGaze()
52
	self.mouth_target = RobotMouth()
53
	self.robot_animation = RobotAnimation()
54

  
55 55
	def __del__(self):
56
		"""destructor
57
		"""
58
		self.logger.debug("destructor of Middleware called")
59
	
60
	def config_logger(self, level):
56
	    """destructor
57
	    """
58
	    self.logger.debug("destructor of Middleware called")
59

  
60
	    def config_logger(self, level):
61 61
		formatter = logging.Formatter('%(asctime)s %(name)-30s %(levelname)-8s > %(message)s')
62 62
		ch = logging.StreamHandler()
63 63
		#ch.setLevel(level)
64 64
		ch.setFormatter(formatter)
65 65
		self.logger.setLevel(level)
66 66
		self.logger.addHandler(ch)
67
	
68
	#######################################################################
69
        #abstract/virtual functions
70
        def die_virtual(self, funcname):
71
		raise NotImplementedError(funcname + "() is virtual, must be overwritten")
72
	
73
	def init_middleware(self):
74
		self.die_virtual(sys._getframe().f_code.co_name)
75
	
76
	def publish_default_emotion(self, emotion, blocking):
77
		self.die_virtual(sys._getframe().f_code.co_name)
78
	
79
	def publish_current_emotion(self, emotion, blocking):
80
		self.die_virtual(sys._getframe().f_code.co_name)
81
	
82
	def publish_gaze_target(self, gaze, blocking):
83
		self.die_virtual(sys._getframe().f_code.co_name)
84
	
85
	def publish_mouth_target(self, mouth, blocking):
86
		self.die_virtual(sys._getframe().f_code.co_name)
87
	
88
	def publish_head_animation(self, animation, blocking):
89
		self.die_virtual(sys._getframe().f_code.co_name)
90
	
91
	def publish_speech(self, text, blocking):
92
		self.die_virtual(sys._getframe().f_code.co_name)
93
	
94
	#######################################################################
95
        #finally some implemented functions
96
        def set_default_emotion(self, emotion, blocking):
97
		"""set the default emotion
98
		:param emotion: RobotEmotion to set 
99
		:param blocking: True if this call should block until execution finished on robot
100
		"""
101
		self.default_emotion = emotion
102
		self.publish_default_emotion(emotion, blocking)
103
		
104
	def set_current_emotion(self, emotion, blocking):
105
		"""set a temporary emotion (duration: see emotion.time_ms)
106
		:param emotion: RobotEmotion to set temporarily
107
		:param blocking: True if this call should block until execution finished on robot
108
		"""
109
		self.current_emotion = emotion
110
		self.publish_current_emotion(emotion, blocking)
111
	
112
	def set_head_animation(self, animation, blocking):
113
		"""trigger a head animation
114
		:param animation: RobotAnimation to set
115
		:param blocking: True if this call should block until execution finished on robot
116
		"""
117
		self.animation = animation
118
		self.publish_head_animation(animation, blocking)
119
	
120
	def set_mouth_target(self, mouth, blocking):
121
		"""set a mouth target
122
		:param mouth: RobotMouth to set
123
		:param blocking: True if this call should block until execution finished on robot
124
		"""
125
		self.mouth_target = mouth
126
		self.publish_mouth_target(mouth, blocking)
127

  
128
	def set_speak(self, text, blocking):
129
		"""trigger a tts speech output
130
		:param text: text to synthesize and speak
131
		:param blocking: True if this call should block until execution finished on robot
132
		"""
133
		self.publish_speech(text, blocking)
134
	
135
	def set_gaze_target(self, gaze, blocking):
136
		"""set a new gaze 
137
		:param gaze: RobotGaze to set
138
		:param blocking: True if this call should block until execution finished on robot
139
		"""
140
		self.gaze_target = gaze
141
		self.publish_gaze_target(gaze, blocking)
142
		
143
	#######################################################################
144
	#some get methods
145
	#def get_current_emotion(self):
146
	#	return self.current_emotion
147
	#
148
	#def get_default_emotion(self):
149
	#	return self.default_emotion
150
	#
67

  
68
		#######################################################################
69
		#abstract/virtual functions
70
		def die_virtual(self, funcname):
71
		    raise NotImplementedError(funcname + "() is virtual, must be overwritten")
72

  
73
		def init_middleware(self):
74
		    self.die_virtual(sys._getframe().f_code.co_name)
75

  
76
		    def publish_default_emotion(self, emotion, blocking):
77
			self.die_virtual(sys._getframe().f_code.co_name)
78

  
79
			def publish_current_emotion(self, emotion, blocking):
80
			    self.die_virtual(sys._getframe().f_code.co_name)
81

  
82
			    def publish_gaze_target(self, gaze, blocking):
83
				self.die_virtual(sys._getframe().f_code.co_name)
84

  
85
				def publish_mouth_target(self, mouth, blocking):
86
				    self.die_virtual(sys._getframe().f_code.co_name)
87

  
88
				    def publish_head_animation(self, animation, blocking):
89
					self.die_virtual(sys._getframe().f_code.co_name)
90

  
91
					def publish_speech(self, text, blocking):
92
					    self.die_virtual(sys._getframe().f_code.co_name)
93

  
94
					    #######################################################################
95
					    #finally some implemented functions
96
					    def set_default_emotion(self, emotion, blocking):
97
						"""set the default emotion
98
						:param emotion: RobotEmotion to set
99
						:param blocking: True if this call should block until execution finished on robot
100
						"""
101
						self.default_emotion = emotion
102
						self.publish_default_emotion(emotion, blocking)
103

  
104
						def set_current_emotion(self, emotion, blocking):
105
						    """set a temporary emotion (duration: see emotion.time_ms)
106
						    :param emotion: RobotEmotion to set temporarily
107
						    :param blocking: True if this call should block until execution finished on robot
108
						    """
109
						    self.current_emotion = emotion
110
						    self.publish_current_emotion(emotion, blocking)
111

  
112
						    def set_head_animation(self, animation, blocking):
113
							"""trigger a head animation
114
							:param animation: RobotAnimation to set
115
							:param blocking: True if this call should block until execution finished on robot
116
							"""
117
							self.animation = animation
118
							self.publish_head_animation(animation, blocking)
119

  
120
							def set_mouth_target(self, mouth, blocking):
121
							    """set a mouth target
122
							    :param mouth: RobotMouth to set
123
							    :param blocking: True if this call should block until execution finished on robot
124
							    """
125
							    self.mouth_target = mouth
126
							    self.publish_mouth_target(mouth, blocking)
127

  
128
							    def set_speak(self, text, blocking):
129
								"""trigger a tts speech output
130
								:param text: text to synthesize and speak
131
								:param blocking: True if this call should block until execution finished on robot
132
								"""
133
								self.publish_speech(text, blocking)
134

  
135
								def set_gaze_target(self, gaze, blocking):
136
								    """set a new gaze
137
								    :param gaze: RobotGaze to set
138
								    :param blocking: True if this call should block until execution finished on robot
139
								    """
140
								    self.gaze_target = gaze
141
								    self.publish_gaze_target(gaze, blocking)
142

  
143
								    #######################################################################
144
								    #some get methods
145
								    #def get_current_emotion(self):
146
									#	return self.current_emotion
147
									#
148
									#def get_default_emotion(self):
149
									    #	return self.default_emotion
150
									    #
client/python/hlrc_client/MiddlewareROS.py
34 34
from hlrc_server.msg import *
35 35

  
36 36
class MiddlewareROS(Middleware):
37
	#default timeout to wait in case of blocking calls
38
	ROS_ACTION_CALL_TIMEOUT = 30.0
39
	
40
	#######################################################################
41
	def __init__(self, scope, loglevel=logging.WARNING):
42
		"""initialise
43
		:param scope: base scope we want to listen on 
44
		"""
45
		#init base settings
46
		Middleware.__init__(self,scope,loglevel)
47
		#call mw init
48
		self.init_middleware()
49
			
37
    #default timeout to wait in case of blocking calls
38
    ROS_ACTION_CALL_TIMEOUT = 30.0
39

  
40
    #######################################################################
41
    def __init__(self, scope, loglevel=logging.WARNING):
42
	"""initialise
43
	:param scope: base scope we want to listen on
44
	"""
45
	#init base settings
46
	Middleware.__init__(self,scope,loglevel)
47
	#call mw init
48
	self.init_middleware()
49

  
50 50
	def __del__(self):
51
		"""destructor
52
		"""
53
		self.logger.debug("destructor of MiddlewareROS called")
54
		
55
	#######################################################################
56
        def init_middleware(self):
51
	    """destructor
52
	    """
53
	    self.logger.debug("destructor of MiddlewareROS called")
54

  
55
	    #######################################################################
56
	    def init_middleware(self):
57 57
		"""initialise middleware
58 58
		"""
59 59
		self.logger.info("initialising ROS middleware connection on scope %s" % (self.base_scope))
......
89 89
		self.logger.info("MiddlewareROS initialised")
90 90
		
91 91

  
92
	#######################################################################
93
	def publish_emotion(self, em_type, emotion, blocking):
94
		"""publish an emotion via mw
95
		:param em_type: type of emotion (RobotEmotion::TYPE_DEFAULT or RobotEmotion::TYPE_CURRENT)
96
		:param emotion: emotion to set
97
		:param blocking: True if this call should block until execution finished on robot
98
		"""
99
		self.logger.debug("calling the emotion rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
100
		
101
		#create a goal to send to the action server.
102
		goal = emotionstateGoal()
103
		goal.value    = self.convert_emotiontype_to_rosval(emotion.value)
104
		goal.duration =  int(emotion.time_ms)
105
		
106
		#which client do we use?
107
		if (em_type == RobotEmotion.TYPE_DEFAULT):
92
		#######################################################################
93
		def publish_emotion(self, em_type, emotion, blocking):
94
		    """publish an emotion via mw
95
		    :param em_type: type of emotion (RobotEmotion::TYPE_DEFAULT or RobotEmotion::TYPE_CURRENT)
96
		    :param emotion: emotion to set
97
		    :param blocking: True if this call should block until execution finished on robot
98
		    """
99
		    self.logger.debug("calling the emotion rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
100

  
101
		    #create a goal to send to the action server.
102
		    goal = emotionstateGoal()
103
		    goal.value    = self.convert_emotiontype_to_rosval(emotion.value)
104
		    goal.duration =  int(emotion.time_ms)
105

  
106
		    #which client do we use?
107
		    if (em_type == RobotEmotion.TYPE_DEFAULT):
108 108
			client = self.default_emotion_client
109
		else:
110
			client = self.current_emotion_client
111
		
112
		#send the goal to the server
113
		client.send_goal(goal)
114
		
115
		#wait for the server to finish 
116
		if (blocking):
117
			timed_out = client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
118
			if (timed_out):
119
				self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))
120
		
121
	def publish_default_emotion(self, emotion, blocking):
122
		self.publish_emotion(RobotEmotion.TYPE_DEFAULT, emotion, blocking)
123
	
124
	def publish_current_emotion(self, emotion, blocking):
125
		self.publish_emotion(RobotEmotion.TYPE_CURRENT, emotion, blocking)
126
	
127
	def publish_head_animation(self, animation, blocking):
128
		"""publish an head animation via mw
129
		:param animation: animation to set
130
		:param blocking: True if this call should block until execution finished on robot
131
		"""
132
		self.logger.debug("calling the animation rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
133
		
134
		#create a goal to send to the action server.
135
		goal = animationGoal()
136
		goal.target = self.convert_animationtype_to_rosval(animation.value)
137
		goal.repetitions = animation.repetitions
138
		goal.duration_each =  int(animation.time_ms)
139
		goal.scale = animation.scale
140
		
141
		#send the goal to the server
142
		self.animation_client.send_goal(goal)
143
		
144
		#wait for the server to finish 
145
		if (blocking):
146
			timed_out = self.animation_client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))
147
			if (timed_out):
148
				self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))
149
		
150
		self.logger.debug("animation rpc done")
151
	
152
	def publish_gaze_target(self, gaze, blocking):
153
		"""publish a gaze target via mw
154
		:param gaze: gaze to set
155
		:param blocking: True if this call should block until execution finished on robot
156
		"""
157
		self.logger.debug("calling the gaze rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))
158
		
159
		#create a goal to send to the action server.
160
		goal = gazetargetGoal()
161
		goal.pan  = gaze.pan
162
		goal.tilt = gaze.tilt
163
		goal.roll = gaze.roll
164
		goal.vergence = gaze.vergence
165
		goal.pan_offset  = gaze.pan_offset
166
		goal.tilt_offset = gaze.tilt_offset
167

  
168
		#type
169
		if (gaze.gaze_type == RobotGaze.GAZETARGET_ABSOLUTE):
170
			goal.gaze_type = gazetargetGoal.GAZETARGET_ABSOLUTE
171
		else:
172
			goal.gaze_type = gazetargetGoal.GAZETARGET_RELATIVE
173

  
174
		goal.timestamp = rospy.Time.from_sec(gaze.timestamp)
175
		
176
		#send the goal to the server
177