def __init__(self):
msg = whereisthis()
self.now_state = msg.GetPoint
self.next_state = msg.DoorOpening
self.Return_time = 0
self.need_help = False
self.control_sub = rospy.Subscriber("/whereisthis_control", whereisthis, self.controlCallback)
self.control_pub = rospy.Publisher("/whereisthis_control", whereisthis, queue_size = 1)
self.sh = SoundClient(blocking = True)
self.voice = rospy.get_param("~voice", "voice_kal_diphone")
msg.NowTask = msg.GetPoint
msg.NextTask = msg.DoorOpening
msg.NeedHelp = False
msg.FinishState = False
rospy.sleep(0.5)
# self.sh.say('now please tell me where there information point is. If you are ready to response, please say jack to launch me first', self.voice)
for i in range(5):
self.control_pub.publish(msg)
print("Start task Whereisthis...")
print("----------GetPoint-----------")
def __init__(self):
msg = whereisthis()
self.now_state = msg.GetPoint
self.next_state = msg.DoorOpening
self.Return_time = 0
self.need_help = False
self.control_sub = rospy.Subscriber("/whereisthis_control",
whereisthis, self.controlCallback)
self.control_pub = rospy.Publisher("/whereisthis_control",
whereisthis,
queue_size=1)
self.sh = SoundClient(blocking=True)
self.voice = rospy.get_param("~voice", "voice_kal_diphone")
msg.NowTask = msg.GetPoint
msg.NextTask = msg.DoorOpening
msg.NeedHelp = False
msg.FinishState = False
rospy.sleep(0.5)
self.sh.say('I am ready to start!', self.voice)
# for i in range(5):
self.control_pub.publish(msg)
print("Start task Whereisthis...")
print("----------GetPoint-----------")
def control_callback(self, msg=whereisthis()):
# 避免重复获取命令
# 执行完成之后就可以转为False
#if self.get_control_order == True :
#return
self.get_control_order = True # no use
self.msg = msg
if self.msg.NowTask == msg.GotoPoint or self.msg.NowTask == msg.Guiding or self.msg.NowTask == msg.BacktoPoint:
if self.msg.NowTask == msg.BacktoPoint:
self.destination = self.infopoint
if self.destination == 1 and msg.FinishState== False:
# 去A
self.start_navigation = True
self.current_goal = self.target_goals["A"]
elif self.destination == 2 and msg.FinishState== False:
# B
self.start_navigation = True
self.current_goal = self.target_goals["B"]
elif self.destination == 3 and msg.FinishState== False:
# C
self.start_navigation = True
self.current_goal = self.target_goals["C"]
elif self.destination == 4 and msg.FinishState== False:
# D
self.start_navigation = True
self.current_goal = self.target_goals["D"]
def imgCallback(self, image_msg):
t = whereisthis()
if self.nowtask == t.GoalDescription:
rospy.loginfo('imgCallback working...')
try:
cv_image = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
except CvBridgeError as e:
print(e)
self.add_user(self.filepath, cv_image)
def controlCallback(self, msg):
if msg.NeedHelp == True:
print("Need help while no help way available.")
# TODO:发布求救的节点（目前没有）
elif msg.FinishState == True:
#self.sh.say('hello!', self.voice)
n_msg = whereisthis() #new msg
# TODO:发布新的消息
n_msg.NeedHelp = False
n_msg.FinishState = False
if msg.NowTask == n_msg.GetPoint and self.now_state == n_msg.GetPoint:
self.sh.say('I got the control command.', self.voice)
n_msg.NowTask = n_msg.DoorOpening
n_msg.NextTask = n_msg.GotoPoint
# self.sh.say('i am ready to enter the arena now', self.voice)
n_msg.FinishState = True
self.control_pub.publish(n_msg)
print("-------DoorOpening-------")
# 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.DoorOpening and self.now_state == n_msg.DoorOpening:
n_msg.NowTask = n_msg.GotoPoint
n_msg.NextTask = n_msg.GoalDescription
self.sh.say(
'thank you. now i will go to the information point.',
self.voice)
print("-------GotoPoint-------")
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.GotoPoint and self.now_state
== n_msg.GotoPoint) or (msg.NowTask == n_msg.BacktoPoint and
self.now_state == n_msg.BacktoPoint):
# self.sh.say(str(msg), self.voice)
if msg.NowTask == n_msg.BacktoPoint and self.now_state == n_msg.BacktoPoint:
if self.Return_time >= 3: # task finish
self.sh.say('I have finished the task.', self.voice)
else:
self.sh.say('I am ready to serve another guest',
self.voice)
self.Return_time += 1
n_msg.NowTask = n_msg.GoalDescription
n_msg.NextTask = n_msg.Guiding
self.sh.say(
'hi dear guest, please stand in front of me, thank you.',
self.voice)
print("-------GoalDescription-------")
thread.start_new_thread(self.launch_image_core, ())
rospy.sleep(
2
) # wait for image function to be ready & guest to stand in front of camera
#for i in range(5):
self.sh.say(
'i am ready to recognize you,and i will take a photo for you.',
self.voice)
self.control_pub.publish(n_msg)
self.now_state = n_msg.NowTask
self.next_state = n_msg.NextTask
elif msg.NowTask == n_msg.GoalDescription and self.now_state == n_msg.GoalDescription:
n_msg.NowTask = n_msg.Guiding
n_msg.NextTask = n_msg.BacktoPoint
self.sh.say('now i will guide you there. please follow me.',
self.voice)
print("-------Guiding-------")
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.Guiding and self.now_state == n_msg.Guiding:
n_msg.NowTask = n_msg.BacktoPoint
n_msg.NextTask = n_msg.GoalDescription
self.sh.say('now i will go back to the information point',
self.voice)
print("-------BacktoPoint-------")
for i in range(5):
self.control_pub.publish(n_msg)
self.now_state = n_msg.NowTask
self.next_state = n_msg.NextTask
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)
elif self.nowmission == 1: # 'Guiding' task, get information point and publish it
# recognize information point
edit_distance = []
ans = str(msg.data)
tokens = nltk.word_tokenize(ans)
tokens = nltk.pos_tag(tokens)
for token in tokens:
if (token[1] == "NNP") or (token[1] == "NN") or (token[1]
== "NNS"):
edit_distance.append([])
for i in range(len(self.hash_id)):
edit_distance[-1].append(
leven.distance(token[0], self.hash_id[i]))
self.inforid = self.generate_minimum_word(edit_distance)
if self.inforid >= 1 and self.inforid <= 8: # legal information point (includin cases of 'outta range')
#publish information point
t = Int8()
t.data = self.inforid
self.information_point.publish(t)
self.sh.say(
'My information point is {}. Thank you.'.format(
self.hash_id[self.inforid - 1]), self.voice)
#publish finish signal to control console
t = whereisthis()
t.NowTask = t.GetPoint
t.NextTask = t.DoorOpening
t.FinishState = True
t.NeedHelp = False
self.controlFeedback.publish(t)
else: # didn't catch the word, i.e. self.inforid is 'None'
self.sh.say(
'Sorry, can you tell me again? If you are ready to tell me, please say jack to launch me first.',
self.voice)
elif self.nowmission == 2: # 'GoalDescription' task, get destination point and publish it
# recognize destination
edit_distance = []
ans = str(msg.data)
tokens = nltk.word_tokenize(ans)
tokens = nltk.pos_tag(tokens)
for token in tokens:
if (token[1] == "NNP") or (token[1] == "NN") or (token[1]
== "NNS"):
edit_distance.append([])
for i in range(len(self.hash_id)):
edit_distance[-1].append(
leven.distance(token[0], self.hash_id[i]))
self.destid = self.generate_minimum_word(edit_distance)
if self.destid >= 1 and self.destid <= 8: # legal destination point (includin cases of 'outta range': only for debugging)
#publish destination point
t = Int8()
t.data = self.destid
self.mark_point.publish(t)
# describe the route
if self.personinfo[self.nowperson -
1][0] == 1: # didn't show up before
self.personinfo[self.nowperson -
1][1] = self.destid # store the goal point
self.sh.say(self.description[self.inforid -
1][self.destid - 1],
self.voice) # route: from info to dest
elif self.personinfo[self.nowperson -
1][0] >= 2: # has come before
self.sh.say(self.description[self.inforid -
1][self.destid - 1],
self.voice) # route from info to dest
pre_dest_id = self.personinfo[self.nowperson - 1][1]
pre_dest_name = self.hash_id[pre_dest_id -
1] # previous destination
cur_dest_name = self.hash_id[self.destid -
1] # current destination
self.sh.say(
'last time you asked me to go to {}. now I will also tell you how to go to {} from there.'
.format(pre_dest_name, cur_dest_name), self.voice)
# route: from previous goal to current goal
self.sh.say(
self.description[pre_dest_id - 1][self.destid - 1],
self.voice)
# publish finish signal to control console
t = whereisthis()
t.NowTask = t.GoalDescription
t.NextTask = t.Guiding
t.FinishState = True
t.NeedHelp = False
self.controlFeedback.publish(t)
else: # didn't catch the word, i.e. self.destid is 'None'
self.sh.say(
'Sorry, can you tell me again? If you are ready to tell me, please say jack to launch me first.',
self.voice)
def __init__(self):
# -----------------初始化节点-----------------
rospy.init_node('where_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
#---------------------用于记录任意时刻的位置与姿态-------------
# 按照需求记录位姿
rospy.Subscriber("/navi/present_pose", Pose, self.get_current_pose, 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...")
#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 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("/speech/infopoint", Int8, self.get_infopoint, queue_size=1)
rospy.Subscriber("/speech/markpoint", Int8, self.get_goalpoint, queue_size=1)
rospy.Subscriber("/whereisthis_control", whereisthis, self.control_callback, queue_size=1)
self.control_pub = rospy.Publisher("/whereisthis_control", whereisthis, queue_size=1)
self.markpoint_pub = rospy.Publisher("/navi/markpoint", markpoint, queue_size=1)
#self.guide_pub = rospy.Publisher("/control", markpoint, queue_size=1)
self.msg = whereisthis()
self.pointmsg = markpoint()
#---------------------等待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
rospy.loginfo("%s",self.current_goal)
self.location = self.current_goal
# 更新初始化位姿,实时更新位姿
#if self.initial_pose.header.stamp == "":
#distance = sqrt(pow(self.location.position.x -
#self.last_location.position.x, 2) +
#pow(self.location.position.y -
#self.last_location.position.y, 2))
#else:
#rospy.loginfo("Updating current pose.")
distance = sqrt(pow(self.location.position.x -
self.last_location.position.x, 2) +
pow(self.location.position.y -
self.last_location.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 = 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))
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 # no use
self.rate.sleep()
def get_current_pose(self, msg=Pose(),msg2=whereisthis()):
print("------------------------GETTING POSE-------------")
rospy.loginfo("get %s : %f %f %f %f"%("current_pose",msg.position.x,msg.position.y,msg.orientation.z,msg.orientation.w))
distanceA = sqrt(pow(msg.position.x - self.target_goals["A"].position.x, 2) +pow(msg.position.y -self.target_goals["A"].position.y, 2))
distanceB = sqrt(pow(msg.position.x - self.target_goals["B"].position.x, 2) +pow(msg.position.y -self.target_goals["B"].position.y, 2))
distanceC = sqrt(pow(msg.position.x - self.target_goals["C"].position.x, 2) +pow(msg.position.y -self.target_goals["C"].position.y, 2))
distanceD = sqrt(pow(msg.position.x - self.target_goals["D"].position.x, 2) +pow(msg.position.y -self.target_goals["D"].position.y, 2))
ky=self.current_goal.position.y-msg.position.y
kx=self.current_goal.position.x-msg.position.x
xky=self.current_goal.position.x*ky
ykx=self.current_goal.position.y*kx
##########################################################
if self.msg.NowTask == self.msg.Guiding:
if distanceA < 0.6 and self.current_goal != self.target_goals["A"] and self.last_location !=self.target_goals["A"]:
rospy.loginfo("I'm near A!")
self.pointmsg.id = 1
if ky*kx >0:
if ky*self.target_goals["A"].position.x-self.target_goals["A"].position.y*kx-xky+ykx >0:
rospy.loginfo("A is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("A is on my LEFT!")
self.pointmsg.side = 0
elif ky*kx <0:
if ky*self.target_goals["A"].position.x-self.target_goals["A"].position.y*kx-xky+ykx <0:
rospy.loginfo("A is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("A is on my LEFT!")
self.pointmsg.side = 0
elif ky==0 and kx>0:
if self.target_goals["A"].position.x > self.current_goal.position.x:
rospy.loginfo("A is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("A is on my LEFT!")
self.pointmsg.side = 0
elif ky==0 and kx>0:
if self.target_goals["A"].position.y < self.current_goal.position.y:
rospy.loginfo("A is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("A is on my LEFT!")
self.pointmsg.side = 0
elif kx==0 and ky>0:
if self.target_goals["A"].position.x > self.current_goal.position.x:
rospy.loginfo("A is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("A is on my LEFT!")
self.pointmsg.side = 1
self.markpoint_pub.publish(self.pointmsg)
if distanceB < 0.6 and self.current_goal != self.target_goals["B"] and self.last_location !=self.target_goals["B"]:
rospy.loginfo("I'm near B!")
self.pointmsg.id = 2
if ky*kx >0:
if ky*self.target_goals["B"].position.x-self.target_goals["B"].position.y*kx-xky+ykx >0:
rospy.loginfo("B is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("B is on my LEFT!")
self.pointmsg.side = 0
elif ky*kx <0:
if ky*self.target_goals["B"].position.x-self.target_goals["B"].position.y*kx-xky+ykx <0:
rospy.loginfo("B is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("B is on my LEFT!")
self.pointmsg.side = 0
elif ky==0 and kx>0:
if self.target_goals["B"].position.x > self.current_goal.position.x:
rospy.loginfo("B is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("B is on my LEFT!")
self.pointmsg.side = 0
elif ky==0 and kx>0:
if self.target_goals["B"].position.y < self.current_goal.position.y:
rospy.loginfo("B is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("B is on my LEFT!")
self.pointmsg.side = 0
elif kx==0 and ky>0:
if self.target_goals["B"].position.x > self.current_goal.position.x:
rospy.loginfo("B is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("B is on my LEFT!")
self.pointmsg.side = 1
self.markpoint_pub.publish(self.pointmsg)
if distanceC < 0.6 and self.current_goal != self.target_goals["C"] and self.last_location !=self.target_goals["C"]:
rospy.loginfo("I'm near C!")
self.pointmsg.id = 3
if ky*kx >0:
if ky*self.target_goals["C"].position.x-self.target_goals["C"].position.y*kx-xky+ykx >0:
rospy.loginfo("C is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("C is on my LEFT!")
self.pointmsg.side = 0
elif ky*kx <0:
if ky*self.target_goals["C"].position.x-self.target_goals["C"].position.y*kx-xky+ykx <0:
rospy.loginfo("C is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("C is on my LEFT!")
self.pointmsg.side = 0
elif ky==0 and kx>0:
if self.target_goals["C"].position.x > self.current_goal.position.x:
rospy.loginfo("C is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("C is on my LEFT!")
self.pointmsg.side = 0
elif ky==0 and kx>0:
if self.target_goals["C"].position.y < self.current_goal.position.y:
rospy.loginfo("C is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("C is on my LEFT!")
self.pointmsg.side = 0
elif kx==0 and ky>0:
if self.target_goals["C"].position.x > self.current_goal.position.x:
rospy.loginfo("C is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("C is on my LEFT!")
self.pointmsg.side = 1
self.markpoint_pub.publish(self.pointmsg)
if distanceD < 0.6 and self.current_goal != self.target_goals["D"] and self.last_location !=self.target_goals["D"]:
rospy.loginfo("I'm near D!")
self.pointmsg.id = 2
if ky*kx >0:
if ky*self.target_goals["D"].position.x-self.target_goals["D"].position.y*kx-xky+ykx >0:
rospy.loginfo("D is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("D is on my LEFT!")
self.pointmsg.side = 0
elif ky*kx <0:
if ky*self.target_goals["D"].position.x-self.target_goals["D"].position.y*kx-xky+ykx <0:
rospy.loginfo("D is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("D is on my LEFT!")
self.pointmsg.side = 0
elif ky==0 and kx>0:
if self.target_goals["D"].position.x > self.current_goal.position.x:
rospy.loginfo("D is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("D is on my LEFT!")
self.pointmsg.side = 0
elif ky==0 and kx>0:
if self.target_goals["D"].position.y < self.current_goal.position.y:
rospy.loginfo("D is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("D is on my LEFT!")
self.pointmsg.side = 0
elif kx==0 and ky>0:
if self.target_goals["D"].position.x > self.current_goal.position.x:
rospy.loginfo("D is on my RIGHT!")
self.pointmsg.side = 1
else:
rospy.loginfo("D is on my LEFT!")
self.pointmsg.side = 1
self.markpoint_pub.publish(self.pointmsg)
