class Robot(Entity):
ENTITY_TYPE = 'robot'
def __init__(self, expression_enum, gesture_enum):
Entity.__init__(self, Robot.ENTITY_TYPE, Robot.ENTITY_TYPE + str(1), None)
self.expression_enum = expression_enum
self.gesture_enum = gesture_enum
self.gaze_client = actionlib.SimpleActionClient('gaze', TargetAction)
self.gaze_found = False
self.expression_client = MultiGoalActionClient('expression', ExpressionAction)
self.expression_found = False
self.tts_client = actionlib.SimpleActionClient('text_to_speech', TextToSpeechAction)
self.tts_found = False
self.gesture_client = MultiGoalActionClient('gesture', GestureAction)
self.gesture_found = False
self.tts_duration_srv = rospy.ServiceProxy('tts_subsentence_duration', TextToSpeechSubsentenceDuration)
self.tts_duration_found = False
self.event = None
self.action_handles = []
self.say_to_plan = SayToPlan(self)
self.gaze_ah = None
self.say_ah = None
self.lock = threading.RLock()
self.listen_cb = None
self.listen_sub = None
def register_listen_callback(self, callback):
self.listen_cb = callback
self.listen_sub = rospy.Subscriber("itf_listen", String, self.__listen, queue_size=10)
def __listen(self, msg):
if msg.data == 'BADINPUT':
text = None
else:
text = msg.data
self.listen_cb(text)
# Text to speech
def say(self, text):
if not self.tts_found:
self.wait_for_action_servers(self.tts_client)
self.tts_found = True
ParamFormatting.assert_types(self.say, text, str)
goal = TextToSpeechGoal()
goal.sentence = text
self.tts_client.send_goal(goal, feedback_cb=self.say_feedback, done_cb=self.say_done)
ah = SingleGoalActionHandle(self.tts_client)
self.say_ah = ah
self.add_action_handle(ah)
return ah
def say_and_wait(self, text):
self.say(text)
self.tts_client.wait_for_result()
def say_done(self, state, result):
with self.lock:
self.remove_action_handle(self.say_ah)
self.say_ah = None
# Gaze
def gaze(self, target, speed=0.5):
if not self.gaze_found:
self.wait_for_action_servers(self.gaze_client)
self.gaze_found = True
ParamFormatting.assert_types(self.gaze, target, Entity)
ParamFormatting.assert_types(self.gaze, speed, float)
ParamFormatting.assert_range(self.gaze, speed, 0.0, 1.0)
World().add_to_world(target)
goal = TargetGoal()
goal.target = target.get_id()
goal.speed = speed
goal.acceleration = 0.3
self.gaze_client.send_goal(goal, feedback_cb=self.gaze_feedback, done_cb=self.gaze_done)
ah = SingleGoalActionHandle(self.gaze_client)
self.gaze_ah = ah
self.add_action_handle(ah)
return ah
def gaze_and_wait(self, target, speed=0.5, timeout=rospy.Duration()):
self.gaze(target, speed)
self.gaze_client.wait_for_result(timeout)
def gaze_feedback(self, feedback):
pass
def gaze_done(self, state, result):
with self.lock:
self.remove_action_handle(self.gaze_ah)
self.gaze_ah = None
# Blinking
def blink(self, blink_duration, blink_rate_mean, blink_rate_sd):
"""
:param blink_duration:
:param blink_rate_mean:
:param blink_rate_sd:
:return:
"""
raise NotImplementedError("Please implement the blink method")
# Facial expressions
def expression(self, expression, intensity=None, speed=None, duration=None):
if not self.expression_found:
self.wait_for_action_servers(self.expression_client)
self.expression_found = True
ParamFormatting.assert_types(self.expression, expression, IExpression)
goal = ExpressionGoal()
goal.expression = expression.name
if intensity is None:
goal.intensity = -1
else:
ParamFormatting.assert_types(self.expression, intensity, float)
ParamFormatting.assert_range(self.expression, intensity, 0.0, 1.0)
goal.intensity = intensity
if speed is None:
goal.speed = -1
else:
ParamFormatting.assert_types(self.expression, speed, float)
ParamFormatting.assert_range(self.expression, speed, 0.0, 1.0)
goal.speed = speed
if duration is None:
goal.duration = -1
else:
ParamFormatting.assert_types(self.expression, duration, float)
ParamFormatting.assert_greater_than(self.expression, duration, 0.0)
goal.duration = duration
gh = self.expression_client.send_goal(goal, done_cb=self.expression_done)
ah = MultiGoalActionHandle(self.expression_client, gh)
self.add_action_handle(ah)
return ah
def expression_and_wait(self, expression, intensity=None, speed=None, duration=None, timeout=rospy.Duration()):
ah = self.expression(expression, intensity, speed, duration)
self.expression_client.wait_for_result(ah.goal_handle, timeout)
def expression_done(self, goal_handle):
with self.lock:
ah = self.get_action_handle(goal_handle)
self.remove_action_handle(ah)
# Gestures
def gesture(self, gesture, target=None, duration=None):
if not self.gesture_found:
self.wait_for_action_servers(self.gesture_client)
self.gesture_found = True
ParamFormatting.assert_types(self.gesture, gesture, IGesture)
goal = GestureGoal()
goal.gesture = gesture.name
if target is None:
goal.target = ''
else:
World().add_to_world(target)
ParamFormatting.assert_types(self.gesture, target, Entity)
goal.target = target.get_id()
if duration is None:
goal.duration = -1
else:
ParamFormatting.assert_types(self.expression, duration, float)
ParamFormatting.assert_greater_than(self.expression, duration, 0.0)
goal.duration = duration
gh = self.gesture_client.send_goal(goal, done_cb=self.gesture_done)
ah = MultiGoalActionHandle(self.gesture_client, gh)
self.add_action_handle(ah)
return ah
def gesture_and_wait(self, gesture, target=None, duration=None, timeout=rospy.Duration()):
ah = self.gesture(gesture, target, duration)
self.gesture_client.wait_for_result(ah.goal_handle, timeout)
def gesture_done(self, goal_handle):
with self.lock:
ah = self.get_action_handle(goal_handle)
self.remove_action_handle(ah)
# Speaking, gazing and gesturing simultaneously
def say_to(self, text, audience):
if not self.tts_duration_found:
self.wait_for_services(self.tts_duration_srv)
self.tts_duration_found = True
ParamFormatting.assert_types(self.say_to, text, str)
ParamFormatting.assert_types(self.say_to, audience, Entity, Query, list)
self.say_to_plan.parse_parameters(text, audience, self.expression_enum, self.gesture_enum, self.tts_duration_srv)
ah = self.say_to_plan.execute()
self.add_action_handle(ah)
return ah
def say_to_and_wait(self, text, audience):
ah = self.say_to(text, audience)
self.wait(ah)
def say_feedback(self, feedback):
if feedback.current_word_index in self.say_to_plan.gaze_change_locations:
if isinstance(self.say_to_plan.audience, Entity):
person = self.say_to_plan.audience
self.say_to_plan.current_gazee = person
self.say_to_plan.gaze_ah = self.gaze(person.head)
elif isinstance(self.say_to_plan.audience, Query):
people = self.say_to_plan.audience.execute()
if len(people) > 1:
#if self.say_to_plan.current_gazee in people:
# people.remove(self.say_to_plan.current_gazee)
person = random.choice(people)
self.say_to_plan.current_gazee = person
self.say_to_plan.gaze_ah = self.gaze(person.head)
elif len(people) == 1:
person = people[0]
self.say_to_plan.current_gazee = person
self.say_to_plan.gaze_ah = self.gaze(person.head)
if feedback.current_word_index in self.say_to_plan.expression_lookup:
expression = self.say_to_plan.expression_lookup[feedback.current_word_num]
ahs = self.do(expression)
self.say_to_plan.add_action_handle(ahs[0])
if feedback.current_word_index in self.say_to_plan.gesture_lookup:
gesture = self.say_to_plan.gesture_lookup[feedback.current_word_index]
ahs = self.do(gesture)
self.say_to_plan.add_action_handle(ahs[0])
def add_action_handle(self, action_handle):
with self.lock:
ParamFormatting.assert_types(self.add_action_handle, action_handle, IActionHandle)
self.action_handles.append(action_handle)
def get_action_handle(self, goal_handle):
with self.lock:
for ah in self.action_handles:
if isinstance(ah, MultiGoalActionHandle):
if ah.goal_handle == goal_handle:
return ah
def remove_action_handle(self, action_handle):
with self.lock:
ParamFormatting.assert_types(self.remove_action_handle, action_handle, IActionHandle)
if action_handle in self.action_handles:
self.action_handles.remove(action_handle)
def do(self, *goals):
action_handles = []
with self.lock:
for goal in goals:
if isinstance(goal, TextToSpeechGoal):
self.tts_client.send_goal(goal)
ah = SingleGoalActionHandle(self.tts_client)
self.add_action_handle(ah)
action_handles.append(ah)
elif isinstance(goal, TargetGoal):
self.gaze_client.send_goal(goal)
ah = SingleGoalActionHandle(self.gaze_client)
self.add_action_handle(ah)
action_handles.append(ah)
elif isinstance(goal, ExpressionGoal):
gh = self.expression_client.send_goal(goal)
ah = MultiGoalActionHandle(self.expression_client, gh)
self.add_action_handle(ah)
action_handles.append(ah)
elif isinstance(goal, GestureGoal):
gh = self.gesture_client.send_goal(goal)
ah = MultiGoalActionHandle(self.gesture_client, gh)
self.add_action_handle(ah)
action_handles.append(ah)
else:
raise TypeError('robot.do parameter goals has a goal with a unsupported type {0}, at index {1}. Should be one of: TextToSpeechGoal, TargetGoal, ExpressionGoal or GestureGoal'.format(type(goal), goals.index(goal)))
return action_handles
# Wait for one or more goals to finish
def wait(self, *action_handles):
for ah in action_handles:
ParamFormatting.assert_types(self.wait, ah, IActionHandle)
ah.wait_for_result()
self.remove_action_handle(action_handle=ah)
# Cancel one or more goals
def cancel(self, *action_handles):
for ah in action_handles:
ParamFormatting.assert_types(self.wait, ah, IActionHandle)
ah.cancel_action()
self.remove_action_handle(action_handle=ah)
def default_tf_frame_id(self):
raise NotImplementedError("Please implement this method")
def tf_frame_id(self):
raise NotImplementedError("Please implement this method")
# Wait for a period of time
def wait_for_period(self, period):
self.event = threading.Event()
self.event.wait(timeout=period)
def cancel_wait_for_period(self):
if self.event is not None:
self.event.set()
class Robot(Entity):
""" The Robot class represents a robot. It contains high level functions for representing and controlling robot
actions, receiving callbacks about action events and subscribing to social communication.
.. note::
The Robot class is an abstract class and is never directly used to program a robot. Instead, each robot has
its own class derived from Robot, e.g. the Nao robot is represented by the Nao class.
See here for more details: http://en.wikipedia.org/wiki/Abstract_type
"""
local_id = 0 # Id counter for robot classes
SERVER_TIMEOUT = 5.0 # Timeout for connecting to action servers and services
def __init__(self):
self.local_id = Robot.local_id
Robot.local_id += 1
Entity.__init__(self, 'robot{0}'.format(self.local_id), None)
robot_actions = Robot.read_supported_actions()
robot_services = Robot.read_supported_services()
self.say_found = False
self.gaze_found = False
self.gesture_found = False
self.expression_found = False
self.say_duration_found = False
# Create the action and service proxies that connect to vendor specific action servers and services.
if SayAction.__name__ in robot_actions:
namespace = robot_actions[SayAction.__name__]['namespace']
self.say_client = actionlib.SimpleActionClient(namespace, SayAction)
self.say_found = Robot.wait_for_action_server(self.say_client, SayAction.__name__)
else:
self.say_client = None
if GazeAction.__name__ in robot_actions:
namespace = robot_actions[GazeAction.__name__]['namespace']
self.gaze_client = actionlib.SimpleActionClient(namespace, TargetAction)
self.gaze_found = Robot.wait_for_action_server(self.gaze_client, GazeAction.__name__)
else:
self.gaze_client = None
if GestureAction.__name__ in robot_actions:
namespace = robot_actions[GestureAction.__name__]['namespace']
self.gesture_client = MultiGoalActionClient(namespace, GestureAction)
self.gesture_found = Robot.wait_for_action_server(self.gesture_client, GestureAction.__name__)
else:
self.gesture_client = None
if ExpressionAction.__name__ in robot_actions:
namespace = robot_actions[Action.expression.name]['namespace']
self.expression_client = MultiGoalActionClient(namespace, ExpressionAction)
self.expression_found = Robot.wait_for_action_server(self.gesture_client, ExpressionAction.__name__)
else:
self.expression_client = None
if SayDuration.__name__ in robot_services:
name = robot_services[SayDuration.__name__]['name']
self.say_duration_srv = rospy.ServiceProxy(namespace, SayDuration)
self.say_duration_found = Robot.wait_for_service(self.say_duration_srv, SayDuration.__name__)
else:
self.say_duration_srv = None
self.gaze_ah = None
self.say_ah = None
self.lock = threading.RLock()
self.listen_cb = None
self.listen_sub = None
def default_body_part(self):
raise NotImplementedError("Please implement this method")
def register_listen_callback(self, callback):
self.listen_cb = callback
self.listen_sub = rospy.Subscriber("itf_listen", String, self.__listen, queue_size=10)
def __listen(self, msg):
if msg.data == 'BADINPUT':
text = None
else:
text = msg.data
self.listen_cb(text)
def say(self, text, feedback_cb=None, done_cb=None):
""" Say the text in the variable text.
This function is asynchronous; it returns an ActionHandle, a unique identifier for the action being performed.
An action handle can be used to wait for an action to complete or to stop an action
:param text: The text the robot should say
:type text: str
:param feedback_cb: a function that will be called when the state of the say action changes. The function
should have one parameter, which will be filled with a SayActionFeedback instance.
:type feedback_cb: Callable
:param done_cb: a function that will be called when the say action finishes.
:type done_cb: Callable
:return: an action handle to keep track of the action
:rtype: SingleGoalActionHandle
:raises TypeError: text is not a str, feedback_cb is not None or Callable, done_cb is not None or Callable
"""
TypeChecker.accepts(inspect.currentframe().f_code.co_name,
(str, (None, Callable), (None, Callable)),
text, feedback_cb, done_cb)
if Robot.is_action_runnable(self.say_client, SayAction.__name__, self.say_found):
goal = SayGoal(text)
self.say_client.send_goal(goal, feedback_cb, done_cb)
ah = SingleGoalActionHandle(self.say_client)
self.say_ah = ah
return ah
def say_and_wait(self, text, feedback_cb=None, done_cb=None):
""" A synchronous version of the say function.
The and_wait syntax signals that the function will not return until it completes.
No ActionHandle is returned because the action completes before it returns.
"""
TypeChecker.accepts(inspect.currentframe().f_code.co_name,
(str, (None, Callable), (None, Callable)),
text, feedback_cb, done_cb)
self.say(text, feedback_cb, done_cb)
self.say_client.wait_for_result()
def gaze(self, target, speed=0.5, feedback_cb=None, done_cb=None):
""" Makes the robot gaze at a target.
This function is asynchronous and returns an SingleGoalActionHandle to keep track of the action.
:param target: The target to gaze at
:type target: Entity
:param speed: The speed of the gaze action (normalised between 0.0 < speed <= 1.0)
:type speed: float
:param feedback_cb: a function that will be called when the state of the gaze action changes. The function
should have one parameter, which will be filled with a GazeActionFeedback instance.
:type feedback_cb: Callable
:param done_cb: a function that will be called when the gaze action finishes.
:type done_cb: Callable
:return: an action handle to keep track of the action
:rtype: SingleGoalActionHandle
:raises TypeError: target is not an Entity, speed is not a float, feedback_cb is not None or Callable, done_cb
is not None or Callable
"""
TypeChecker.accepts(inspect.currentframe().f_code.co_name,
(Entity, float, (None, Callable), (None, Callable)),
target, speed, feedback_cb, done_cb)
if Robot.is_action_runnable(self.gaze_client, GazeAction.__name__, self.gaze_found):
World().add_to_world(target)
goal = GazeGoal(target, speed, 0.3) #Todo override acceleration goal.acceleration = 0.3
self.gaze_client.send_goal(goal, feedback_cb, done_cb)
ah = SingleGoalActionHandle(self.gaze_client)
self.gaze_ah = ah
return ah
def gaze_and_wait(self, target, speed=0.5, feedback_cb=None, done_cb=None):
""" A synchronous version of the gaze function.
No ActionHandle is returned because the action completes before it returns.
"""
TypeChecker.accepts(inspect.currentframe().f_code.co_name,
(Entity, float, (None, Callable), (None, Callable)),
target, speed, feedback_cb, done_cb)
self.gaze(target, speed, feedback_cb, done_cb)
self.gaze_client.wait_for_result()
def gesture(self, gesture, duration=Default(), target=None, feedback_cb=None, done_cb=None):
""" Makes the robot perform a gesture, e.g. make a robot wave its left arm.
The default values for the duration of each gesture are specified in the robots IGesture enumeration definition.
:param gesture: the gesture to perform, an IGesture Python enumeration member e.g. for Nao, Gesture.WaveLArm.
:type gesture: IGesture
:param duration: the length of time the gesture plays for (seconds). If Default then the gesture is performed for
its default duration. Use positive infinity for a never ending gesture, hint: float('inf')
:type duration: float, Default
:param target: the target to orient the gesture toward
:type target: None, Entity
:param feedback_cb: a function that will be called when the state of the gesture action changes. The function
should have one parameter, which will be filled with a GestureActionFeedback instance.
:type feedback_cb: Callable
:param done_cb: a function that will be called when the gesture action finishes.
:type done_cb: Callable
:return: an action handle to keep track of the action
:rtype: MultiGoalActionHandle
:raises TypeError: gesture is not an IGesture member, duration is not a float or Default, target is not None or
an Entity, feedback_cb is not None or Callable, done_cb is not None or Callable
"""
TypeChecker.accepts(inspect.currentframe().f_code.co_name,
(IGesture, (Default, float), (None, Entity), (None, Callable), (None, Callable)),
gesture, duration, feedback_cb, done_cb)
if Robot.is_action_runnable(self.gesture_client, GestureAction.__name__, self.gesture_found):
if target is not None:
World().add_to_world(target)
goal = GestureGoal(gesture, duration, target) #TODO: check goal.target = target.get_id()
gh = self.gesture_client.send_goal(goal, feedback_cb, done_cb)
ah = MultiGoalActionHandle(self.gesture_client, gh)
return ah
def gesture_and_wait(self, gesture, duration=Default, target=None, feedback_cb=None, done_cb=None):
""" A synchronous version of the gesture function.
No ActionHandle is returned because the action completes before it returns.
"""
TypeChecker.accepts(inspect.currentframe().f_code.co_name,
(IGesture, (Default, float), (None, Entity), (None, Callable), (None, Callable)),
gesture, duration, feedback_cb, done_cb)
ah = self.gesture(gesture, duration, target, feedback_cb, done_cb)
self.gesture_client.wait_for_result(ah.goal_handle)
def expression(self, expression, intensity=Default(), speed=Default(), duration=Default(), feedback_cb=None, done_cb=None):
""" Makes the robot perform a facial expression, e.g. smile.
The default values for the intensity, speed and duration of each gesture are specified in the robots IExpression
enumeration definition.
This function is asynchronous and returns an ActionHandle.
:param expression: The type of expression to perform, an IExpression enumeration member (e.g.for Zeno, Expression.Smile).
:type expression: IExpression subclass member
:param intensity: The strength of the expression e.g. a smile with an intensity of 1.0 is the largest possible smile
(normalised between 0.0 < speed <= 1.0).
:type intensity: float, Default
:param speed: how fast the expression is performed, e.g. a smile expression with a speed of 1.0 is
performed at the fastest possible speed (normalized between 0.0 <= speed <= 1.0). If Default() then the
expression is performed at its default speed.
:type speed: float, Default
:param duration: how long the expression lasts (seconds). If Default() then the expression is performed for its
default duration. Use positive infinity for a never ending expression, hint: float('inf')
:type duration: float, Default
:param feedback_cb: a function that will be called when the state of the expression action changes. The function
should have one parameter, which will be filled with a ExpressionActionFeedback instance.
:type feedback_cb: Callable
:param done_cb: a function that will be called when the expression action finishes.
:type done_cb: Callable
:return: an action handle to keep track of the action
:rtype: MultiGoalActionHandle
:raises TypeError: expression is not an IExpression member, intensity is not a float or Default, speed is not a
float or Default, duration is not a float or Default, feedback_cb is not None or Callable, done_cb is not None or Callable
"""
TypeChecker.accepts(inspect.currentframe().f_code.co_name,
(IExpression, (Default, float), (Default, float), (Default, float), (None, Callable), (None, Callable)),
expression, intensity, speed, duration, feedback_cb, done_cb)
if Robot.is_action_runnable(self.expression_client, ExpressionAction.__name__, self.expression_found):
goal = ExpressionGoal(expression, intensity, speed, duration)
gh = self.expression_client.send_goal(goal, feedback_cb, done_cb)
ah = MultiGoalActionHandle(self.expression_client, gh)
return ah
def expression_and_wait(self, expression, intensity=Default, speed=Default, duration=Default, feedback_cb=None, done_cb=None):
""" A synchronous version of the expression function.
No ActionHandle is returned because the action completes before it returns.
"""
TypeChecker.accepts(inspect.currentframe().f_code.co_name,
(IExpression, (Default, float), (Default, float), (Default, float), (None, Callable), (None, Callable)),
expression, intensity, speed, duration, feedback_cb, done_cb)
ah = self.expression(expression, intensity, speed, duration, feedback_cb, done_cb)
self.expression_client.wait_for_result(ah.goal_handle)
def simultaneously(self, *goals):
action_handles = []
with self.lock:
for goal in goals:
if isinstance(goal, SayGoal):
if Robot.is_action_runnable(self.say_client, SayAction.__name__, self.say_found):
self.say_client.send_goal(goal)
ah = SingleGoalActionHandle(self.say_client)
action_handles.append(ah)
elif isinstance(goal, GazeGoal):
if Robot.is_action_runnable(self.gaze_client, GazeAction.__name__, self.gaze_found):
self.gaze_client.send_goal(goal)
ah = SingleGoalActionHandle(self.gaze_client)
action_handles.append(ah)
elif isinstance(goal, GestureGoal):
if Robot.is_action_runnable(self.gesture_client, GestureAction.__name__, self.gesture_found):
gh = self.gesture_client.send_goal(goal)
ah = MultiGoalActionHandle(self.gesture_client, gh)
action_handles.append(ah)
elif isinstance(goal, ExpressionGoal):
if Robot.is_action_runnable(self.expression_client, ExpressionAction.__name__, self.expression_found):
gh = self.expression_client.send_goal(goal)
ah = MultiGoalActionHandle(self.expression_client, gh)
action_handles.append(ah)
else:
raise TypeError('robot.simultaneously parameter goals has a goal with a unsupported type {0}, at index {1}. Should be one of: TextToSpeechGoal, TargetGoal, ExpressionGoal or GestureGoal'.format(type(goal), goals.index(goal)))
return action_handles
# TODO: simultaneously action handle
# TODO: consecutively method and action handle
# Wait for one or more goals to finish
def wait(self, *action_handles):
for ah in action_handles:
#TODO: ParamAssertions.assert_types(self.wait, ah, IActionHandle)
ah.wait_for_result()
# Cancel one or more goals
def cancel(self, *action_handles):
for ah in action_handles:
#TODO: ParamAssertions.assert_types(self.wait, ah, IActionHandle)
ah.cancel_action()
@staticmethod
def is_action_runnable(action_client, action_type_name, action_found):
if action_client is None:
rospy.logerr("Cannot do {0} action because this action hasn't been configured for your robot. Please define "
"the {0} action and the namespace of its action server in your "
"robots yaml configuration file".format(action_type_name))
return False
elif not action_found:
rospy.logerr("Cannot do {0} action because the {0} action server could not be found. Please check that "
"the {0} action server with the namespace {1} is running. "
"Hint: rostopic list | grep {1}".format(action_type_name, action_client.action_client.ns))
return False
return True
@staticmethod
def is_service_callable(service_proxy, service_type_name, service_found):
if service_proxy is None:
rospy.logerr("Cannot call {0} service because this service hasn't been defined for your robot. Please define "
"the {0} service and its name in your robots yaml configuration file".format(service_type_name))
return False
elif not service_found:
rospy.logerr("Cannot call {0} service because the {0} service could not be found. Please check that "
"the {0} service with the name {1} is running. "
"Hint: rosservice list | grep {1}".format(service_type_name, service_proxy.name))
return False
return True
@staticmethod
def wait_for_action_server(action_client, action_type_name):
connected = action_client.wait_for_server(timeout=rospy.Duration.from_sec(Robot.SERVER_TIMEOUT))
if not connected:
rospy.logerr("{0} actions are disabled because I could not connect to the {0} action server. Please "
"check that the {0} action server with the namespace {1} is running. "
"Hint: rostopic list | grep {1}".format(action_type_name, action_client.action_client.ns))
return connected
@staticmethod
def wait_for_service(service_proxy, service_type_name):
connected = service_proxy.wait_for_service(timeout=rospy.Duration.from_sec(Robot.SERVER_TIMEOUT))
if not connected:
rospy.logerr("The {0} service is disabled because I could not connect to the {0} service. Please "
"check that the {0} service with the namespace {1} is running. "
"Hint: rosservice list | grep {1}".format(service_type_name, service_proxy.name))
return connected
class TestMultiGoalActionClient(TestCase):
ACTION_SERVER_NAME = "test_action_server"
def setUp(self):
global count
rospy.init_node("test", anonymous=True)
self.action_server = TimerActionServer(
TestMultiGoalActionClient.ACTION_SERVER_NAME + "_" + str(count))
self.action_server.start_server()
self.client = MultiGoalActionClient(
TestMultiGoalActionClient.ACTION_SERVER_NAME + "_" + str(count),
TimerAction)
self.client.wait_for_server()
count += 1
def tearDown(self):
self.client.stop()
def test_wait_for_server(self):
started1 = self.client.wait_for_server(
timeout=rospy.Duration.from_sec(1.0))
client2 = MultiGoalActionClient("i_dont_exist", TimerAction)
started2 = client2.wait_for_server(
timeout=rospy.Duration.from_sec(1.0))
self.assertEqual(started1, True)
self.assertEqual(started2, False)
def test_send_goal(self):
timer = TimerGoal()
timer.duration = 0.5
gh1 = self.client.send_goal(timer)
success = self.client.wait_for_result(
gh1, timeout=rospy.Duration.from_sec(0.6))
self.assertEqual(success, True)
def test_get_result(self):
timer = TimerGoal()
timer.duration = 0.5
gh1 = self.client.send_goal(timer)
self.client.wait_for_result(gh1, timeout=rospy.Duration.from_sec(0.55))
result = self.client.get_result(gh1)
self.assertEqual(isinstance(result, TimerResult), True)
self.assertEqual(result.success, True)
def test_send_two_goals_serial(self):
# First goal
timer1 = TimerGoal()
timer1.duration = 0.5
gh1 = self.client.send_goal(timer1)
success1 = self.client.wait_for_result(
gh1, timeout=rospy.Duration.from_sec(0.55))
# Second goal
timer2 = TimerGoal()
timer2.duration = 0.5
gh2 = self.client.send_goal(timer2)
success2 = self.client.wait_for_result(
gh2, timeout=rospy.Duration.from_sec(0.6))
self.assertEqual(success1, True)
self.assertEqual(success2, True)
def test_send_two_goals_parallel(self):
timer1 = TimerGoal()
timer2 = TimerGoal()
timer1.duration = 1.0
timer2.duration = 1.0
start = time.time()
# Send both goals
gh1 = self.client.send_goal(timer1)
gh2 = self.client.send_goal(timer2)
result1 = self.client.wait_for_result(
gh1, timeout=rospy.Duration.from_sec(1.1))
result2 = self.client.wait_for_result(
gh2, timeout=rospy.Duration.from_sec(1.1))
end = time.time()
duration = end - start
self.assertEqual(result1, True)
self.assertEqual(result2, True)
# self.assertAlmostEqual(duration, 1.0, places=1)
def test_get_goal_id(self):
timer = TimerGoal()
timer.duration = 0.1
gh1 = self.client.send_goal(timer)
goal_id = self.client.goal_id(gh1)
self.assertIsNotNone(goal_id)
def test_is_tracking_goal(self):
timer = TimerGoal()
timer.duration = 0.1
gh1 = self.client.send_goal(timer)
tracking = self.client.is_tracking_goal(gh1)
self.assertEqual(tracking, True)
def test_get_action_handle(self):
timer = TimerGoal()
timer.duration = 1.0
gh1 = self.client.send_goal(timer)
time.sleep(0.5)
action_handle = self.client.get_action_handle(gh1)
self.assertEqual(isinstance(action_handle, ActionHandle), True)
def test_get_state(self):
timer = TimerGoal()
timer.duration = 1.0
gh1 = self.client.send_goal(timer)
pending = self.client.get_state(gh1)
time.sleep(0.5)
active = self.client.get_state(gh1)
self.client.wait_for_result(gh1, timeout=rospy.Duration.from_sec(1.1))
succeeded = self.client.get_state(gh1)
# gh2 = self.client.send_goal(timer)
# time.sleep(0.5)
# self.client.cancel_goal(gh2)
#preeempted = self.client.get_state(gh2)
self.assertEqual(pending, GoalStatus.PENDING)
self.assertEqual(active, GoalStatus.ACTIVE)
self.assertEqual(succeeded, GoalStatus.SUCCEEDED)
# self.assertEqual(preeempted, GoalStatus)
def test_cancel_goal(self):
timer1 = TimerGoal()
timer1.duration = 5.0
gh1 = self.client.send_goal(timer1)
pending = self.client.get_state(gh1)
time.sleep(0.5)
active = self.client.get_state(gh1)
self.client.cancel_goal(gh1)
time.sleep(0.5)
preempted = self.client.get_state(gh1)
self.assertEqual(GoalStatus.PENDING, pending)
self.assertEqual(GoalStatus.ACTIVE, active)
self.assertEqual(GoalStatus.PREEMPTED, preempted)
def test_cancel_all_goals(self):
timer1 = TimerGoal()
timer2 = TimerGoal()
timer1.duration = 1.0
timer2.duration = 1.0
gh1 = self.client.send_goal(timer1)
gh2 = self.client.send_goal(timer2)
time.sleep(0.5)
self.client.cancel_all_goals()
time.sleep(0.5)
self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state(gh1))
self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state(gh2))
def test_cancel_goals_at_and_before_time(self):
before_time1 = TimerGoal()
before_time2 = TimerGoal()
before_time1.duration = 5.0
before_time2.duration = 5.0
gh1 = self.client.send_goal(before_time1)
gh2 = self.client.send_goal(before_time2)
cancel_time = rospy.Time().now()
after_time_1 = TimerGoal()
after_time_1.duration = 1.0
gh3 = self.client.send_goal(after_time_1)
time.sleep(0.5)
self.client.cancel_goals_at_and_before_time(cancel_time)
success = self.client.wait_for_result(
gh3, timeout=rospy.Duration.from_sec(1.1))
self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state(gh1))
self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state(gh2))
self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state(gh3))
self.assertEqual(success, True)
def test_done_callback(self):
timer = TimerGoal()
timer.duration = 0.5
mock = Mock()
gh1 = self.client.send_goal(timer, done_cb=mock)
time.sleep(1.0)
mock.assert_called_once_with(gh1)
def test_active_callback(self):
timer = TimerGoal()
timer.duration = 0.5
mock = Mock()
gh1 = self.client.send_goal(timer, active_cb=mock)
self.client.wait_for_result(gh1, timeout=rospy.Duration.from_sec(1.1))
mock.assert_called_once_with(gh1)
def test_feedback_callback(self):
timer = TimerGoal()
timer.duration = 1.0
mock = Mock()
gh1 = self.client.send_goal(timer, feedback_cb=mock)
self.client.wait_for_result(gh1, timeout=rospy.Duration.from_sec(1.1))
mock.assert_called_once_with(gh1, TimerFeedback(current_time=10))
class Robot(Entity):
ENTITY_TYPE = "robot"
def __init__(self, expression_enum, gesture_enum):
Entity.__init__(self, Robot.ENTITY_TYPE, Robot.ENTITY_TYPE + str(1), None)
self.expression_enum = expression_enum
self.gesture_enum = gesture_enum
self.gaze_client = actionlib.SimpleActionClient("gaze", TargetAction)
self.gaze_found = False
self.expression_client = MultiGoalActionClient("expression", ExpressionAction)
self.expression_found = False
self.tts_client = actionlib.SimpleActionClient("text_to_speech", TextToSpeechAction)
self.tts_found = False
self.gesture_client = MultiGoalActionClient("gesture", GestureAction)
self.gesture_found = False
self.tts_duration_srv = rospy.ServiceProxy("tts_subsentence_duration", TextToSpeechSubsentenceDuration)
self.tts_duration_found = False
self.event = None
self.action_handles = []
self.say_to_plan = SayToPlan(self)
self.gaze_ah = None
self.say_ah = None
self.lock = threading.RLock()
self.listen_cb = None
self.listen_sub = None
def register_listen_callback(self, callback):
self.listen_cb = callback
self.listen_sub = rospy.Subscriber("itf_listen", String, self.__listen, queue_size=10)
def __listen(self, msg):
if msg.data == "BADINPUT":
text = None
else:
text = msg.data
self.listen_cb(text)
# Text to speech
def say(self, text):
if not self.tts_found:
self.wait_for_action_servers(self.tts_client)
self.tts_found = True
ParamFormatting.assert_types(self.say, text, str)
goal = TextToSpeechGoal()
goal.sentence = text
self.tts_client.send_goal(goal, feedback_cb=self.say_feedback, done_cb=self.say_done)
ah = SingleGoalActionHandle(self.tts_client)
self.say_ah = ah
self.add_action_handle(ah)
return ah
def say_and_wait(self, text):
self.say(text)
self.tts_client.wait_for_result()
def say_done(self, state, result):
with self.lock:
self.remove_action_handle(self.say_ah)
self.say_ah = None
# Gaze
def gaze(self, target, speed=0.5):
if not self.gaze_found:
self.wait_for_action_servers(self.gaze_client)
self.gaze_found = True
ParamFormatting.assert_types(self.gaze, target, Entity)
ParamFormatting.assert_types(self.gaze, speed, float)
ParamFormatting.assert_range(self.gaze, speed, 0.0, 1.0)
World().add_to_world(target)
goal = TargetGoal()
goal.target = target.get_id()
goal.speed = speed
goal.acceleration = 0.3
self.gaze_client.send_goal(goal, feedback_cb=self.gaze_feedback, done_cb=self.gaze_done)
ah = SingleGoalActionHandle(self.gaze_client)
self.gaze_ah = ah
self.add_action_handle(ah)
return ah
def gaze_and_wait(self, target, speed=0.5, timeout=rospy.Duration()):
self.gaze(target, speed)
self.gaze_client.wait_for_result(timeout)
def gaze_feedback(self, feedback):
pass
def gaze_done(self, state, result):
with self.lock:
self.remove_action_handle(self.gaze_ah)
self.gaze_ah = None
# Blinking
def blink(self, blink_duration, blink_rate_mean, blink_rate_sd):
"""
:param blink_duration:
:param blink_rate_mean:
:param blink_rate_sd:
:return:
"""
raise NotImplementedError("Please implement the blink method")
# Facial expressions
def expression(self, expression, intensity=None, speed=None, duration=None):
if not self.expression_found:
self.wait_for_action_servers(self.expression_client)
self.expression_found = True
ParamFormatting.assert_types(self.expression, expression, IExpression)
goal = ExpressionGoal()
goal.expression = expression.name
if intensity is None:
goal.intensity = -1
else:
ParamFormatting.assert_types(self.expression, intensity, float)
ParamFormatting.assert_range(self.expression, intensity, 0.0, 1.0)
goal.intensity = intensity
if speed is None:
goal.speed = -1
else:
ParamFormatting.assert_types(self.expression, speed, float)
ParamFormatting.assert_range(self.expression, speed, 0.0, 1.0)
goal.speed = speed
if duration is None:
goal.duration = -1
else:
ParamFormatting.assert_types(self.expression, duration, float)
ParamFormatting.assert_greater_than(self.expression, duration, 0.0)
goal.duration = duration
gh = self.expression_client.send_goal(goal, done_cb=self.expression_done)
ah = MultiGoalActionHandle(self.expression_client, gh)
self.add_action_handle(ah)
return ah
def expression_and_wait(self, expression, intensity=None, speed=None, duration=None, timeout=rospy.Duration()):
ah = self.expression(expression, intensity, speed, duration)
self.expression_client.wait_for_result(ah.goal_handle, timeout)
def expression_done(self, goal_handle):
with self.lock:
ah = self.get_action_handle(goal_handle)
self.remove_action_handle(ah)
# Gestures
def gesture(self, gesture, target=None, duration=None):
if not self.gesture_found:
self.wait_for_action_servers(self.gesture_client)
self.gesture_found = True
ParamFormatting.assert_types(self.gesture, gesture, IGesture)
goal = GestureGoal()
goal.gesture = gesture.name
if target is None:
goal.target = ""
else:
World().add_to_world(target)
ParamFormatting.assert_types(self.gesture, target, Entity)
goal.target = target.get_id()
if duration is None:
goal.duration = -1
else:
ParamFormatting.assert_types(self.expression, duration, float)
ParamFormatting.assert_greater_than(self.expression, duration, 0.0)
goal.duration = duration
gh = self.gesture_client.send_goal(goal, done_cb=self.gesture_done)
ah = MultiGoalActionHandle(self.gesture_client, gh)
self.add_action_handle(ah)
return ah
def gesture_and_wait(self, gesture, target=None, duration=None, timeout=rospy.Duration()):
ah = self.gesture(gesture, target, duration)
self.gesture_client.wait_for_result(ah.goal_handle, timeout)
def gesture_done(self, goal_handle):
with self.lock:
ah = self.get_action_handle(goal_handle)
self.remove_action_handle(ah)
# Speaking, gazing and gesturing simultaneously
def say_to(self, text, audience):
if not self.tts_duration_found:
self.wait_for_services(self.tts_duration_srv)
self.tts_duration_found = True
ParamFormatting.assert_types(self.say_to, text, str)
ParamFormatting.assert_types(self.say_to, audience, Entity, Query, list)
self.say_to_plan.parse_parameters(
text, audience, self.expression_enum, self.gesture_enum, self.tts_duration_srv
)
ah = self.say_to_plan.execute()
self.add_action_handle(ah)
return ah
def say_to_and_wait(self, text, audience):
ah = self.say_to(text, audience)
self.wait(ah)
def say_feedback(self, feedback):
if feedback.current_word_index in self.say_to_plan.gaze_change_locations:
if isinstance(self.say_to_plan.audience, Entity):
person = self.say_to_plan.audience
self.say_to_plan.current_gazee = person
self.say_to_plan.gaze_ah = self.gaze(person.head)
elif isinstance(self.say_to_plan.audience, Query):
people = self.say_to_plan.audience.execute()
if len(people) > 1:
# if self.say_to_plan.current_gazee in people:
# people.remove(self.say_to_plan.current_gazee)
person = random.choice(people)
self.say_to_plan.current_gazee = person
self.say_to_plan.gaze_ah = self.gaze(person.head)
elif len(people) == 1:
person = people[0]
self.say_to_plan.current_gazee = person
self.say_to_plan.gaze_ah = self.gaze(person.head)
if feedback.current_word_index in self.say_to_plan.expression_lookup:
expression = self.say_to_plan.expression_lookup[feedback.current_word_num]
ahs = self.do(expression)
self.say_to_plan.add_action_handle(ahs[0])
if feedback.current_word_index in self.say_to_plan.gesture_lookup:
gesture = self.say_to_plan.gesture_lookup[feedback.current_word_index]
ahs = self.do(gesture)
self.say_to_plan.add_action_handle(ahs[0])
def add_action_handle(self, action_handle):
with self.lock:
ParamFormatting.assert_types(self.add_action_handle, action_handle, IActionHandle)
self.action_handles.append(action_handle)
def get_action_handle(self, goal_handle):
with self.lock:
for ah in self.action_handles:
if isinstance(ah, MultiGoalActionHandle):
if ah.goal_handle == goal_handle:
return ah
def remove_action_handle(self, action_handle):
with self.lock:
ParamFormatting.assert_types(self.remove_action_handle, action_handle, IActionHandle)
if action_handle in self.action_handles:
self.action_handles.remove(action_handle)
def do(self, *goals):
action_handles = []
with self.lock:
for goal in goals:
if isinstance(goal, TextToSpeechGoal):
self.tts_client.send_goal(goal)
ah = SingleGoalActionHandle(self.tts_client)
self.add_action_handle(ah)
action_handles.append(ah)
elif isinstance(goal, TargetGoal):
self.gaze_client.send_goal(goal)
ah = SingleGoalActionHandle(self.gaze_client)
self.add_action_handle(ah)
action_handles.append(ah)
elif isinstance(goal, ExpressionGoal):
gh = self.expression_client.send_goal(goal)
ah = MultiGoalActionHandle(self.expression_client, gh)
self.add_action_handle(ah)
action_handles.append(ah)
elif isinstance(goal, GestureGoal):
gh = self.gesture_client.send_goal(goal)
ah = MultiGoalActionHandle(self.gesture_client, gh)
self.add_action_handle(ah)
action_handles.append(ah)
else:
raise TypeError(
"robot.do parameter goals has a goal with a unsupported type {0}, at index {1}. Should be one of: TextToSpeechGoal, TargetGoal, ExpressionGoal or GestureGoal".format(
type(goal), goals.index(goal)
)
)
return action_handles
# Wait for one or more goals to finish
def wait(self, *action_handles):
for ah in action_handles:
ParamFormatting.assert_types(self.wait, ah, IActionHandle)
ah.wait_for_result()
self.remove_action_handle(action_handle=ah)
# Cancel one or more goals
def cancel(self, *action_handles):
for ah in action_handles:
ParamFormatting.assert_types(self.wait, ah, IActionHandle)
ah.cancel_action()
self.remove_action_handle(action_handle=ah)
def default_tf_frame_id(self):
raise NotImplementedError("Please implement this method")
def tf_frame_id(self):
raise NotImplementedError("Please implement this method")
# Wait for a period of time
def wait_for_period(self, period):
self.event = threading.Event()
self.event.wait(timeout=period)
def cancel_wait_for_period(self):
if self.event is not None:
self.event.set()