class HmcBackToTheCar(AbstractModule):
def __init__(self):
super(HmcBackToTheCar, self).__init__(node_name="hmc_back_to_the_car")
self.move_base_client = actionlib.SimpleActionClient(
"/move_base", MoveBaseAction)
self.amount_client = actionlib.SimpleActionClient(
"/move/amount", AmountAction)
self.marker = RvizMarker()
rospy.Subscriber("/human_detection/finish", String,
self.back_to_the_car)
def back_to_the_car(self, argument):
# type: (String) -> None
"""
human_detection終了後,協力者を連れて車に戻る
:param: argument: 関数用の引数が格納されている.
本関数では空である.
:return: なし
"""
self.print_node(argument.data)
self.speak("I want you to help carrying groceries into the house.")
self.speak("Please follow me.")
rospy.wait_for_service('/location/request_location', timeout=1)
coordinate = rospy.ServiceProxy('/location/request_location',
RequestLocation)("car").location
self.send_move_base(coordinate)
self.speak("Here is the car")
def send_move_base(self, point):
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = "map"
goal.target_pose.pose.position = Point(point.x, point.y, point.z)
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()
class RestaurantFinal:
def __init__(self):
rospy.init_node("final", anonymous=True)
self.marker = RvizMarker()
self.move_base_client = actionlib.SimpleActionClient(
"/move_base", MoveBaseAction)
self.call_ducker_pub = rospy.Publisher("/call_ducker/control",
String,
queue_size=10)
self.active_flag = False
rospy.Subscriber("/trigger/start", String, self.start_callback)
rospy.Subscriber("/order_input", String, self.order_text_callback)
rospy.Subscriber("/call_ducker/finish", Bool,
self.call_ducker_callback)
def start_callback(self, data):
# type:(String) -> None
"""
トリガーのcallback関数
最初の発話
:param data: 空文字
:return:なし
"""
self.print_function("start_callback")
self.speak("Hello, everyone. I will start the demonstration now.")
time.sleep(0.5)
self.speak(
"If you want to order, please input the item from the tablet.")
def order_text_callback(self, data):
# type:(String) -> None
"""
Webから入力文のsubscribeするcallback関数
:param data: オーダーのテキスト
:return: なし
"""
if self.active_flag:
return
self.active_flag = True
self.print_function("order_text_callback")
rospy.wait_for_service('/location/register_current_location',
timeout=1)
rospy.ServiceProxy('/location/register_current_location',
RegisterLocation)("kitchen")
web_text = data.data.lower()
order_sentence_list = [
"i want beans", "i want biscuits", "i want coca cola",
"i want cookies", "i want grape juice", "i want green tea",
"i want olive", "i want onion soup", "i want pringles"
]
# 類似度計算
score = 0
order_sentence = ""
for x in order_sentence_list:
s = difflib.SequenceMatcher(None, x, web_text).ratio()
if score < s:
score = s
order_sentence = x
order = " ".join(order_sentence.split()[2:]) # 商品名
self.speak("Order is {}.".format(order))
self.speak("Please give me items.")
time.sleep(5)
self.speak("I will deliver {}.".format(order))
# call_duckerにメッセージを送信
self.call_ducker_pub.publish("start")
def call_ducker_callback(self, msg):
# type:(Bool) -> None
"""
call_duckerのcallback関数
call_ducekrの成功/失敗を判定
:param: msg: Bool
:return: なし
"""
self.print_function("call_ducker_callback")
if msg.data:
# テーブルに着いた
self.speak(
"Thank you for waiting. Here you are. So, I want you to take items."
)
time.sleep(5)
rospy.wait_for_service("/location/request_location", timeout=1)
response = rospy.ServiceProxy("/location/request_location",
RequestLocation)("kitchen").location
self.send_move_base(response)
else:
self.speak("Sorry, Please carry it in front of the customer.")
print "お客さんの前に運んでください。"
self.active_flag = False
def send_move_base(self, point):
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = "map"
goal.target_pose.pose.position = Point(point.x, point.y, point.z)
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(text):
# type: (str) -> None
"""
sound_system_ros に対して発話を要求する
発話が終了するまで待機
:param text: 発話内容
:return: なし
"""
rospy.wait_for_service("/sound_system/speak")
rospy.ServiceProxy("/sound_system/speak", StringService)(text)
@staticmethod
def print_function(name):
# type: (str) -> None
"""
関数名を表示
:return: なし
"""
print("\n###########################################\n")
print(" Function: {}".format(name))
print("\n###########################################\n")
class AbstractModule(object):
def __init__(self, node_name):
# type: (str) -> None
rospy.init_node(node_name)
self.nlp_pub = rospy.Publisher(
"/natural_language_processing/speak_sentence",
String,
queue_size=10)
self.marker = RvizMarker()
self.move_base_client = actionlib.SimpleActionClient(
"/move_base", MoveBaseAction)
def send_place_msg(self, place):
# type: (str) -> None
"""
navigationに場所を伝える
:param place: 場所
:return:
"""
rospy.wait_for_service("/location/request_location", timeout=1)
response = rospy.ServiceProxy("/location/request_location",
RequestLocation)(place).location
self.send_move_base(response)
def send_move_base(self, point):
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = "map"
goal.target_pose.pose.position = Point(point.x, point.y, point.z)
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(text):
# type: (str) -> None
"""
sound_system_ros に対して発話を要求する
発話が終了するまで待機
:param text: 発話内容
:return: なし
"""
rospy.wait_for_service("/sound_system/speak")
rospy.ServiceProxy("/sound_system/speak", StringService)(text)
@staticmethod
def print_node(name):
# type: (str) -> None
"""
ノード名を表示
:return: なし
"""
print("\n###########################################\n")
print(" Node: {}".format(name))
print("\n###########################################\n")
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
class HmcSendPlaceMsg(AbstractModule):
def __init__(self):
super(HmcSendPlaceMsg, self).__init__(node_name="hmc_send_place_msg")
self.place = ""
self.arm_pub = rospy.Publisher("/arm/control", Int32, queue_size=10)
self.human_detection_pub = rospy.Publisher("/human_detection/control", String, queue_size=10)
self.move_base_client = actionlib.SimpleActionClient("/move_base", MoveBaseAction)
self.marker = RvizMarker()
rospy.Subscriber("/natural_language_processing/send_place_msg", String, self.send_place_msg)
rospy.Subscriber("/hmc/send_place_msg", String, self.save_place_info)
def send_place_msg(self, argument):
# type: (String) -> None
"""
bagを運ぶ先の場所情報をpublishする
:param argument: ゴミ
:return: なし
"""
self.print_node(argument.data)
self.arm_pub.publish()
print self.place
self.speak("OK, I will go to the {}.".format(self.place))
self.arm_pub.publish(2)
rospy.sleep(3)
rospy.wait_for_service('/location/request_location', timeout=1)
coordinate = rospy.ServiceProxy('/location/request_location', RequestLocation)(self.place).location
self.send_move_base(coordinate)
self.release_bag()
self.human_detection_pub.publish("start")
def release_bag(self):
# type: () -> None
"""
目的地到着後,バッグを置く.
:param: argument: 関数用の引数が格納されている.
本関数では空である.
:return: なし
"""
self.speak('I put the bag')
self.arm_pub.publish(3)
rospy.sleep(6)
self.arm_pub.publish(4)
rospy.sleep(4)
def save_place_info(self, place):
"""
場所情報を保管するためのcallback関数
:param place: 場所名
:return: なし
"""
self.place = place.data
print self.place
def send_move_base(self, point):
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = "map"
goal.target_pose.pose.position = Point(point.x, point.y, point.z)
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()
class HumanDetection:
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_degree = 0
self.flag = True
self.marker = RvizMarker()
self.person_position = {}
rospy.init_node("human_detection")
rospy.Subscriber("/ros_posenet/result",
Poses,
self.pose_callback,
queue_size=1)
rospy.Subscriber("/human_detection/control",
String,
self.control_callback,
queue_size=1)
rospy.Subscriber("/odom", Odometry, self.odometry_callback)
self.move_base_client = actionlib.SimpleActionClient(
"/move_base", MoveBaseAction)
self.amount_client = actionlib.SimpleActionClient(
"/move/amount", AmountAction)
self.finish_pub = rospy.Publisher("/human_detection/finish",
String,
queue_size=1)
@staticmethod
def print_node(name):
# type: (str) -> None
"""
ノード名を表示
:return: なし
"""
print("\n###########################################\n")
print(" Node: {}".format(name))
print("\n###########################################\n")
@staticmethod
def to_angle(rad):
return rad * 180 / math.pi
@staticmethod
def to_radian(angle):
return (angle * math.pi) / 180
def to_quaternion_ang(self, w, z):
if abs(z > 0):
return self.to_angle(math.acos(w) * 2)
else:
return 360 - self.to_angle(math.acos(w) * 2)
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.4
self.amount_client.wait_for_server()
self.amount_client.send_goal(goal)
self.amount_client.wait_for_result(rospy.Duration(30))
def calc_person_position(self, pose):
try:
# 人間の三次元座標を計算
count = 0
sum_x = 0
sum_y = 0
sum_z = 0
image_sum_x = 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
image_sum_x += key.image_position.x
count += 1
if count < 4:
return None
ave_x = sum_x / count
ave_y = sum_y / count
ave_z = sum_z / count
ave_image_x = image_sum_x / count
distance = math.sqrt(ave_x**2 + ave_y**2 + ave_z**2)
return distance, (ave_x, ave_y, ave_z), ave_image_x
except ZeroDivisionError:
print "countが0 @raise_hand"
return None
def calc_raise_person_position(self, persons):
"""
手を上げている人の3D場所を計算
:return:
"""
sum_x = 0
sum_y = 0
for person in persons:
sum_x += person[0]
sum_y += person[1]
ave_x = sum_x / len(persons)
ave_y = sum_y / len(persons)
result = self.calc_safe_position(MARGIN, (ave_x, ave_y))
print result
return result[0], result[1]
def calc_real_position(self, point):
# type:(tuple)->tuple
relative_theta = self.sensor_degree
relative_x = point[2]
relative_y = point[0]
delta_theta = math.atan(relative_y / relative_x)
# x = relative_x * math.cos(relative_theta) - relative_y * math.sin(relative_theta)
# y = relative_x * math.sin(relative_theta) + relative_y * math.cos(relative_theta)
x = math.sqrt(relative_x**2 +
relative_y**2) * math.cos(relative_theta + delta_theta)
y = math.sqrt(relative_x**2 +
relative_y**2) * math.sin(relative_theta + delta_theta)
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 control_callback(self, msg):
# type:(String)->None
self.print_node("human_detection")
if msg.data == "start":
self.flag = False
for i in range(360 // 45):
self.move_turn(45)
time.sleep(1)
self.flag = True
if len(self.person_position) == 0:
self.speak("sorry, not found.")
self.finish_pub.publish(String(data="finish"))
return
print min(self.person_position)
self.move_turn(self.person_position[min(self.person_position)][1] -
self.sensor_degree)
self.speak("Done.")
self.finish_pub.publish(String(data="finish"))
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_degree = self.to_quaternion_ang(
msg.pose.pose.orientation.w, msg.pose.pose.orientation.z)
def pose_callback(self, msgs):
# type: (Poses)->None
"""
関節推定の結果を受け取り
:param msgs:
:return:
"""
if self.flag:
return
for pose in msgs.poses:
result = self.calc_person_position(pose)
if result is None:
continue
if not 300 < result[2] < 400:
continue
print self.sensor_degree
self.person_position.setdefault(result[0],
(result[1], self.sensor_degree))
self.speak("found")