def find_people(self, image):
segment_data = self.msgtobody(image)
result = self.client_body.bodyAttr(segment_data, self.options_body)
if 'person_num' in result:
person_num = result['person_num']
# self.objpos_pub.publish()
else:
person_num = 0
print("THe number of person is " + str(person_num))
if person_num == 0:
self.ROI.x_offset = 0
self.ROI.y_offset = 0
self.ROI.width = 0
self.ROI.height = 0
self.roi_pub.publish(self.ROI)
for num in range(0, int(person_num)):
# 不认识的人则发布信息ROI
cv2.imwrite(self.filepath, image)
if self.face_search(self.filepath) == False:
location = result['person_info'][num]['location']
print(location)
# print('upper_color:',upper_color,' upper_wear_fg:',upper_wear_fg)
self.ROI.x_offset = int(location['left'])
self.ROI.y_offset = int(location['top'])
self.ROI.width = int(location['width'])
self.ROI.height = int(location['height'])
self.roi_pub.publish(self.ROI)
print(self.ROI)
msg = reception()
msg.NowTask = msg.GuestRecognition
msg.NextTask = msg.Requesting
msg.FinishState = True
self.control_pub.publish(msg)
def __init__(self):
msg = reception()
self.now_state = msg.DoorDetection
self.next_state = msg.EnterSite
self.Return_time = 0
self.need_help = False
self.control_sub = rospy.Subscriber("/reception_control", reception, self.controlCallback)
self.control_pub = rospy.Publisher("/reception_control", reception)
msg.NowTask = msg.DoorDetection
msg.NextTask = msg.EnterSite
msg.NeedHelp = False
msg.FinishState = False
rospy.sleep(0.5)
for i in range(5):
self.control_pub.publish(msg)
self.sh.say('i am ready to enter the arena.', self.voice)
print("Start task Receptionist...")
print("----------DoorDetection-----------")
def control_callback(self, msg=reception()):
# 避免重复获取命令
# 执行完成之后就可以转为False
if self.get_control_order == True:
return
self.get_control_order = True
self.msg = msg
if msg.NowTask == msg.EnterSite and msg.FinishState == False:
# 去门口(入场)
self.start_navigation = True
self.current_goal = self.target_goals["Door"]
elif msg.NowTask == msg.Leading and msg.FinishState == False:
# 导航到客厅
self.start_navigation = True
self.current_goal = self.target_goals["Livingroom"]
elif msg.NowTask == msg.Backward and msg.FinishState == False:
# 导航回门口
self.start_navigation = True
self.current_goal = self.target_goals["Door"]
else:
return
def receptionist_Callback(self, msg):
self.nowtask = msg.NowTask
if msg.NowTask == msg.Requesting and msg.FinishState == False:
self.name.append("anonymous")
self.drink.append("water")
self.age.append("100")
self.sh.say(
'Please tell me your name and what would you like to drink, sir? If you are ready to response, please say jack to launch me first',
self.voice)
self.sh.nowmission = 0
if msg.NowTask == msg.CannotOpen and msg.FinishState == False:
self.sh.say('Please help me open the door', self.voice)
task_msg = reception()
task_msg.NowTask = task_msg.CannotOpen
task_msg.NextTask = task_msg.GuestRecognition
task_msg.FinishState = True
task_msg.NeedHelp = False
self.finish_task.publish(task_msg)
def arm_Callback(self, msg):
task_msg = reception()
if self.nowtask == 7: # Introducing
if len(self.name) == 1:
introduce_content = 'Hi, John! This is {}, {} likes to drink {}.'.format(
self.name[-1], self.name[-1], self.drink[-1])
intro_to_guest_content = '{}, This is John, John likes to drink {}.'.format(
self.name[-1], self.drink[0])
else:
if len(self.name) == 2:
introduce_content = 'Hi, John and {}! This is {}, {} likes to drink {}.'.format(
self.name[0], self.name[-1], self.name[-1],
self.drink[-1])
intro_to_guest_content = '{}, This is John and {}, John likes to drink {} and {} likes to drink {}.'.format(
self.name[-1], self.name[1], self.drink[0],
self.name[1], self.drink[1])
self.sh.say(introduce_content, self.voice)
self.sh.say(intro_to_guest_content, self.voice)
task_msg.NowTask = self.nowtask
task_msg.NextTask = task_msg.Serving
task_msg.FinishState = True
task_msg.NeedHelp = False
elif self.nowtask == 8: # Serving
self.sh.say('Dear guest, you can sit here', self.voice)
task_msg.NowTask = self.nowtask
task_msg.NextTask = task_msg.Backward
task_msg.FinishState = True
task_msg.NeedHelp = False
else:
self.sh.say('there is no task that matches', self.voice)
self.finish_task.publish(task_msg)
def xfeiCallback(self, msg):
if msg.data.strip() == '':
self.sh.say(
"Sorry I did not hear what you just said clearly, please tell me again",
self.voice)
# self.sh.say("please tell me again", self.voice)
else:
if self.nowmission == 0:
ans = str(msg.data)
tokens = nltk.word_tokenize(ans)
tokens = nltk.pos_tag(tokens)
for token in tokens:
if token[1] == "NNP":
self.name[-1] = token[0]
if token[1] == "NN":
self.drink[-1] = token[0]
self.sh.say(
'Your name is {} and your favorite drink is {}, right? Please say jack to launch me first, then say yes or no'
.format(self.name[-1], self.drink[-1]), self.voice)
self.nowmission = 1
elif self.nowmission == 1:
ans = str(msg.data)
tokens = nltk.word_tokenize(ans)
print(tokens)
for i in tokens:
if (i == "no") or (i == "NO") or (i == "No"):
self.sh.say("can you tell me again, please?",
self.voice)
self.nowmission = 0
break
else:
if (i == "Yes") or (i == "YES") or (i == "yes") or (
i == "yeah") or (i == "Yeah"):
self.sh.say(
"can you tell me your age, please? If you are ready to response, please say jack to launch me first",
self.voice)
self.nowmission = 2
break
elif self.nowmission == 2:
ans = str(msg.data)
tokens = nltk.word_tokenize(ans)
tokens = nltk.pos_tag(tokens)
for token in tokens:
if token[1] == "CD":
self.age[-1] = token[0]
break
# self.nowmission = 0
self.sh.say(
"ok, your name is {}, your favorite drink is {}, and you are {}. And now please follow me to the living room."
.format(self.name[-1], self.drink[-1],
self.age[-1]), self.voice)
task_msg = reception()
task_msg.NowTask = task_msg.Requesting
task_msg.NextTask = task_msg.Leading
task_msg.FinishState = True
task_msg.NeedHelp = False
self.finish_task.publish(task_msg)
def __init__(self):
# 初始化节点
rospy.init_node('receptionist_nav_core', anonymous=False)
# 建立响应函数
rospy.on_shutdown(self.shutdown)
# 控制频率
self.rate = rospy.Rate(1)
# 速度控制话题,用于速度缓冲 Publisher to manually control the robot (e.g. to stop it, queue_size=5)
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# 目标点状态Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# --------------------定义初始的目标序列--------------------
self.target_goals = dict()
# TODO:需要初始定义的地方
self.init_position()
# 记录当前任务下需要前往的目标点
self.current_goal = Pose()
# 是否开始导航
self.start_navigation = False
# 是否得到控制命令
self.get_control_order = False
#---------------------用于记录任意时刻的位置与姿态-------------
# 按照需求记录位姿
self.get_current_pose = False # ?
# 如果需要的时候就把当前的位姿记录下来
rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, queue_size=1)
# 同时记录目标位置标记
self.current_pose_label = str()
#--------------------------------链接move_base 服务------------------------------
# 链接服务器
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
#-------------------------------设置初始位姿--------------------------------------
#-------------------------------阻塞进程-----------------------------------------
# 阻塞进程,获取初始位姿
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
self.initial_pose = PoseWithCovarianceStamped()
rospy.loginfo(
"*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
) # emmmm, 需要改一下，因为到时候initialpose已知了
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
rospy.Subscriber('/initialpose', PoseWithCovarianceStamped,
self.update_initial_pose)
self.pub_current_pose = rospy.Publisher('/initialpose',
PoseWithCovarianceStamped,
queue_size=1)
# 阻塞进程
# Make sure we have the initial pose
while self.initial_pose.header.stamp == "":
rospy.loginfo("header.stamp...")
rospy.sleep(1)
#-------------------------设置初始变量------------------------------------------------
# 下面都是一些简单的信息,用于记录导航状态
# Variables to keep track of success rate, running time,
# and distance traveled
i = 0
n_goals = 0
n_successes = 0
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
# Get the initial pose from the user
# -------------------------等待控制流唤醒---------------------------------------------
rospy.Subscriber("/control",
reception,
self.control_callback,
queue_size=1)
self.control_pub = rospy.Publisher("/control", reception, queue_size=1)
self.msg = reception()
#---------------------等待goal唤醒---------------------------------
self.last_location = Pose()
self.location = Pose()
#----------------------开始导航----------------------------------------
# Begin the main loop and run through a sequence of locations
rospy.loginfo("Starting navigation test")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the current sequence,
# start with a new random sequence
if not self.start_navigation:
rospy.loginfo("Wait for new goal!")
self.rate.sleep()
continue
# 避免重复进入
self.start_navigation = False
self.location = self.current_goal
# 更新初始化位姿,实时更新位姿
if self.initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[self.location].position.x -
locations[self.last_location].position.x, 2) + pow(
locations[self.location].position.y -
locations[self.last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[self.location].position.x -
self.initial_pose.pose.pose.position.x, 2) + pow(
locations[self.location].position.y -
self.initial_pose.pose.pose.position.y, 2))
self.initial_pose.header.stamp = ""
# Store the last location for distance calculations
self.last_location = self.location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[self.location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: %f %f %f" %
(self.location.position.x, self.location.position.y,
self.location.position.z))
# Start the robot toward the next location
# 开始机器人向下一个节点
self.move_base.send_goal(self.goal,
done_cb=self.donecb,
active_cb=self.activecb,
feedback_cb=self.feedbackcb)
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(300))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
rospy.loginfo("Timed out achieving goal")
self.pub_control()
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes / n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " +
str(trunc(distance_traveled, 1)) + " m")
# 接收下一个命令
self.get_control_order = False
self.rate.sleep()
def controlCallback(self, msg):
if msg.NeedHelp == True:
print("Need help while there's no help way available.")
# TODO:发布求救的节点（目前没有）
elif msg.FinishState == True:
n_msg = reception() #new msg
# TODO:发布新的消息
n_msg.NeedHelp = False
n_msg.FinishState = False
if msg.NowTask == n_msg.DoorDetection and self.now_state == n_msg.DoorDetection:
n_msg.NowTask = n_msg.EnterSite
n_msg.NextTask = n_msg.CannotOpen
print("-------EnterSite-------")
for i in range(5):
self.control_pub.publish(n_msg)
self.now_state = n_msg.NowTask
self.next_state= n_msg.NextTask
elif msg.NowTask == n_msg.EnterSite and self.now_state == n_msg.EnterSite:
n_msg.NowTask = n_msg.CannotOpen
n_msg.NextTask = n_msg.GuestRecognition
print("-------CannotOpen-------")
for i in range(5):
self.control_pub.publish(n_msg)
self.now_state = n_msg.NowTask
self.next_state= n_msg.NextTask
elif msg.NowTask == n_msg.CannotOpen and self.now_state == n_msg.CannotOpen:
n_msg.NowTask = n_msg.GuestRecognition
n_msg.NextTask = n_msg.Requesting
print("-------GuestRecognition-------")
for i in range(5):
self.control_pub.publish(n_msg)
self.now_state = n_msg.NowTask
self.next_state= n_msg.NextTask
elif msg.NowTask == n_msg.GuestRecognition and self.now_state == n_msg.GuestRecognition:
n_msg.NowTask = n_msg.Requesting
n_msg.NextTask = n_msg.Leading
print("-------Requesting-------")
for i in range(5):
self.control_pub.publish(n_msg)
self.now_state = n_msg.NowTask
self.next_state= n_msg.NextTask
elif msg.NowTask == n_msg.Requesting and self.now_state == n_msg.Requesting:
n_msg.NowTask = n_msg.Leading
n_msg.NextTask = n_msg.Introducing
print("-------Leading-------")
for i in range(5):
self.control_pub.publish(n_msg)
self.now_state = n_msg.NowTask
self.next_state= n_msg.NextTask
elif msg.NowTask == n_msg.Leading and self.now_state == n_msg.Leading:
n_msg.NowTask = n_msg.Introducing
n_msg.NextTask = n_msg.Serving
print("-------Introducing-------")
for i in range(5):
self.control_pub.publish(n_msg)
self.now_state = n_msg.NowTask
self.next_state= n_msg.NextTask
elif msg.NowTask == n_msg.Introducing and self.now_state == n_msg.Introducing:
n_msg.NowTask = n_msg.Serving
n_msg.NextTask = n_msg.Backward
print("-------Serving-------")
thread.start_new_thread(self.open_yolo, ())
rospy.sleep(2) # wait for node yolo to open
for i in range(5):
self.control_pub.publish(n_msg)
self.now_state = n_msg.NowTask
self.next_state= n_msg.NextTask
elif msg.NowTask == n_msg.Serving and self.now_state == n_msg.Serving:
n_msg.NowTask = n_msg.Backward
n_msg.NextTask = n_msg.GuestRecognition
print("-------Backward-------")
thread.start_new_thread(self.kill_yolo, ())
for i in range(5):
self.control_pub.publish(n_msg)
self.now_state = n_msg.NowTask
self.next_state= n_msg.NextTask
elif msg.NowTask == n_msg.Backward and self.now_state == n_msg.Backward:
self.Return_time += 1
if self.Return_time >= 2:
print("1st guest task finished!")
else:
n_msg.NowTask = n_msg.CannotOpen
n_msg.NextTask = n_msg.GuestRecognition
for i in range(5):
self.control_pub.publish(n_msg)
print("Once Again!")
print("-------CannotOpen-------")
self.now_state = n_msg.NowTask
self.next_state = n_msg.NextTask