def map_run():
global feedbackData, scanData, mapData, goalData, PATH, nowPoint
start = time.time()
while not rospy.is_shutdown():
if feedbackData == Pose() or scanData == [] or goalData == MyPoint():
continue
if time.time() - start < 10:
if feedbackData.position.x == PATH[nowPoint].point.position.x and feedbackData.position.y == PATH[nowPoint].point.position.y:
if nowPoint + 1 == len(PATH):
continue
elif nowPoint + 2 == len(PATH):
nowPoint += 1
ret = MyPoint()
ret.x = PATH[nowPoint].point.position.x
ret.y = PATH[nowPoint].point.position.y
ret.z = goalData.z
ret.say = goalData.say
goalPoint_pub.publish(ret)
else:
nowPoint += 1
ret = MyPoint()
ret.x = PATH[nowPoint].point.position.x
ret.y = PATH[nowPoint].point.position.y
ret.z = euler_angles[PATH[nowPoint].dir]
ret.say = ""
goalPoint_pub.publish(ret)
def my_move_base():
global feedbackData, scanData, goalPointData, say_key, goalSize, mapData, pointSize, width, height, Startx, Starty, moveKey
say_ser("I'm OK!")
while not rospy.is_shutdown():
if feedbackData == Pose() or scanData == [] or goalPointData == MyPoint() or mapData == [] or moveKey is not 0:
continue
continueKey = True
# 如果已经走到目标点, 调整朝向
if (feedbackData.position.x - goalPointData.x) ** 2 + (feedbackData.position.y - goalPointData.y) ** 2 < goalSize:
goalSize = 0.25
quaternion = (feedbackData.orientation.x, feedbackData.orientation.y, feedbackData.orientation.z, feedbackData.orientation.w)
z = euler_from_quaternion(quaternion, axes='sxyz')
if z[2] > math.pi:
z[2] %= math.pi
z[2] -= 2 * math.pi
elif theta < 0 - math.pi:
z[2] %= math.pi
z[2] += 2 * math.pi
#print z[2], goalPointData.z
theta = z[2] - goalPointData.z
if theta > math.pi:
theta %= math.pi
theta -= 2 * math.pi
elif theta < 0 - math.pi:
theta %= math.pi
theta += 2 * math.pi
#print theta
speed = theta * 1000
if speed < 0:
speed = max(speed, -333)
else:
speed = min(speed, 333)
if theta < 0 - 0.10:
motor_ser(0, 0, 0-speed)
elif theta > 0.10:
motor_ser(0, 0, 0-speed)
elif say_key:
motor_ser(0, 0, 0)
say_ser(goalPointData.say)
arrive_pub.publish(1)
say_key = False
else:
motor_ser(0, 0, 0)
continueKey = False
continue
if continueKey:
# 更新目标点的相对坐标
nowx = goalPointData.x - feedbackData.position.x
nowy = goalPointData.y - feedbackData.position.y
theta = 0
if math.fabs(nowx) < 0.01:
if nowy < 0:
theta = 0 - math.pi / 2
else:
theta = math.pi / 2
elif nowx > 0:
theta = math.atan(nowy / nowx)
else:
if nowy > 0:
theta = math.atan(nowy / nowx) + math.pi
else:
theta = math.atan(nowy / nowx) - math.pi
quaternion = (feedbackData.orientation.x, feedbackData.orientation.y, feedbackData.orientation.z, feedbackData.orientation.w)
z = euler_from_quaternion(quaternion, axes='sxyz')
theta = theta - z[2]
if theta > math.pi:
theta %= math.pi
theta -= 2 *math.pi
elif theta < 0 - math.pi:
theta %= math.pi
theta += 2 * math.pi
# 调整朝向为目标点方向
#print theta
speed = theta * 1000
if speed < 0:
speed = max(speed, -333)
else:
speed = min(speed, 333)
if True:
flag = True
while flag:
i = 0
flag = False
nowx = goalPointData.x - feedbackData.position.x
nowy = goalPointData.y - feedbackData.position.y
Nowx = Startx + int(feedbackData.orientation.x / pointSize)
Nowy = Starty + int(feedbackData.orientation.y / pointSize)
for it in scanData:
if 140 < i < 400:
if 0.09 < it < 0.5 and mapData[int(Nowy + it * math.sin((i - 270) * 0.00999999977648 + z[2]) * width / pointSize) + int(Nowx + it * math.cos((i-270) * 0.00999999977648 + z[2]) / pointSize)] != 100:
#print mapData[int(Nowy + it * math.cos((i - 270) * 0.00999999977648)) * width + int(Nowx + it * math.sin((i-270) * 0.00999999977648))], it, i
flag = True
if i < 270:
motor_ser(0, 250, 0)
else:
motor_ser(0, 0 - 250, 0)
break
elif 0.09 < it < 0.30:
flag = True
if nowy > 0:
motor_ser(0, 250, 0)
else:
motor_ser(0, 0 - 250, 0)
break
elif 90 < i < 220:
if 0.09 < it < 0.35:
flag = True
motor_ser(250, 250, speed / 2)
break
elif 320 < i < 450:
if 0.09 < it < 0.35:
flag = True
motor_ser(250, 0 - 250, speed / 2)
break
i += 1
if theta < 0 - 0.05:
motor_ser(500, 0, speed)
#print speed
elif theta > 0.03:
motor_ser(500, 0, speed)
#print speed
else:
motor_ser(500, 0, 0)
rospy.sleep(0.05)
motor_ser(500, 0, speed)
#print speed
else:
motor_ser(500, 0, 0)
rospy.sleep(0.05)
if __name__ == "__main__":
rospy.init_node("my_move_base")
motor_ser = rospy.ServiceProxy("/hc_motor_cmd/vector_speed", VectorSpeed)
say_ser = rospy.ServiceProxy("AudioPlay/TTS", say)
feedbackData = Pose()
scanData = list()
goalPointData = MyPoint()
goalSize = 0.01
mapData = []
moveKey = 0
arrive_pub = rospy.Publisher("/arrive", Arr, queue_size=10)
map_pub = rospy.Subscriber("/map_map", OccupancyGrid, updata_mapData)
move_base_feedback_pub = rospy.Subscriber("/goalCoords", Pose, update_feedbackData)
scan_pub = rospy.Subscriber("/scan", LaserScan, update_scanData)
goalPoint_pub = rospy.Subscriber("/my_move_base/goalPoint", MyPoint, update_goalPointData)
moveKey_pub = rospy.Subscriber("/StartFollow", sf, update_moveKey)
#say_ser("I can start!")
my_move_base()
def map_run():
global feedbackData, goalData, PATH, nowPoint, getKey, pointSize, Start
start = time.time()
while not rospy.is_shutdown():
if feedbackData == Pose() or goalData == MyPoint() or getKey:
continue
if time.time() - start < 100:
#print Now.x, Now.y, nowPoint, PATH[nowPoint].x, PATH[nowPoint].y
if abs(Now.x - PATH[nowPoint].x) < 3 and abs(Now.y - PATH[nowPoint].y) < 3:
if nowPoint + 1 == len(PATH):
continue
elif nowPoint + 2 == len(PATH):
nowPoint += 1
ret = MyPoint()
ret.x = (PATH[nowPoint].x - Start.x) * pointSize
ret.y = (PATH[nowPoint].y - Start.y) * pointSize
ret.z = goalData.z
ret.say = goalData.say
goalPoint_pub.publish(ret)
else:
nowPoint += 1
ret = MyPoint()
ret.x = (PATH[nowPoint].x - Start.x) * pointSize
ret.y = (PATH[nowPoint].y - Start.y) * pointSize
ret.z = euler_angles[PATH[nowPoint].ddir]
ret.say = ""
goalPoint_pub.publish(ret)
else:
nowPoint += 1
ret = MyPoint()
ret.x = (PATH[nowPoint].x - Start.x) * pointSize
ret.y = (PATH[nowPoint].y - Start.y) * pointSize
ret.z = goalData.z
ret.say = goalData.say
goalPoint_pub.publish(ret)
else:
start = time.time()
if getKey:
continue
pathLen = getPath()
while getKey:
print "continue2"
continue
if pathLen == 0:
print "i can't get there"
elif pathLen == 1:
ret = MyPoint()
ret.x = (PATH[nowPoint].x - Start.x) * pointSize
ret.y = (PATH[nowPoint].y - Start.y) * pointSize
ret.z = goalData.z
ret.say = goalData.say
goalPoint_pub.publish(ret)
else :
ret = MyPoint()
ret.x = (PATH[nowPoint].x - Start.x) * pointSize
ret.y = (PATH[nowPoint].y - Start.y) * pointSize
ret.z = euler_angles[PATH[nowPoint].ddir]
ret.say = ""
goalPoint_pub.publish(ret)
ret = MyPoint()
ret.x = (PATH[nowPoint].x - Start.x) * pointSize
ret.y = (PATH[nowPoint].y - Start.y) * pointSize
ret.z = euler_angles[PATH[nowPoint].ddir]
ret.say = ""
goalPoint_pub.publish(ret)
if __name__ == "__main__":
rospy.init_node("my_map")
goalPoint_pub = rospy.Publisher("/my_move_base/goalPoint", MyPoint, queue_size=10)
# feedbackData, goalData, scanData 单位是m
feedbackData = Pose()
goalData = MyPoint()
scanData = []
# Now, Start, Goal 存对应mapData中的下标
Now = tpoint()
Start = tpoint()
Goal = tpoint()
# mapDataInit 一个格代表0.025m
mapDataInit = []
pointSize = 0
getKey = False
nowPoint = 0
move_base_feedback_pub = rospy.Subscriber("/tf", tfMessage, update_feedbackData)
map_pub = rospy.Subscriber("/map", OccupancyGrid, update_mapData)
goal_pub = rospy.Subscriber("/my_map/goalPoint", MyPoint, update_goalData)
def map_run():
global feedbackData, scanData, mapData, goalData, PATH, nowPoint, getKey
start = time.time() - 100
while not rospy.is_shutdown():
if feedbackData == Pose() or scanData == [] or goalData == MyPoint():
continue
if time.time() - start < 100:
if abs(feedbackData.position.x - (PATH[nowPoint].x - Start.x) / 10.0) < 0.055 and abs(feedbackData.position.y - (PATH[nowPoint].y - Start.y) / 10.0) < 0.055:
if nowPoint + 1 == len(PATH):
continue
elif nowPoint + 2 == len(PATH):
nowPoint += 1
ret = MyPoint()
ret.x = (PATH[nowPoint].x - Start.x) / 10.0
ret.y = (PATH[nowPoint].y - Start.y) / 10.0
ret.z = goalData.z
ret.say = goalData.say
goalPoint_pub.publish(ret)
else:
nowPoint += 1
ret = MyPoint()
ret.x = (PATH[nowPoint].x - Start.x) / 10.0
ret.y = (PATH[nowPoint].y - Start.y) / 10.0
ret.z = euler_angles[PATH[nowPoint].ddir]
ret.say = ""
goalPoint_pub.publish(ret)
else:
while getKey:
pass
nowPoint = 0
ret = MyPoint()
ret.x = (PATH[nowPoint].x - Start.x) / 10.0
ret.y = (PATH[nowPoint].y - Start.y) / 10.0
ret.z = euler_angles[PATH[nowPoint].ddir]
ret.say = ""
goalPoint_pub.publish(ret)
start = time.time()
