diff --git a/.gitignore b/.gitignore
index 48db89c4..36978665 100644
--- a/.gitignore
+++ b/.gitignore
@@ -29,6 +29,7 @@ var/
# before PyInstaller builds the exe, so as to inject date/other infos into it.
*.manifest
*.spec
+*.pem
# Installer logs
pip-log.txt
diff --git a/doc/tutorials/Setting-Up-a-New-Robot.md b/doc/tutorials/Setting-Up-a-New-Robot.md
index c56e93e6..bba37e4b 100644
--- a/doc/tutorials/Setting-Up-a-New-Robot.md
+++ b/doc/tutorials/Setting-Up-a-New-Robot.md
@@ -1,3 +1,121 @@
# Setting Up A Robot
STUB -- need to define interfaces that would be attached to
+
+
+# MISTY robot (API based) V0.1
+
+In order to be able to run HARMONI on an API based robotic platform, a few modifications are needed.
+
+First of all, the IP of the robot, which is needed in most of the _service file, is saved as a ros parameter in the initialization of the harmoni_core/harmoni_common_lib/service_manager.py class; and then retrieved directly in the implementation of the actual service class.
+
+Here are listed the packages and the files that have been modified and added in order to make HARMONI work on the robot.
+
+The docker-compose-full.yml file have been modified in order to be able to run docker in distributed mode, and enable communications between harmoni inside the docker and external networks. In particular, port forwarding paths are necessary, and the network ros_net must not be specified. The network mode must be set to "host".
+## Usage
+
+## Parameters
+
+## Testing
+
+## References
+
+
+### harmoni_actuators
+
+#### harmoni_face
+
+the service of this package does not need to be modified.
+
+A "caller" node file has been added, it's only role is to call the "Display web page" API of the robot.
+
+The node needs to be added to the test and launch files.
+
+The face has been adapted to better fit the robot display, modifying the web/src/js/face.js file.
+
+#### harmoni_gesture
+
+This part of the implementation is still not completed, so the documentation will be adjusted.
+
+Two files have been added to the package: gesture_service_misty.py and misty_gesture_interface.py.
+
+The first one is meant to replace the normal gesture_service file, and thus has been substituted in the launch and test files.
+
+In it, the DO operation that was used before for ordering the robot to make a gesture, has been replaced by a REQUEST operation, which is more in line with the API nature of the interaction.
+
+It is important to remember that when a REQUEST operation is called, it is necessary to set the flag response_received = True when the API call is over.
+
+The parse_gesture_misty() function has been added to the file, and there the gesture described in the xml files in the data folder are processed and for each command composing the gesture an API call is made. A function which prepares the url and the payload for each possible API call has been added to the file, and then the request is forwared to the robot. If the call for any reason timed-out, a second call is made with a longer timeout (sometimes connection problem can happen), and if also that one fails, the function return setting the done flag to false.
+
+Currently the misty_gesture_interface file only publishes some required parameters to ros, but we are planning on moving the api calls described above in this file, in the attempt to better resamble the previous work done on qt. The documentation will be updated consequently.
+
+The xml files contained in data/Misty folder resemble the format for the API calls of the robot, adding only a time parameter which is used for choosing after how much time each single movemente should be performed. Be aware that the time for sending the call is not calculated in a strict manner, so the execution time may consequently be not completely precise.
+
+#### harmoni_speaker
+TODO
+
+
+In this package, the speaker_service_misty.py has been added, and the launch and test files have been modified consequently.
+
+In it, the DO operation that was used before for ordering the robot to play a recording, has been replaced by a REQUEST operation, which is more in line with the API nature of the interaction.
+
+It is important to remember that when a REQUEST operation is called, it is necessary to set the flag response_received = True when the API call is over.
+
+Since Misty needs to recieve the files in base64 format, the file_path_to_audio_data function has been modified to convert the wav file in that format. Currently, the implementation of the robot call providing directly the data is not implemented, and the data must always be given to the node as a wav file. In case free data are provided, the node assumes that it is the tts package calling the speaker, and so retrieve the data where the tts node saves the file as a temporary .wav file.
+
+The function prepares the url and the payload for the API call has been added to the file, and then the request is forwared to the robot. If the call for any reason timed-out, a second call is made with a longer timeout (sometimes connection problem can happen), and if also that one fails, the function return setting the done flag to false.
+
+It is important to note that, since the wav file can be heavier than the maximum file size for the parameters, the payload must be passed as data and not as a parameter to the API call.
+
+An argument robot_ip is provided in the service_manager class.
+In misty, the file must either be encoded in base64 or provided as file. In the speaker_service_misty class the data are converted after being uploaded from a .wav file, either that they are provided to the class directly or via path.
+
+To reproduce the file the flag ImmediatelyApply has been set to True in the API call.
+The flag OverwriteExisting makes the new file replace the old, since the file is always called with the same name.
+Another possible way of doing this was to separate into two different API call, SaveAudio with ImmediatelyApply set to false and then PlayAudioClip.
+
+When replacing harmoni's speaker_service with speaker_service_misty, the type of action that the class performs has been changed from DO to REQUEST: this was done because conceptually we are requesting to an external service (the robot) to perform an action, and not performing the action directly inside HARMONI. This change requires to modify everything that interfaces with the speaker service
+
+Be sure to add the correct robot_ip argument to the launch file if you want to test the case with no exception, or to add a mock one if you to test the exception.
+
+#### harmoni_tts
+
+this package has not been relevantly modified.
+
+#### harmoni_web
+
+this package has not been relevantly modified.
+
+### harmoni_core
+
+#### harmoni_common_lib
+
+the only relevant modification is the addition of the IP of the robot, saved as a ros parameter in the initialization of the harmoni_core/harmoni_common_lib/service_manager.py class. In future this parameter is expected to be passed throught a configuration file. (TODO)
+
+#### harmoni_decision
+
+the configuration file has been modified, adding the parameter gesture: ["misty"] that was previusly commented.
+
+#### harmoni_pattern
+
+two interactions have been added: pattern_scripting/misty_demo.json pattern_scripting/misty_interaction.json and (one for tests and one for the actual interactions). The interactions name must be added also to configuration file too. currently the demos are sort of "mock-ups" of real interactions.
+
+The node has not been relevantly modified.
+
+### harmoni_detectors
+
+this package has not been modified yet.
+
+### harmoni_dialogues
+
+this package has not been modified yet.
+
+### harmoni_sensors
+
+this package has not been modified yet.
+
+
+### HARMONI face: Using Misty API
+An argument robot_ip is provided in the service_manager class.
+
+A class misty_caller has been added to implement automatically a request to display the webView of harmoni face on robot's screen.
diff --git a/docker-run-command.md b/docker-run-command.md
new file mode 100644
index 00000000..9d83a5c3
--- /dev/null
+++ b/docker-run-command.md
@@ -0,0 +1,17 @@
+
+sudo docker run -it --name harmoni_core --network host --init \
+-e "ROS_DISTRO=noetic" \
+-e "CATKIN_WS=harmoni_catkin_ws" \
+-e "IS_DOCKER_ENV=true" \
+-v /home/afar/harmoni_catkin_ws/src/HARMONI/dockerfiles/config/setup_script.sh:"/root/.setup_script.sh" \
+-v /home/afar/harmoni_catkin_ws/src/HARMONI/dockerfiles/config/dev_setup_script.sh:"/root/.dev_setup_script.sh" \
+-v /home/afar/harmoni_catkin_ws/src/HARMONI/dockerfiles/config/asoundrc:"/root/.asoundrc" \
+-v /tmp/.X11-unix:/tmp/.X11-unix \
+-v /home/afar/.ssh/:/root/.ssh:ro \
+-v /home/afar/.vimrc:/root/.vimrc:ro \
+-v /home/afar/.vim/:/root/.vim/:ro \
+-v /etc/timezone:/etc/timezone:ro \
+-v /etc/localtime:/etc/localtime:ro \
+-v /home/afar/harmoni_catkin_ws/src/HARMONI/:/root/local_mount/HARMONI/ \
+-w "/root/harmoni_catkin_ws/src/HARMONI" \
+harmoniteam/harmoni:noetic-full bash
\ No newline at end of file
diff --git a/dockerfiles/compose_templates/docker-compose-full-misty.yml b/dockerfiles/compose_templates/docker-compose-full-misty.yml
new file mode 100644
index 00000000..8e3fa1f6
--- /dev/null
+++ b/dockerfiles/compose_templates/docker-compose-full-misty.yml
@@ -0,0 +1,62 @@
+version: "3.7"
+
+services:
+ harmoni_full:
+ container_name: harmoni_full
+ build:
+ context: .
+ dockerfile: dockerfiles/harmoni/kinetic/full/dockerfile #substitute $ROS_DISTRO with kinetic for now.
+ network: host
+ image: harmoniteam/harmoni:kinetic-full
+ init: true
+ environment:
+# DISPLAY: $DISPLAY
+# QT_GRAPHICSSYSTEM: native
+ ROS_DISTRO: kinetic
+# ROS_MASTER_URI: http://172.18.3.4:11311
+ IS_DOCKER_ENV: "true"
+# ROS_HOSTNAME: harmoni_full
+ CATKIN_WS: harmoni_catkin_ws
+# privileged: true
+ network_mode: "host"
+# networks:
+# ros_net:
+# ipv4_address: 172.18.3.4
+# hostname: harmoni_full
+ ports:
+ - "11312:11312"
+ - "33691:33691"
+ - "8081:8081" #harmoni_face
+ - "8082:8082" #harmoni_web
+ devices:
+# - /dev/dri:/dev/dri
+ - /dev/snd:/dev/snd
+# - /dev/video0:/dev/video0
+ volumes:
+ # - harmoni_catkin_ws:/root/harmoni_catkin_ws
+ - ../HARMONI/:/root/local_mount/HARMONI/
+ # Configuration
+ - ~/.aws:/root/.aws/
+ - ./dockerfiles/config/.asoundrc:/root/.asoundrc
+ - ~/.gcp/private-keys.json:/root/.gcp/private-keys.json
+ # Other
+ - /home/.ssh/:/root/.ssh:ro
+ - /tmp/.X11-unix:/tmp/.X11-unix
+ - /etc/timezone:/etc/timezone:ro
+ - /etc/localtime:/etc/localtime:ro
+ working_dir: /root/harmoni_catkin_ws/src/HARMONI
+
+ command:
+ tail -f /dev/null
+ # bash -c "terminator -ue \"echo 'Entering harmoni_full Container... \\n start with roscore and rlharmoniservices' && bash\""
+
+#networks:
+# ros_net:
+# driver: bridge
+# ipam:
+# driver: default
+# config:
+# - subnet: 172.18.3.0/24
+# volumes:
+# harmoni_catkin_ws:
+# wav2letter:
diff --git a/harmoni_actuators/harmoni_face/launch/face_service_misty.launch b/harmoni_actuators/harmoni_face/launch/face_service_misty.launch
new file mode 100755
index 00000000..79c6de10
--- /dev/null
+++ b/harmoni_actuators/harmoni_face/launch/face_service_misty.launch
@@ -0,0 +1,21 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/harmoni_actuators/harmoni_face/nodes/http_server_runner.py b/harmoni_actuators/harmoni_face/nodes/http_server_runner.py
index 393b33e1..eeed15bc 100755
--- a/harmoni_actuators/harmoni_face/nodes/http_server_runner.py
+++ b/harmoni_actuators/harmoni_face/nodes/http_server_runner.py
@@ -42,6 +42,12 @@ def is_int(value):
if is_int(args[PORT_IDX]):
port = args[PORT_IDX]
command += ["-p", port]
+ #command += ["-S"]
+
+ #cert_path = args[DIR_IDX] + "/server_cert.pem"
+ #key_path = args[DIR_IDX] + "/server_key.pem"
+ #command += ["-C", cert_path]
+ #command += ["-K", key_path]
p = subprocess.Popen(command)
atexit.register(p.terminate)
diff --git a/harmoni_actuators/harmoni_face/src/harmoni_face/face_service.py b/harmoni_actuators/harmoni_face/src/harmoni_face/face_service.py
old mode 100755
new mode 100644
index 25b0e1f9..e0214446
--- a/harmoni_actuators/harmoni_face/src/harmoni_face/face_service.py
+++ b/harmoni_actuators/harmoni_face/src/harmoni_face/face_service.py
@@ -12,79 +12,122 @@
# Specific Imports
from harmoni_common_lib.constants import ActuatorNameSpace
from harmoni_face.msg import FaceRequest
+from harmoni_face.face_client import Face
+from std_msgs.msg import String, Bool
+from geometry_msgs.msg import Point
from threading import Timer
+from collections import deque
import json
import ast
import os
+class NoseService(HarmoniServiceManager):
+ """
+ Nose service
+ """
+
+ def __init__(self, name, param_nose, face):
+ """Init of the nose service and setup as Harmoni service
+ Args:
+ name (str): service name
+ param (obj): list of parameters useful for the service
+ gaze_speed (int): velocity of the gaze command (milliseconds)
+ """
+ super().__init__(name)
+ self.name = name
+ self.service_id = hf.get_child_id(self.name)
+ self.face = face
+ if not self.face.connected:
+ self.face.setup_ros()
+ self.face_nose_pub = rospy.Publisher(
+ ActuatorNameSpace.face.value + self.service_id + "/expressing/nose",
+ FaceRequest,
+ queue_size=1,
+ )
+ self.state = State.INIT
+ return
+
+ def do(self, data):
+ """Do expression of nose in web face
+ Args:
+ data ([str]): stringified json from tts results
+ Returns:
+ response (int): whether SUCCESS of FAIL
+ message (str): result message
+ """
+ self.actuation_completed = False
+ try:
+ self.state = State.REQUEST
+ face_bool = self.face.face_sequential_request(data)
+ rospy.loginfo("Completed Expression")
+ self.state = State.SUCCESS
+ self.actuation_completed = True
+ self.result_msg=""
+ except Exception:
+ self.state = State.FAILED
+ self.actuation_completed = True
+ self.result_msg=""
+ return {"response": self.state, "message": self.result_msg}
+
+ def send_face_nose_request(self):
+ """ Send the request to the web page"""
+ rospy.loginfo("Sending request to webpage of the face eyes")
+ self.face_nose_pub.publish(self.face_request_nose)
+ return
+
class EyesService(HarmoniServiceManager):
"""
- TODO: Eyes service
+ Eyes service
"""
- def __init__(self, name, param):
+ def __init__(self, name, param_eyes, face):
+ """Init of the Eyes service and setup as Harmoni service
+ Args:
+ name (str): service name
+ param (obj): list of parameters useful for the service
+ gaze_speed (int): velocity of the gaze command (milliseconds)
+ """
super().__init__(name)
- """ Initialization of variables and face parameters """
self.name = name
- self.gaze_speed = param["gaze_speed"]
+ self.face = face
+ if not self.face.connected:
+ self.face.setup_ros()
+ self.gaze_speed = param_eyes["gaze_speed"]
+ self.timer_interval = param_eyes["timer_interval"]
self.service_id = hf.get_child_id(self.name)
- """ Setup the face """
- self.setup_face()
- """ Setup the publisher for the face """
- self.face_pub = rospy.Publisher(
- ActuatorNameSpace.face.value + self.service_id + "/expressing",
+ self.face_eyes_pub = rospy.Publisher(
+ ActuatorNameSpace.face.value + self.service_id + "/expressing/eyes",
FaceRequest,
queue_size=1,
)
- """Setup the face service as server """
self.state = State.INIT
return
def do(self, data):
- """ Do the expression"""
+ """Do expression of eyes in web face
+ Args:
+ data ([str]): stringified json from tts results
+ Returns:
+ response (int): whether SUCCESS of FAIL
+ message (str): result message
+ """
self.actuation_completed = False
- [valid_face_expression, visemes] = self.get_face_data(data)
+ [gaze_bool, gaze_data] = self._get_gaze_data(data)
try:
self.state = State.REQUEST
-
- if visemes != []:
- viseme_ids = list(map(lambda b: b["id"], visemes))
- viseme_times = list(map(lambda b: b["start"], visemes))
- self.face_request = FaceRequest(
- visemes=viseme_ids, viseme_ms=self.speed_viseme, times=viseme_times
- )
- t = Timer(self.timer_interval, self.send_face_request)
- t.start()
- start_time = rospy.Time.now()
- rospy.loginfo("The last viseme lasts %i" % viseme_times[-1])
- time_sleep = int(viseme_times[-1]) + self.min_duration_viseme
- rospy.sleep(time_sleep)
- if valid_face_expression != []:
- rospy.loginfo("Valid face expression not null")
- if len(valid_face_expression) > 1:
- for ind, f in range(0, len(valid_face_expression) - 1):
- rospy.loginfo("The valid expression is %s" % f)
- aus = list(map(lambda s: s[2:], f["aus"]))
- au_ms = f["au_ms"] * 1000
- self.face_request = FaceRequest(
- aus=aus, au_degrees=f["au_degrees"], au_ms=au_ms
- )
- t = Timer(self.timer_interval, self.send_face_request)
- t.start()
- start_time = rospy.Time.now()
- aus = list(map(lambda s: s[2:], valid_face_expression[-1]["aus"]))
- au_ms = valid_face_expression[-1]["au_ms"] * 1000
- self.face_request = FaceRequest(
- aus=aus,
- au_degrees=valid_face_expression[-1]["au_degrees"],
- au_ms=au_ms,
- )
- t = Timer(self.timer_interval, self.send_face_request)
+ # The gaze eyes are played first
+ if gaze_bool:
+ rospy.loginfo("Valid gaze")
+ rospy.loginfo(f"The valid gaze is {gaze_data}")
+ self.face_request_eyes = FaceRequest(
+ retarget_gaze=gaze_bool, gaze_target=gaze_data, hold_gaze=2
+ )
+ t = Timer(self.timer_interval, self.send_face_eyes_request)
t.start()
start_time = rospy.Time.now()
- rospy.loginfo("The last facial expression")
- rospy.sleep(valid_face_expression[-1]["au_ms"])
+ face_bool = self.face.face_sequential_request(data)
+ rospy.loginfo("Completed Expression")
self.state = State.SUCCESS
self.actuation_completed = True
self.result_msg=""
@@ -92,132 +135,69 @@ def do(self, data):
self.state = State.FAILED
self.actuation_completed = True
self.result_msg=""
- rospy.loginfo("Completed Expression")
return {"response": self.state, "message": self.result_msg}
- def setup_face(self):
- """ Setup the face """
- rospy.loginfo("Setting up the %s eyes" % self.name)
- rospy.loginfo("Checking that face is connected to ROS websocket")
- #rospy.wait_for_service("/harmoni/actuating/face/is_connected")
- rospy.loginfo("Done, face is connected to ROS websocket")
- [
- self.face_expression,
- self.face_expression_names,
- ] = self.get_facial_expressions_list()
- self.visemes = [
- "BILABIAL",
- "LABIODENTAL",
- "INTERDENTAL",
- "DENTAL_ALVEOLAR",
- "POSTALVEOLAR",
- "VELAR_GLOTTAL",
- "CLOSE_FRONT_VOWEL",
- "OPEN_FRONT_VOWEL",
- "MID_CENTRAL_VOWEL",
- "OPEN_BACK_VOWEL",
- "CLOSE_BACK_VOWEL",
- "IDLE",
- ]
- return
-
- def send_face_request(self):
+ def send_face_eyes_request(self):
""" Send the request to the web page"""
- rospy.loginfo("Sending request to webpage of the face")
- self.face_pub.publish(self.face_request)
+ rospy.loginfo("Sending request to webpage of the face eyes")
+ self.face_eyes_pub.publish(self.face_request_eyes)
return
- def get_facial_expressions_list(self):
- """ Get facial expression list from the resource file"""
- facial_expression_list = []
- face_expression_au = {}
- base_dir = os.path.dirname(__file__)
- with open(
- base_dir + "/resource/cordial_face_expression.json", "r"
- ) as json_file:
- data = json.load(json_file)
- for facial_expression in data:
- facial_expression_list.append(facial_expression)
- au_name = str(facial_expression)
- aus = []
- for dofs in data[facial_expression]["dofs"]:
- aus.append(str(dofs))
- for keyframe in data[facial_expression]["keyframes"]:
- au_degrees = keyframe["pose"]
- au_ms = keyframe["time"]
- face_expression_au[au_name] = {
- "aus": aus,
- "au_degrees": au_degrees,
- "au_ms": au_ms,
- }
- return face_expression_au, facial_expression_list
-
- def get_face_data(self, data):
- """ Get the validated data of the face"""
- # rospy.loginfo("The face expressions available are %s" % self.face_expression)
-
+ def _get_gaze_data(self, data):
+ """Get target gaze data"""
data = ast.literal_eval(data)
if "behavior_data" in data:
behavior_data = ast.literal_eval(data["behavior_data"])
else:
behavior_data = data
- viseme_set = []
- facial_expression = []
- sentence = []
+ gaze_bool = False
+ gaze_target = Point()
for b in behavior_data:
if "id" in b.keys():
- if b["id"] in self.visemes:
- viseme_set.append(b)
- if b["id"] in self.face_expression_names:
- facial_expression.append(b)
- if "character" in b.keys():
- sentence.append(b["value"])
- # viseme = list(filter(lambda b: b["id"] in self.visemes, data))
- # facial_expression = list(filter(lambda b: b["id"] in self.face_expression_names, data))
- rospy.loginfo("These facial expressions include %s" % facial_expression)
- ordered_facial_data = list(
- sorted(facial_expression, key=lambda face: face["start"])
- )
- validated_face_expr = []
- for fexp in ordered_facial_data:
- validated_face_expr.append(self.face_expression[fexp["id"]])
- viseme_set = []
- rospy.loginfo("The validated facial expressions are %s" % validated_face_expr)
- rospy.loginfo("The validated visemes are %s" % viseme_set)
- print("Finished getting face data for sentence:", sentence)
- return (validated_face_expr, viseme_set)
-
-
+ if b["id"] == "target":
+ gaze_target.x = b["point"][0]
+ gaze_target.y = b["point"][1]
+ gaze_target.z = b["point"][2]
+ gaze_bool = True
+ return gaze_bool, gaze_target
+
+
class MouthService(HarmoniServiceManager):
"""
Mouth service
"""
- def __init__(self, name, param):
+ def __init__(self, name, param_mouth, face):
+ """Init of the Mouth service and setup as Harmoni service
+ Args:
+ name (str): service name
+ param_mouth (obj): list of parameters useful for the service
+ min_duration_viseme (int): minimum amount of time a viseme can last (seconds)
+ speed_viseme (int): velocity of the viseme execution (milliseconds)
+ timer_interval (int): interval of time between two consecutive visemes (seconds)
+ """
super().__init__(name)
- """ Initialization of variables and face parameters """
rospy.loginfo("MouthService initializing")
self.name = name
- self.min_duration_viseme = param["min_duration_viseme"]
- self.speed_viseme = param["speed_viseme"]
- self.timer_interval = param["timer_interval"]
+ self.face = face
+ if not self.face.connected:
+ self.face.setup_ros()
+ self.min_duration_viseme = param_mouth["min_duration_viseme"]
+ self.speed_viseme = param_mouth["speed_viseme"]
+ self.timer_interval = param_mouth["timer_interval"]
self.service_id = hf.get_child_id(self.name)
- self.setup_face()
- self.face_pub = rospy.Publisher(
- ActuatorNameSpace.face.value + self.service_id + "/expressing",
+ self.face_mouth_pub = rospy.Publisher(
+ ActuatorNameSpace.face.value + self.service_id + "/expressing/mouth",
FaceRequest,
queue_size=1,
)
- """Setup the face service as server """
self.state = State.INIT
return
def do(self, data):
"""Do expression in web face
-
Args:
data ([str]): stringified json from tts results
-
Returns:
response (int): whether SUCCESS of FAIL
message (str): result message
@@ -225,185 +205,101 @@ def do(self, data):
rospy.loginfo("Do expressions")
self.actuation_completed = False
self.result_msg=""
- [valid_face_expression, visemes] = self.get_face_data(data)
+ [viseme_bool, visemes] = self._get_viseme_data(data)
try:
self.state = State.REQUEST
-
- if visemes != []:
+ # The visemes are played at first
+ if viseme_bool:
viseme_ids = list(map(lambda b: b["id"], visemes))
viseme_times = list(map(lambda b: b["start"], visemes))
- self.face_request = FaceRequest(
+ self.face_mouth_request = FaceRequest(
visemes=viseme_ids, viseme_ms=self.speed_viseme, times=viseme_times
)
-
- t = Timer(self.timer_interval, self.send_face_request)
+ t = Timer(self.timer_interval, self.send_face_mouth_request)
t.start()
start_time = rospy.Time.now()
rospy.loginfo("The last viseme lasts %i" % viseme_times[-1])
time_sleep = int(viseme_times[-1]) + self.min_duration_viseme
rospy.sleep(time_sleep)
- if valid_face_expression != []:
- rospy.loginfo("Valid face expression not null")
- if len(valid_face_expression) > 1:
- for ind, f in range(0, len(valid_face_expression) - 1):
- rospy.loginfo("The valid expression is %s" % f)
- aus = list(map(lambda s: s[2:], f["aus"]))
- au_ms = f["au_ms"] * 1000
- self.face_request = FaceRequest(
- aus=aus, au_degrees=f["au_degrees"], au_ms=au_ms
- )
- t = Timer(self.timer_interval, self.send_face_request)
- t.start()
- start_time = rospy.Time.now()
- aus = list(map(lambda s: s[2:], valid_face_expression[-1]["aus"]))
- au_ms = valid_face_expression[-1]["au_ms"] * 1000
- self.face_request = FaceRequest(
- aus=aus,
- au_degrees=valid_face_expression[-1]["au_degrees"],
- au_ms=au_ms,
- )
- t = Timer(self.timer_interval, self.send_face_request)
- t.start()
- start_time = rospy.Time.now()
- rospy.loginfo("The last facial expression")
- rospy.sleep(valid_face_expression[-1]["au_ms"])
+ face_bool = self.face.face_sequential_request(data)
self.state = State.SUCCESS
self.actuation_completed = True
+ rospy.loginfo("Completed Expression")
except Exception:
self.state = State.FAILED
self.actuation_completed = True
- rospy.loginfo("Completed Expression")
return {"response": self.state, "message": self.result_msg}
- def setup_face(self):
- """Setup the face, waiting for the connection with the web page
- """
- rospy.loginfo("Setting up the %s mouth" % self.name)
- rospy.loginfo("Checking that face is connected to ROS websocket")
- rospy.wait_for_service("/harmoni/actuating/face/is_connected")
- rospy.loginfo("Done, face is connected to ROS websocket")
- [
- self.face_expression,
- self.face_expression_names,
- ] = self.get_facial_expressions_list()
- self.visemes = [
- "BILABIAL",
- "LABIODENTAL",
- "INTERDENTAL",
- "DENTAL_ALVEOLAR",
- "POSTALVEOLAR",
- "VELAR_GLOTTAL",
- "CLOSE_FRONT_VOWEL",
- "OPEN_FRONT_VOWEL",
- "MID_CENTRAL_VOWEL",
- "OPEN_BACK_VOWEL",
- "CLOSE_BACK_VOWEL",
- "IDLE",
- ]
- return
- def send_face_request(self):
+ def send_face_mouth_request(self):
""" Send the request to the web page"""
rospy.loginfo("Sending request to webpage of the face")
- self.face_pub.publish(self.face_request)
+ self.face_mouth_pub.publish(self.face_mouth_request)
return
- def get_facial_expressions_list(self):
- """ Get facial expression list from the resource file"""
- facial_expression_list = []
- face_expression_au = {}
- base_dir = os.path.dirname(__file__)
- with open(
- base_dir + "/resource/cordial_face_expression.json", "r"
- ) as json_file:
- data = json.load(json_file)
- for facial_expression in data:
- facial_expression_list.append(facial_expression)
- au_name = str(facial_expression)
- aus = []
- for dofs in data[facial_expression]["dofs"]:
- aus.append(str(dofs))
- for keyframe in data[facial_expression]["keyframes"]:
- au_degrees = keyframe["pose"]
- au_ms = keyframe["time"]
- face_expression_au[au_name] = {
- "aus": aus,
- "au_degrees": au_degrees,
- "au_ms": au_ms,
- }
- return face_expression_au, facial_expression_list
-
- def get_face_data(self, data):
+ def _get_viseme_data(self, data):
""" Get the validated data of the face"""
- # rospy.loginfo("The face expressions available are %s" % self.face_expression)
-
+ viseme_bool = False
data = ast.literal_eval(data)
if "behavior_data" in data:
behavior_data = ast.literal_eval(data["behavior_data"])
else:
behavior_data = data
viseme_set = []
- facial_expression = []
- sentence = []
for b in behavior_data:
if "id" in b.keys():
- if b["id"] in self.visemes:
+ if b["id"] in self.face.visemes:
viseme_set.append(b)
- if b["id"] in self.face_expression_names:
- facial_expression.append(b)
- if "character" in b.keys():
- sentence.append(b["value"])
- # viseme = list(filter(lambda b: b["id"] in self.visemes, data))
-
- # facial_expression = list(filter(lambda b: b["id"] in self.face_expression_names, data))
- rospy.loginfo("These facial expressions include %s" % facial_expression)
-
- ordered_facial_data = list(
- sorted(facial_expression, key=lambda face: face["start"])
- )
-
- validated_face_expr = []
- for fexp in ordered_facial_data:
- validated_face_expr.append(self.face_expression[fexp["id"]])
+ viseme_bool = True
+ if viseme_bool:
+ for i in range(0, len(viseme_set) - 1):
+ viseme_set[i]["duration"] = (
+ viseme_set[i + 1]["start"] - viseme_set[i]["start"]
+ )
- for i in range(0, len(viseme_set) - 1):
- viseme_set[i]["duration"] = (
- viseme_set[i + 1]["start"] - viseme_set[i]["start"]
+ viseme_set[-1]["duration"] = self.min_duration_viseme
+ viseme_behaviors = list(
+ filter(lambda b: b["duration"] >= self.min_duration_viseme, viseme_set)
)
-
- viseme_set[-1]["duration"] = self.min_duration_viseme
-
- viseme_behaviors = list(
- filter(lambda b: b["duration"] >= self.min_duration_viseme, viseme_set)
- )
- ordered_visemes = list(sorted(viseme_behaviors, key=lambda b: b["start"]))
- rospy.loginfo("The validated facial expressions are %s" % validated_face_expr)
- rospy.loginfo("The validated visemes are %s" % viseme_set)
- print("Finished getting face data for sentence:", sentence)
- return (validated_face_expr, viseme_set)
-
-
+ ordered_visemes = list(sorted(viseme_behaviors, key=lambda b: b["start"]))
+ rospy.loginfo("The validated visemes are %s" % viseme_set)
+ return viseme_bool, viseme_set
def main():
"""Set names, collect params, and give service to server"""
service_name = ActuatorNameSpace.face.name
- instance_id = rospy.get_param("/instance_id")
+ instance_id = rospy.get_param("/instance_id") #default
service_id_mouth = f"{service_name}_mouth_{instance_id}"
service_id_eyes = f"{service_name}_eyes_{instance_id}"
+ service_id_nose = f"{service_name}_nose_{instance_id}"
try:
rospy.init_node(service_name)
+ param = rospy.get_param(service_name + "/" + instance_id + "_param")
param_eyes = rospy.get_param(service_name + "/" + instance_id + "_param/eyes/")
param_mouth = rospy.get_param(service_name + "/" + instance_id + "_param/mouth/")
- s_eyes = EyesService(service_name + "_eyes_" + instance_id, param_eyes)
- s_mouth = MouthService(service_name + "_mouth_" + instance_id, param_mouth)
+ param_nose = rospy.get_param(service_name + "/" + instance_id + "_param/nose/")
+ face = Face(ActuatorNameSpace.face.value, instance_id, param)
+ s_eyes = EyesService(service_name + "_eyes_" + instance_id, param_eyes, face)
+ s_mouth = MouthService(service_name + "_mouth_" + instance_id, param_mouth, face)
+ s_nose = NoseService(service_name + "_nose_" + instance_id, param_nose, face)
service_server_eyes = HarmoniServiceServer(service_id_eyes, s_eyes)
service_server_mouth = HarmoniServiceServer(service_id_mouth, s_mouth)
+ service_server_nose = HarmoniServiceServer(service_id_nose, s_nose)
+
+ print(service_name)
+ print("****************************************************************************")
+ print(service_id_eyes)
+ print(service_id_mouth)
+ print(service_id_nose)
+
service_server_eyes.start_sending_feedback()
service_server_mouth.start_sending_feedback()
+ service_server_nose.start_sending_feedback()
+
rospy.spin()
except rospy.ROSInterruptException:
pass
if __name__ == "__main__":
- main()
+ main()
\ No newline at end of file
diff --git a/harmoni_actuators/harmoni_face/src/harmoni_face/misty_caller.py b/harmoni_actuators/harmoni_face/src/harmoni_face/misty_caller.py
new file mode 100755
index 00000000..b92b3440
--- /dev/null
+++ b/harmoni_actuators/harmoni_face/src/harmoni_face/misty_caller.py
@@ -0,0 +1,43 @@
+#!/usr/bin/env python3
+
+import rospy
+import requests
+from requests.exceptions import Timeout
+import socket
+import sys
+from subprocess import check_output
+
+
+def is_int(value):
+ try:
+ int(value)
+ return True
+ except ValueError:
+ return False
+
+#DIR_IDX = 1
+PORT_IDX = 2
+
+if __name__ == "__main__":
+ rospy.init_node("misty_caller")
+ robot_ip = rospy.get_param("/robot_ip")
+
+ local_ip = str(check_output(['hostname', '--all-ip-addresses']).split()[0])[2:-1]
+
+ args = sys.argv
+ if is_int(args[PORT_IDX]):
+ port = args[PORT_IDX]
+
+ payload = {'URL': 'http://' + local_ip + ':' + port ,
+ 'layer': 'DefaultWebViewLayer1'}
+ print(payload['URL'])
+
+ try:
+ response = requests.post('http://{}/api/webviews/display'.format(robot_ip),
+ params = payload,
+ timeout = 4)
+ except Timeout:
+ rospy.logwarn("web_player failed: The ip of the robot appears unreachable")
+ while not rospy.is_shutdown():
+ rospy.sleep(1)
+ #rospy.spin()
\ No newline at end of file
diff --git a/harmoni_actuators/harmoni_face/src/harmoni_face/misty_face_service_.py b/harmoni_actuators/harmoni_face/src/harmoni_face/misty_face_service_.py
new file mode 100755
index 00000000..04d63846
--- /dev/null
+++ b/harmoni_actuators/harmoni_face/src/harmoni_face/misty_face_service_.py
@@ -0,0 +1,408 @@
+#!/usr/bin/env python3
+
+# Common Imports
+import rospy
+import roslib
+
+from harmoni_common_lib.constants import State
+from harmoni_common_lib.service_server import HarmoniServiceServer
+from harmoni_common_lib.service_manager import HarmoniServiceManager
+import harmoni_common_lib.helper_functions as hf
+
+# Specific Imports
+from harmoni_common_lib.constants import ActuatorNameSpace
+from harmoni_face.msg import FaceRequest
+from threading import Timer
+import json
+import ast
+import os
+
+
+class EyesService(HarmoniServiceManager):
+ """
+ TODO: Eyes service
+ """
+
+ def __init__(self, name, param):
+ super().__init__(name)
+ """ Initialization of variables and face parameters """
+ self.name = name
+ self.gaze_speed = param["gaze_speed"]
+ self.service_id = hf.get_child_id(self.name)
+ """ Setup the face """
+ self.setup_face()
+ """ Setup the publisher for the face """
+ self.face_pub = rospy.Publisher(
+ ActuatorNameSpace.face.value + self.service_id + "/expressing",
+ FaceRequest,
+ queue_size=1,
+ )
+ """Setup the face service as server """
+ self.state = State.INIT
+ return
+
+ def do(self, data):
+ """ Do the expression"""
+ self.actuation_completed = False
+ [valid_face_expression, visemes] = self.get_face_data(data)
+ try:
+ self.state = State.REQUEST
+
+ if visemes != []:
+ viseme_ids = list(map(lambda b: b["id"], visemes))
+ viseme_times = list(map(lambda b: b["start"], visemes))
+ self.face_request = FaceRequest(
+ visemes=viseme_ids, viseme_ms=self.speed_viseme, times=viseme_times
+ )
+ t = Timer(self.timer_interval, self.send_face_request)
+ t.start()
+ start_time = rospy.Time.now()
+ rospy.loginfo("The last viseme lasts %i" % viseme_times[-1])
+ time_sleep = int(viseme_times[-1]) + self.min_duration_viseme
+ rospy.sleep(time_sleep)
+ if valid_face_expression != []:
+ rospy.loginfo("Valid face expression not null")
+ if len(valid_face_expression) > 1:
+ for ind, f in range(0, len(valid_face_expression) - 1):
+ rospy.loginfo("The valid expression is %s" % f)
+ aus = list(map(lambda s: s[2:], f["aus"]))
+ au_ms = f["au_ms"] * 1000
+ self.face_request = FaceRequest(
+ aus=aus, au_degrees=f["au_degrees"], au_ms=au_ms
+ )
+ t = Timer(self.timer_interval, self.send_face_request)
+ t.start()
+ start_time = rospy.Time.now()
+ aus = list(map(lambda s: s[2:], valid_face_expression[-1]["aus"]))
+ au_ms = valid_face_expression[-1]["au_ms"] * 1000
+ self.face_request = FaceRequest(
+ aus=aus,
+ au_degrees=valid_face_expression[-1]["au_degrees"],
+ au_ms=au_ms,
+ )
+ t = Timer(self.timer_interval, self.send_face_request)
+ t.start()
+ start_time = rospy.Time.now()
+ rospy.loginfo("The last facial expression")
+ rospy.sleep(valid_face_expression[-1]["au_ms"])
+ self.state = State.SUCCESS
+ self.actuation_completed = True
+ self.result_msg=""
+ except Exception:
+ self.state = State.FAILED
+ self.actuation_completed = True
+ self.result_msg=""
+ rospy.loginfo("Completed Expression")
+ return {"response": self.state, "message": self.result_msg}
+
+ def setup_face(self):
+ """ Setup the face """
+ rospy.loginfo("Setting up the %s eyes" % self.name)
+ rospy.loginfo("Checking that face is connected to ROS websocket")
+ #rospy.wait_for_service("/harmoni/actuating/face/is_connected")
+ rospy.loginfo("Done, face is connected to ROS websocket")
+ [
+ self.face_expression,
+ self.face_expression_names,
+ ] = self.get_facial_expressions_list()
+ self.visemes = [
+ "BILABIAL",
+ "LABIODENTAL",
+ "INTERDENTAL",
+ "DENTAL_ALVEOLAR",
+ "POSTALVEOLAR",
+ "VELAR_GLOTTAL",
+ "CLOSE_FRONT_VOWEL",
+ "OPEN_FRONT_VOWEL",
+ "MID_CENTRAL_VOWEL",
+ "OPEN_BACK_VOWEL",
+ "CLOSE_BACK_VOWEL",
+ "IDLE",
+ ]
+ return
+
+ def send_face_request(self):
+ """ Send the request to the web page"""
+ rospy.loginfo("Sending request to webpage of the face")
+ self.face_pub.publish(self.face_request)
+ return
+
+ def get_facial_expressions_list(self):
+ """ Get facial expression list from the resource file"""
+ facial_expression_list = []
+ face_expression_au = {}
+ base_dir = os.path.dirname(__file__)
+ with open(
+ base_dir + "/resource/cordial_face_expression.json", "r"
+ ) as json_file:
+ data = json.load(json_file)
+ for facial_expression in data:
+ facial_expression_list.append(facial_expression)
+ au_name = str(facial_expression)
+ aus = []
+ for dofs in data[facial_expression]["dofs"]:
+ aus.append(str(dofs))
+ for keyframe in data[facial_expression]["keyframes"]:
+ au_degrees = keyframe["pose"]
+ au_ms = keyframe["time"]
+ face_expression_au[au_name] = {
+ "aus": aus,
+ "au_degrees": au_degrees,
+ "au_ms": au_ms,
+ }
+ return face_expression_au, facial_expression_list
+
+ def get_face_data(self, data):
+ """ Get the validated data of the face"""
+ # rospy.loginfo("The face expressions available are %s" % self.face_expression)
+
+ data = ast.literal_eval(data)
+ if "behavior_data" in data:
+ behavior_data = ast.literal_eval(data["behavior_data"])
+ else:
+ behavior_data = data
+ viseme_set = []
+ facial_expression = []
+ sentence = []
+ for b in behavior_data:
+ if "id" in b.keys():
+ if b["id"] in self.visemes:
+ viseme_set.append(b)
+ if b["id"] in self.face_expression_names:
+ facial_expression.append(b)
+ if "character" in b.keys():
+ sentence.append(b["value"])
+ # viseme = list(filter(lambda b: b["id"] in self.visemes, data))
+ # facial_expression = list(filter(lambda b: b["id"] in self.face_expression_names, data))
+ rospy.loginfo("These facial expressions include %s" % facial_expression)
+ ordered_facial_data = list(
+ sorted(facial_expression, key=lambda face: face["start"])
+ )
+ validated_face_expr = []
+ for fexp in ordered_facial_data:
+ validated_face_expr.append(self.face_expression[fexp["id"]])
+ viseme_set = []
+ rospy.loginfo("The validated facial expressions are %s" % validated_face_expr)
+ rospy.loginfo("The validated visemes are %s" % viseme_set)
+ print("Finished getting face data for sentence:", sentence)
+ return (validated_face_expr, viseme_set)
+
+
+class MouthService(HarmoniServiceManager):
+ """
+ Mouth service
+ """
+
+ def __init__(self, name, param):
+ super().__init__(name)
+ """ Initialization of variables and face parameters """
+ rospy.loginfo("MouthService initializing")
+ self.name = name
+ self.min_duration_viseme = param["min_duration_viseme"]
+ self.speed_viseme = param["speed_viseme"]
+ self.timer_interval = param["timer_interval"]
+ self.service_id = hf.get_child_id(self.name)
+ self.setup_face()
+ self.face_pub = rospy.Publisher(
+ ActuatorNameSpace.face.value + self.service_id + "/expressing",
+ FaceRequest,
+ queue_size=1,
+ )
+ """Setup the face service as server """
+ self.state = State.INIT
+ return
+
+ def do(self, data):
+ """Do expression in web face
+
+ Args:
+ data ([str]): stringified json from tts results
+
+ Returns:
+ response (int): whether SUCCESS of FAIL
+ message (str): result message
+ """
+ rospy.loginfo("Do expressions")
+ self.actuation_completed = False
+ self.result_msg=""
+ [valid_face_expression, visemes] = self.get_face_data(data)
+ try:
+ self.state = State.REQUEST
+
+ if visemes != []:
+ viseme_ids = list(map(lambda b: b["id"], visemes))
+ viseme_times = list(map(lambda b: b["start"], visemes))
+ self.face_request = FaceRequest(
+ visemes=viseme_ids, viseme_ms=self.speed_viseme, times=viseme_times
+ )
+
+ t = Timer(self.timer_interval, self.send_face_request)
+ t.start()
+ start_time = rospy.Time.now()
+ rospy.loginfo("The last viseme lasts %i" % viseme_times[-1])
+ time_sleep = int(viseme_times[-1]) + self.min_duration_viseme
+ rospy.sleep(time_sleep)
+ if valid_face_expression != []:
+ rospy.loginfo("Valid face expression not null")
+ if len(valid_face_expression) > 1:
+ for ind, f in range(0, len(valid_face_expression) - 1):
+ rospy.loginfo("The valid expression is %s" % f)
+ aus = list(map(lambda s: s[2:], f["aus"]))
+ au_ms = f["au_ms"] * 1000
+ self.face_request = FaceRequest(
+ aus=aus, au_degrees=f["au_degrees"], au_ms=au_ms
+ )
+ t = Timer(self.timer_interval, self.send_face_request)
+ t.start()
+ start_time = rospy.Time.now()
+ aus = list(map(lambda s: s[2:], valid_face_expression[-1]["aus"]))
+ au_ms = valid_face_expression[-1]["au_ms"] * 1000
+ self.face_request = FaceRequest(
+ aus=aus,
+ au_degrees=valid_face_expression[-1]["au_degrees"],
+ au_ms=au_ms,
+ )
+ t = Timer(self.timer_interval, self.send_face_request)
+ t.start()
+ start_time = rospy.Time.now()
+ rospy.loginfo("The last facial expression")
+ rospy.sleep(valid_face_expression[-1]["au_ms"])
+ self.state = State.SUCCESS
+ self.actuation_completed = True
+ except Exception:
+ self.state = State.FAILED
+ self.actuation_completed = True
+ rospy.loginfo("Completed Expression")
+ return {"response": self.state, "message": self.result_msg}
+
+ def setup_face(self):
+ """Setup the face, waiting for the connection with the web page
+ """
+ rospy.loginfo("Setting up the %s mouth" % self.name)
+ rospy.loginfo("Checking that face is connected to ROS websocket")
+ rospy.wait_for_service("/harmoni/actuating/face/is_connected")
+ rospy.loginfo("Done, face is connected to ROS websocket")
+ [
+ self.face_expression,
+ self.face_expression_names,
+ ] = self.get_facial_expressions_list()
+ self.visemes = [
+ "BILABIAL",
+ "LABIODENTAL",
+ "INTERDENTAL",
+ "DENTAL_ALVEOLAR",
+ "POSTALVEOLAR",
+ "VELAR_GLOTTAL",
+ "CLOSE_FRONT_VOWEL",
+ "OPEN_FRONT_VOWEL",
+ "MID_CENTRAL_VOWEL",
+ "OPEN_BACK_VOWEL",
+ "CLOSE_BACK_VOWEL",
+ "IDLE",
+ ]
+ return
+
+ def send_face_request(self):
+ """ Send the request to the web page"""
+ rospy.loginfo("Sending request to webpage of the face")
+ self.face_pub.publish(self.face_request)
+ return
+
+ def get_facial_expressions_list(self):
+ """ Get facial expression list from the resource file"""
+ facial_expression_list = []
+ face_expression_au = {}
+ base_dir = os.path.dirname(__file__)
+ with open(
+ base_dir + "/resource/cordial_face_expression.json", "r"
+ ) as json_file:
+ data = json.load(json_file)
+ for facial_expression in data:
+ facial_expression_list.append(facial_expression)
+ au_name = str(facial_expression)
+ aus = []
+ for dofs in data[facial_expression]["dofs"]:
+ aus.append(str(dofs))
+ for keyframe in data[facial_expression]["keyframes"]:
+ au_degrees = keyframe["pose"]
+ au_ms = keyframe["time"]
+ face_expression_au[au_name] = {
+ "aus": aus,
+ "au_degrees": au_degrees,
+ "au_ms": au_ms,
+ }
+ return face_expression_au, facial_expression_list
+
+ def get_face_data(self, data):
+ """ Get the validated data of the face"""
+ # rospy.loginfo("The face expressions available are %s" % self.face_expression)
+ data = ast.literal_eval(data)
+ if "behavior_data" in data:
+ behavior_data = ast.literal_eval(data["behavior_data"])
+ else:
+ behavior_data = data
+ viseme_set = []
+ facial_expression = []
+ sentence = []
+ for b in behavior_data:
+ if "id" in b.keys():
+ if b["id"] in self.visemes:
+ viseme_set.append(b)
+ if b["id"] in self.face_expression_names:
+ facial_expression.append(b)
+ if "character" in b.keys():
+ sentence.append(b["value"])
+ # viseme = list(filter(lambda b: b["id"] in self.visemes, data))
+
+ # facial_expression = list(filter(lambda b: b["id"] in self.face_expression_names, data))
+ rospy.loginfo("These facial expressions include %s" % facial_expression)
+
+ ordered_facial_data = list(
+ sorted(facial_expression, key=lambda face: face["start"])
+ )
+
+ validated_face_expr = []
+ for fexp in ordered_facial_data:
+ validated_face_expr.append(self.face_expression[fexp["id"]])
+
+ for i in range(0, len(viseme_set) - 1):
+ viseme_set[i]["duration"] = (
+ viseme_set[i + 1]["start"] - viseme_set[i]["start"]
+ )
+
+ viseme_set[-1]["duration"] = self.min_duration_viseme
+
+ viseme_behaviors = list(
+ filter(lambda b: b["duration"] >= self.min_duration_viseme, viseme_set)
+ )
+ ordered_visemes = list(sorted(viseme_behaviors, key=lambda b: b["start"]))
+ rospy.loginfo("The validated facial expressions are %s" % validated_face_expr)
+ rospy.loginfo("The validated visemes are %s" % viseme_set)
+ print("Finished getting face data for sentence:", sentence)
+ return (validated_face_expr, viseme_set)
+
+
+
+def main():
+ """Set names, collect params, and give service to server"""
+ service_name = ActuatorNameSpace.face.name
+ instance_id = rospy.get_param("/instance_id")
+ service_id_mouth = f"{service_name}_mouth_{instance_id}"
+ service_id_eyes = f"{service_name}_eyes_{instance_id}"
+ try:
+ rospy.init_node(service_name)
+ param_eyes = rospy.get_param(service_name + "/" + instance_id + "_param/eyes/")
+ param_mouth = rospy.get_param(service_name + "/" + instance_id + "_param/mouth/")
+ s_eyes = EyesService(service_name + "_eyes_" + instance_id, param_eyes)
+ s_mouth = MouthService(service_name + "_mouth_" + instance_id, param_mouth)
+ service_server_eyes = HarmoniServiceServer(service_id_eyes, s_eyes)
+ service_server_mouth = HarmoniServiceServer(service_id_mouth, s_mouth)
+ service_server_eyes.start_sending_feedback()
+ service_server_mouth.start_sending_feedback()
+ rospy.spin()
+ except rospy.ROSInterruptException:
+ pass
+
+
+if __name__ == "__main__":
+ main()
diff --git a/harmoni_actuators/harmoni_face/test/face.test b/harmoni_actuators/harmoni_face/test/face.test
index db4905b1..700bb1e2 100644
--- a/harmoni_actuators/harmoni_face/test/face.test
+++ b/harmoni_actuators/harmoni_face/test/face.test
@@ -8,6 +8,6 @@
-
+
-
+
\ No newline at end of file
diff --git a/harmoni_actuators/harmoni_face/test/face_misty.test b/harmoni_actuators/harmoni_face/test/face_misty.test
new file mode 100644
index 00000000..c406d049
--- /dev/null
+++ b/harmoni_actuators/harmoni_face/test/face_misty.test
@@ -0,0 +1,15 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/harmoni_actuators/harmoni_face/web/index.html b/harmoni_actuators/harmoni_face/web/index.html
index 6b5b334b..73aee203 100755
--- a/harmoni_actuators/harmoni_face/web/index.html
+++ b/harmoni_actuators/harmoni_face/web/index.html
@@ -12,7 +12,7 @@
-
-
+
+