#!/usr/bin/env python
#coding: utf-8
import rospy
from geometry_msgs.msg import Point
import threading
import actionlib
import time
from actionlib_msgs.msg import GoalStatus
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseWithCovarianceStamped
from tf_conversions import transformations
from ar_track_alvar_msgs.msg import AlvarMarkers
from math import pi
from std_msgs.msg import String
import sys
reload(sys)
sys.setdefaultencoding('utf-8')
class Navigation:
def __init__(self):
self.set_pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=5)
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
self.move_base.wait_for_server(rospy.Duration(60))
def set_pose(self, p):
if self.move_base is None:
return False
x, y, th = p
pose = PoseWithCovarianceStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = 'map'
pose.pose.pose.position.x = x
pose.pose.pose.position.y = y
q = transformations.quaternion_from_euler(0.0, 0.0, th/180.0*pi)
pose.pose.pose.orientation.x = q[0]
pose.pose.pose.orientation.y = q[1]
pose.pose.pose.orientation.z = q[2]
pose.pose.pose.orientation.w = q[3]
self.set_pose_pub.publish(pose)
return True
def _feedback_cb(self, feedback):
msg = feedback
#rospy.loginfo("[Navi] navigation feedback\r\n%s"%feedback)
def goto(self, p):
rospy.loginfo("[Navigation] goto %s"%p)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = p[0]
goal.target_pose.pose.position.y = p[1]
q = transformations.quaternion_from_euler(0.0, 0.0, p[2]/180.0*pi)
goal.target_pose.pose.orientation.x = q[0]
goal.target_pose.pose.orientation.y = q[1]
goal.target_pose.pose.orientation.z = q[2]
goal.target_pose.pose.orientation.w = q[3]
self.move_base.send_goal(goal, self._done_cb, self._active_cb, self._feedback_cb)
result = self.move_base.wait_for_result(rospy.Duration(60))
if not result:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("reach goal %s succeeded!"%p)
return True
def _done_cb(self, status, result):
rospy.loginfo("navigation done! status:%d result:%s"%(status, result))
def _active_cb(self):
rospy.loginfo("[Navi] navigation has be actived")
def cancel(self):
self.move_base.cancel_all_goals()
return True
class ARTracker:
def __init__(self):
self.ar_sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.ar_cb)
def ar_cb(self,data):
global tag_id
for marker in data.markers:
tag_id = marker.id
class Object_position:
def __init__(self):
self.ar_sub = rospy.Subscriber('/object_position', Point, self.find_cb)
self.tts_pub = rospy.Publisher('/voiceWords', String, queue_size=10)
def find_cb(self,data):
global find_id
point_msg = data
if(point_msg.z>=1 and point_msg.z<=5):
find_id = 1
self.tts_pub.publish(str(find_id))
elif(point_msg.z>=9 and point_msg.z<=15):
find_id = 2
self.tts_pub.publish(str(find_id))
elif(point_msg.z>=16 and point_msg.z<=23):
find_id = 3
self.tts_pub.publish(str(find_id))
elif(point_msg.z>=25 and point_msg.z<=26):
find_id = 4
self.tts_pub.publish(str(find_id))
elif(point_msg.z>=36 and point_msg.z<=40):
find_id = 5
self.tts_pub.publish(str(find_id))
elif(point_msg.z>=41 and point_msg.z<=43):
find_id = 6
self.tts_pub.publish(str(find_id))
elif(point_msg.z>=70 and point_msg.z<=71):
find_id = 7
self.tts_pub.publish(str(find_id))
elif(point_msg.z>=80 and point_msg.z<=81):
find_id = 8
self.tts_pub.publish(str(find_id))
else:
find_id = 0
#print("id为0,没有识别到!")
def process():
rospy.spin()
find_id = 0
tag_id = 0
both_id =0
if __name__ == '__main__':
rospy.init_node('navigation_demo',anonymous=True)
goalListX = rospy.get_param('~goalListX', '2.0, 2.0,2.0')
goalListY = rospy.get_param('~goalListY', '2.0, 4.0,2.0')
goalListYaw = rospy.get_param('~goalListYaw', '0, 90.0,2.0')
goals = [[float(x), float(y), float(yaw)] for (x, y, yaw) in zip(goalListX.split(","),goalListY.split(","),goalListYaw.split(","))]
##为了方便记忆,goals中[0]是终点,[1]到[8]分别对应场上的8个点,9是第一个框的识别点,10是第二个框的识别点,11是第三个框的识别点,12是第四个框的识别点
object_position = Object_position()
ar_acker = ARTracker()
executor_thread = threading.Thread(target=process).start()
navi = Navigation()
find_point_flag=[0,0,0,0]
have_nav_flag=[0,0,0,0]
time.sleep(10)
navi.goto(goals[9])
find_point_flag[0]=1
while True:
if find_id==1 or tag_id==1:
both_id=1
elif find_id==2 or tag_id==2:
both_id=2
elif find_id==3 or tag_id==3:
both_id=3
elif find_id==4 or tag_id==4:
both_id=4
elif find_id==5 or tag_id==5:
both_id=5
elif find_id==6 or tag_id==6:
both_id=6
elif find_id==7 or tag_id==7:
both_id=7
elif find_id==8 or tag_id==8:
both_id=8
else:
both_id=0
if both_id==0:
if have_nav_flag[0]==1 and find_point_flag[1]==0:
navi.goto(goals[10])
find_point_flag[1]=1
if have_nav_flag[1]==1 and find_point_flag[2]==0:
navi.goto(goals[11])
find_point_flag[2]=1
if have_nav_flag[2]==1 and find_point_flag[3]==0:
navi.goto(goals[12])
find_point_flag[3]=1
if have_nav_flag[3]==1:
navi.goto(goals[0])
break
else:
if both_id==1 and have_nav_flag[0]==0:
navi.goto(goals[1])
have_nav_flag[0]=1
if both_id==2 and have_nav_flag[0]==0:
navi.goto(goals[2])
have_nav_flag[0]=1
if both_id==3 and have_nav_flag[0]==0:
navi.goto(goals[3])
have_nav_flag[1]=1
if both_id==4 and have_nav_flag[0]==0:
navi.goto(goals[4])
have_nav_flag[1]=1
if both_id==5 and have_nav_flag[0]==0:
navi.goto(goals[5])
have_nav_flag[3]=1
if both_id==6 and have_nav_flag[0]==0:
navi.goto(goals[6])
have_nav_flag[3]=1
if both_id==7 and have_nav_flag[0]==0:
navi.goto(goals[7])
have_nav_flag[4]=1
if both_id==8 and have_nav_flag[0]==0:
navi.goto(goals[8])
have_nav_flag[4]=1