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()