class HotwordDetector:
def __init__(self):
self.interrupted = False
self.model = rospkg.RosPack().get_path(
'sound_system') + "/etc/model/Hey_Ducker.pmdl"
self.detector = snowboydecoder.HotwordDetector(self.model,
sensitivity=0.5)
self.se = SE()
rospy.init_node('hotword', anonymous=False)
rospy.Service("/sound_system/hotword", HotwordService,
self.hotword_detect)
def hotword_detect(self, request):
"""
Hotwordの検出を行う
:param request: HotwordRequest型だが中身は何もなし
:return: HotwordResponse型だが中身は何もなし
"""
print("Hotword 待機...")
is_detect = self.detector.start()
self.detector.terminate()
if is_detect:
self.se.play(self.se.WAKEUP)
print("Hotword 検出")
return HotwordServiceResponse()
def __init__(self):
self.interrupted = False
self.model = rospkg.RosPack().get_path(
'sound_system') + "/etc/model/Hey_Ducker.pmdl"
self.detector = snowboydecoder.HotwordDetector(self.model,
sensitivity=0.5)
self.se = SE()
rospy.init_node('hotword', anonymous=False)
rospy.Service("/sound_system/hotword", HotwordService,
self.hotword_detect)
def __init__(self):
rospy.init_node("sphinx")
# launchのパラメータ読み取り
self.dict = rospy.get_param("/{}/dict".format(rospy.get_name()))
self.gram = rospy.get_param("/{}/gram".format(rospy.get_name()))
# 音響モデルのディレクトリの絶対パス
self.model_path = "/usr/local/lib/python2.7/dist-packages/pocketsphinx/model"
self.dictionary_path = rospkg.RosPack().get_path('sound_system') + "/etc/dictionary" # 辞書のディレクトリの絶対パス
self.speech = None
self.start = False
self.result = None
self.noise_words = []
self.se = SE()
rospy.Service("/sound_system/recognition", StringService, self.recognition)
rospy.Service("/sound_system/recognition_stop", StringService, self.recognition_stop)
self.log_heard_pub = rospy.Publisher("/sound_system/log/heard", String, queue_size=10)
self.result_pub = rospy.Publisher("/sound_system/result", String, queue_size=10)
self.pause()
def __init__(self):
self.cv_image = None
self.point_list = [
"nose", "leftEye", "rightEye", "leftEar", "rightEar",
"leftShoulder", "rightShoulder"
]
self.sensor_x = 0
self.sensor_y = 0
self.sensor_rad = 0
self.status = None
self.marker = RvizMarker()
self.sound_source_angle_list = []
self.se = SE()
self.failed = False
rospy.init_node("raise_hand_human")
rospy.Subscriber("/tfpose_ros/result",
Poses,
self.pose_callback,
queue_size=1)
rospy.Subscriber("/call_ducker/control",
String,
self.control_callback,
queue_size=1)
rospy.Subscriber("/odom", Odometry, self.odometry_callback)
rospy.Subscriber("/sound_direction", Int32, self.respeaker_callback)
self.move_base_client = actionlib.SimpleActionClient(
"/move_base", MoveBaseAction)
self.amount_client = actionlib.SimpleActionClient(
"/move/amount", AmountAction)
self.shutter_pub = rospy.Publisher("/tfpose_ros/shutter",
String,
queue_size=10)
self.finish_pub = rospy.Publisher("/call_ducker/finish",
Bool,
queue_size=10)
class CallDucker:
def __init__(self):
self.cv_image = None
self.point_list = [
"nose", "leftEye", "rightEye", "leftEar", "rightEar",
"leftShoulder", "rightShoulder"
]
self.sensor_x = 0
self.sensor_y = 0
self.sensor_rad = 0
self.status = None
self.marker = RvizMarker()
self.sound_source_angle_list = []
self.se = SE()
self.failed = False
rospy.init_node("raise_hand_human")
rospy.Subscriber("/tfpose_ros/result",
Poses,
self.pose_callback,
queue_size=1)
rospy.Subscriber("/call_ducker/control",
String,
self.control_callback,
queue_size=1)
rospy.Subscriber("/odom", Odometry, self.odometry_callback)
rospy.Subscriber("/sound_direction", Int32, self.respeaker_callback)
self.move_base_client = actionlib.SimpleActionClient(
"/move_base", MoveBaseAction)
self.amount_client = actionlib.SimpleActionClient(
"/move/amount", AmountAction)
self.shutter_pub = rospy.Publisher("/tfpose_ros/shutter",
String,
queue_size=10)
self.finish_pub = rospy.Publisher("/call_ducker/finish",
Bool,
queue_size=10)
@staticmethod
def to_angle(rad):
return rad * 180 / math.pi
@staticmethod
def to_radian(angle):
return (angle * math.pi) / 180
@staticmethod
def to_quaternion_rad(w, z):
return math.acos(w) * 2 * np.sign(z)
def send_move_base(self, point):
# type:(tuple)->int
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = "map"
goal.target_pose.pose.position = Point(point[0], point[1], point[2])
goal.target_pose.pose.orientation = Quaternion(0, 0, 0, 1)
self.marker.register(goal.target_pose.pose)
self.move_base_client.wait_for_server()
print "move_baseに送信"
self.move_base_client.send_goal(goal)
self.move_base_client.wait_for_result()
return self.move_base_client.get_state()
@staticmethod
def speak(sentence):
# type: (str) -> None
"""
発話関数
:param sentence:
:return:
"""
rospy.wait_for_service("/sound_system/speak")
rospy.ServiceProxy("/sound_system/speak", StringService)(sentence)
def move_turn(self, angle):
"""
角度送信
:param angle:
:return:
"""
goal = AmountGoal()
goal.amount.angle = angle
goal.velocity.angular_rate = 0.5
self.amount_client.wait_for_server()
self.amount_client.send_goal(goal)
self.amount_client.wait_for_result(rospy.Duration(10))
def turn_sound_source(self):
"""
音源定位した後の移動
:return:
"""
# 回転メッセージを投げる
angle = self.sound_source_angle_list[3] # 時間を遡る
if angle - 180 > 0:
angle = -(360 - angle)
self.move_turn(angle)
def calc_person_position(self, pose):
try:
# 人間の三次元座標を計算
count = 0
sum_x = 0
sum_y = 0
sum_z = 0
for key in pose.keypoints:
if key.part in self.point_list:
sum_x += key.position.x
sum_y += key.position.y
sum_z += key.position.z
count += 1
ave_x = sum_x / count
ave_y = sum_y / count
ave_z = sum_z / count
distance = math.sqrt(ave_x**2 + ave_y**2 + ave_z**2)
return distance, (ave_x, ave_y, ave_z)
except ZeroDivisionError:
print "countが0 @raise_hand"
return None
@staticmethod
def is_raise_hand(key_points):
# type: (dict)->bool
"""
腕を上げているかの判定
:return:
"""
body_part_dict = {}
for key in key_points:
body_part_dict.setdefault(
key.part, [key.position.x, key.position.y, key.position.z])
if "rightShoulder" in body_part_dict:
if "rightElbow" in body_part_dict:
if body_part_dict["rightShoulder"][1] > body_part_dict[
"rightElbow"][1]:
return True
if "rightWrist" in body_part_dict:
if body_part_dict["rightShoulder"][1] > body_part_dict[
"rightWrist"][1]:
return True
if "leftShoulder" in body_part_dict:
if "leftElbow" in body_part_dict:
if body_part_dict["leftShoulder"][1] > body_part_dict[
"leftElbow"][1]:
return True
if "leftWrist" in body_part_dict:
if body_part_dict["leftShoulder"][1] > body_part_dict[
"leftWrist"][1]:
return True
return False
def calc_real_position(self, point):
# type:(tuple)->tuple
relative_theta = self.sensor_rad
relative_x = point[2]
relative_y = -point[0]
x = (relative_x * math.cos(relative_theta) -
relative_y * math.sin(relative_theta)) + self.sensor_x
y = (relative_x * math.sin(relative_theta) +
relative_y * math.cos(relative_theta)) + self.sensor_y
print "real", x, y
return x, y
@staticmethod
def calc_safe_position(margin, person_position):
# type: (float,tuple)->tuple
"""
人間を中心にdistanceを半径とした円と、人間からロボットまで結んだ直線の交点を計算
:param margin:
:param person_position:
:return:
"""
real_person_x = person_position[0]
real_person_y = person_position[1]
"""
distance = math.sqrt((real_person_x - self.sensor_x) ** 2 + (real_person_y - self.sensor_y) ** 2)
# 三角形の相似を利用して算出
safety_person_x = real_person_x - (margin * abs(real_person_x - self.sensor_x)) / distance
safety_person_y = real_person_y - (margin * (real_person_y - self.sensor_y)) / distance
"""
safety_person_x = real_person_x - margin
safety_person_y = real_person_y
return safety_person_x, safety_person_y
################################################################################
def respeaker_callback(self, msg):
# type:(Int32)->None
"""
angle_listにスタック
:param msg:
:return:
"""
angle = msg.data
if len(self.sound_source_angle_list) > 10:
self.sound_source_angle_list.pop(0)
self.sound_source_angle_list.append(angle)
def control_callback(self, msg):
# type:(String)->None
if msg.data == "start":
while True:
self.status = None
self.failed = False
self.speak("Please raise your hand.")
time.sleep(3)
self.shutter_pub.publish(String(data="take"))
print "シャッター"
while self.status is None and not self.failed:
pass
if self.status is None:
continue
if self.status == actionlib.GoalStatus.SUCCEEDED:
self.finish_pub.publish(Bool(data=True))
return
self.finish_pub.publish(Bool(data=False))
def odometry_callback(self, msg):
# type: (Odometry)->None
"""
位置情報の受け取り
:param msg:
:return:
"""
self.sensor_x = msg.pose.pose.position.x
self.sensor_y = msg.pose.pose.position.y
self.sensor_rad = self.to_quaternion_rad(msg.pose.pose.orientation.w,
msg.pose.pose.orientation.z)
def pose_callback(self, msgs):
# type: (Poses)->None
"""
関節推定の結果を受け取り
:param msgs:
:return:
"""
person_position = {}
for pose in msgs.poses:
if not self.is_raise_hand(pose.keypoints):
continue
result = self.calc_person_position(pose)
if result is None:
continue
person_position.setdefault(result[0], result[1])
# 手を上げている一番近い人間
if len(person_position) == 0:
print "誰もいない"
self.speak("Sorry. I can't found you.")
self.failed = True
return
if min(person_position) < MARGIN:
print "人間が近すぎます"
self.failed = True
return
print "発見"
self.se.play(self.se.WARNING)
print "se"
real_position = self.calc_real_position(
person_position[min(person_position)])
result = self.calc_safe_position(MARGIN,
(real_position[0], real_position[1]))
self.status = self.send_move_base((result[0], result[1], 0))
if self.status == actionlib.GoalStatus.SUCCEEDED:
print "到着"
else:
print "失敗"
self.speak("Sorry. I can't found you.")
self.failed = True
def pause():
SE.play(SE.STOP)
def resume():
SE.play(SE.START)
class Sphinx:
def __init__(self):
rospy.init_node("sphinx")
# launchのパラメータ読み取り
self.dict = rospy.get_param("/{}/dict".format(rospy.get_name()))
self.gram = rospy.get_param("/{}/gram".format(rospy.get_name()))
# 音響モデルのディレクトリの絶対パス
self.model_path = "/usr/local/lib/python2.7/dist-packages/pocketsphinx/model"
self.dictionary_path = rospkg.RosPack().get_path('sound_system') + "/etc/dictionary" # 辞書のディレクトリの絶対パス
self.speech = None
self.start = False
self.result = None
self.noise_words = []
self.se = SE()
rospy.Service("/sound_system/recognition", StringService, self.recognition)
rospy.Service("/sound_system/recognition_stop", StringService, self.recognition_stop)
self.log_heard_pub = rospy.Publisher("/sound_system/log/heard", String, queue_size=10)
self.result_pub = rospy.Publisher("/sound_system/result", String, queue_size=10)
self.pause()
def read_noise_word(self):
words = []
with open(os.path.join(self.dictionary_path, self.gram)) as f:
for line in f.readlines():
if "<noise>" not in line:
continue
if "<rule>" in line:
continue
line = line.replace("<noise>", "").replace("=", "").replace(" ", "").replace("\n", "").replace(";", "")
words = line.split("|")
return words
def resume(self):
print("== START RECOGNITION ==")
self.speech = LiveSpeech(
verbose=False, sampling_rate=8000, buffer_size=2048, no_search=False, full_utt=False,
hmm=os.path.join(self.model_path, "en-us"),
lm=False,
dic=os.path.join(self.dictionary_path, self.dict),
jsgf=os.path.join(self.dictionary_path, self.gram)
)
# 音声認識ストップ ###########################################
def pause(self):
print("== STOP RECOGNITION ==")
self.speech = LiveSpeech(no_search=True)
self.start = False
# 音声認識を開始 #############################################
def recognition(self, message):
# type: (StringServiceRequest) -> StringServiceResponse
if message.request != '':
self.dict = message.request + '.dict'
self.gram = message.request + '.gram'
self.noise_words = self.read_noise_word()
print "dict: ", self.dict
print "gram: ", self.gram
self.start = True
return StringServiceResponse()
def recognition_stop(self, message):
self.pause()
return StringServiceResponse()
############################################################
def multi_thread(self):
"""
マルチスレッド
メインスレッドでしかResumeできないSphinxへの対策
self.start, self.resultでフラグ管理を行っている
self.startが処理の開始用フラグ
:return:
"""
while not rospy.core.is_shutdown():
# 音声認識の命令が来るまで待機
if not self.start:
continue
self.se.play(self.se.START)
self.resume()
for text in self.speech:
score = text.confidence()
print(str(text), score)
if score > 0.1 and str(text) not in self.noise_words:
self.se.play(self.se.STOP)
self.pause()
text = str(text)
self.result = text
self.log_heard_pub.publish(text)
self.result_pub.publish(self.result)
break
else:
print("**noise**")