def make_data(N, num_landmarks, world_size, measurement_range, motion_noise,
measurement_noise, distance):
try:
check_for_data(num_landmarks, world_size, measurement_range,
motion_noise, measurement_noise)
except ValueError:
print(
"Error: You must implement the sense function in robot_class.py.")
return []
complete = False
r = robot(world_size, measurement_range, motion_noise, measurement_noise)
r.make_landmarks(num_landmarks)
while not complete:
data = []
seen = [False for row in range(num_landmarks)]
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
for k in range(N - 1):
Z = r.sense()
for i in range(len(Z)):
seen[Z[i][0]] = True
while not r.move(dx, dy):
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
data.append([Z, [dx, dy]])
complete = (sum(seen) == num_landmarks)
print(' ')
print('Landmarks: ', r.landmarks)
print(r)
return data
def check_for_data(num_landmarks, world_size, measurement_range, motion_noise,
measurement_noise):
r = robot(world_size, measurement_range, motion_noise, measurement_noise)
r.make_landmarks(num_landmarks)
sense_z = r.sense()
if (sense_z is None):
raise ValueError
def make_data(N, num_landmarks, world_size, measurement_range, motion_noise,
measurement_noise, distance):
"""
:param N: number of poses
:param num_landmarks:number of landmarks
:param world_size:size of 2D world
:param measurement_range:(float) Minimum distance to detect landmark
:param motion_noise:(float) noise (variance) of motion .
:param measurement_noise :noise(variance) of measurement
:param distance:(float) distance of one step.
:return :Nested list axis 0 --> time stamp each time stamp contains [measurements ,[dx,dy]]
"""
complete = False
r = robot(world_size, measurement_range, motion_noise, measurement_noise)
r.make_landmarks(num_landmarks)
while not complete:
data = []
seen = [False for row in range(num_landmarks)]
# guess an initial motion
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
for k in range(N - 1):
# collect sensor measurements in a list, Z
Z = r.sense()
# check off all landmarks that were observed
for i in range(len(Z)):
seen[Z[i][0]] = True
# move
while not r.move(dx, dy):
# if we'd be leaving the robot world, pick instead a new direction
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
# collect/memorize all sensor and motion data
data.append([Z, [dx, dy]])
# we are done when all landmarks were observed; otherwise re-run
complete = (sum(seen) == num_landmarks)
print(' ')
print('Landmarks: ', r.landmarks)
print(r)
return data
def make_data_3(N, num_landmarks, world_size, measurement_range, motion_noise,
measurement_noise, distance):
# check that data has been made
try:
check_for_data(num_landmarks, world_size, measurement_range,
motion_noise, measurement_noise)
except ValueError:
print(
'Error: You must implement the sense function in robot_class.py.')
return []
# print('Check done')
complete = False
r = robot(world_size, measurement_range, motion_noise, measurement_noise)
r.make_landmarks_non_rand(num_landmarks)
counter = 0
while not complete:
data = []
seen = [False for row in range(num_landmarks)]
# guess an initial motion
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
for k in range(N - 1):
# collect sensor measurements in a list, Z
Z = r.sense()
# print(Z)
# check off all landmarks that were observed
for i in range(len(Z)):
seen[Z[i][0]] = True
# move
while not r.move(dx, dy):
# if we'd be leaving the robot world, pick instead a new direction
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
# collect/memorize all sensor and motion data
data.append([Z, [dx, dy]])
# we are done when all landmarks were observed; otherwise re-run
complete = (sum(seen) == num_landmarks)
counter += 1
# print(counter, seen)
print(' ')
print('Landmarks: ', r.landmarks)
print(r)
return data
def check_for_data(num_landmarks, world_size, measurement_range, motion_noise, measurement_noise):
# make robot and landmarks
r = robot(world_size, measurement_range, motion_noise, measurement_noise)
r.make_landmarks(num_landmarks)
# check that sense has been implemented/data has been made
test_Z = r.sense()
if(test_Z is None):
raise ValueError
def make_data(N,
num_landmarks,
world_size,
measurement_range,
motion_noise,
measurement_noise,
distance,
landmarks=None):
# check if data has been made
complete = False
while not complete:
data = []
# make robot and landmarks
r = robot(world_size, measurement_range, motion_noise,
measurement_noise)
if landmarks is None:
r.make_landmarks(num_landmarks)
else:
r.landmarks = landmarks
seen = [False for row in range(num_landmarks)]
# guess an initial motion
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
for k in range(N - 1):
# collect sensor measurements in a list, Z
Z = r.sense()
# check off all landmarks that were observed
for i in range(len(Z)):
seen[Z[i][0]] = True
# move
while not r.move(dx, dy):
# if we'd be leaving the robot world, pick instead a new direction
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
# collect/memorize all sensor and motion data
data.append([Z, [dx, dy]])
# we are done when all landmarks were observed; otherwise re-run
complete = (sum(seen) == num_landmarks)
print(' ')
print('Landmarks: ', r.landmarks)
print(r)
return data, r
def __init__(self, n):
self.steps = 0
self.found = False
self.circ = False
self.skip = False
self.grid = [['.'] * matrixDim for x in range(matrixDim)]
self.robos = []
roboMatrixLen = int(round(math.sqrt(n)))
x, y = 1, 1
id = 1
for vert in range(x, roboMatrixLen + x):
for hori in range(y, roboMatrixLen + y):
robo = robot(id, hori, vert)
id += 1
self.robos.append(robo)
self.grid[vert][hori] = 'x'
extras = n - (roboMatrixLen *
roboMatrixLen) + 1 if self.skip else n - (roboMatrixLen *
roboMatrixLen)
for extra in range(extras):
robo = robot(id, extra + y, roboMatrixLen + x)
id += 1
self.robos.append(robo)
self.grid[roboMatrixLen + x][extra + y] = 'x'
from math import pi
import random
from robot_class import robot
myrobot = robot()
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
print(Z)
print(myrobot)
N = 1000
p = []
for _ in range(N):
x = robot()
x.set_noise(0.05, 0.05, 5.0)
p.append(x)
p2 = []
for i in range(N):
p2.append(p[i].move(0.1, 5.0))
p = p2
# Now we want to give weight to our
# particles. This program will print a
# list of 1000 particle weights.
w = []
W = 0
for i in range(N):
from robot_class import robot
i2c = busio.I2C(board.SCL, board.SDA)
vl53 = adafruit_vl53l0x.VL53L0X(i2c)
ser = serial.Serial('/dev/ttyS0',
38400,
parity=PARITY_NONE,
stopbits=STOPBITS_ONE,
bytesize=EIGHTBITS,
timeout=1) # open serial port
range = 650 #set IR range threshold (mm)
SumoBot = robot()
#Parse data??
#Sweep ring
#def button():
# SumoBot.getSensors()
# if SumoBot.bttn[1]:
# SumoBot.moveFwd()
from robot_class import robot, eval from math import * import random myrobot = robot() ##set a position of the robot #myrobot.set(10.0,10.0,0.0) #starts at coordinates 30, 50 heading north (pi/2). myrobot.set(30.0, 50.0, pi / 2) print(myrobot) print(myrobot.sense()) ##move the robot 10m forward #myrobot = myrobot.move(0.0,10.0) ##turn clockwise by pi/2, move 15 m, and sense myrobot = myrobot.move(-pi / 2, 15.0) print(myrobot) print(myrobot.sense()) ##turn the robot pi/2 further and move 10m forward myrobot = myrobot.move(-pi / 2, 10.0) print(myrobot) ##generate measurements. print(myrobot.sense())
def make_data(N, num_landmarks, world_size, measurement_range, motion_noise,
measurement_noise, distance):
# check that data has been made 这里是为了确认robot的sense()方法已经实现了
try:
check_for_data(num_landmarks, world_size, measurement_range,
motion_noise, measurement_noise)
except ValueError:
print(
'Error: You must implement the sense function in robot_class.py.')
return []
complete = False
r = robot(world_size, measurement_range, motion_noise,
measurement_noise) #实例化一个robor，初始位置为world的中心
r.make_landmarks(
num_landmarks
) #在world中随机建立num_landmarks个landmark，每个landmark的[x，y]都是整数(注：机器人坐标可为小数)，存储在r.landmarks中
while not complete:
data = []
seen = [False for row in range(num_landmarks)
] #先初始化seen为含num_landmarks个False的列表
# guess an initial motion
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
for k in range(
N - 1): #因为tN-1时不sense和move了，tN-1是最后一步有sense和move的，所以这里N要减掉1
# collect sensor measurements in a list, Z (注：都是在感知范围之内的landmark，而感知范围之外的landmark不会出现在Z中)
Z = r.sense(
) #Z为[index, dx, dy]的列表，index为landmark在r.landmarks中的下标，dx、dy为该landmark与当前机器人所在位置x、y方向的距离。
# check off all landmarks that were observed
for i in range(len(Z)):
seen[Z[i][
0]] = True #被感知的landmark在seen中对应设为True，剩下的num_landmarks-len(Z)个landmark对应位置依然保持False
# move 这里while循环的作用是：如果机器人移动超出了world，就重来一次；如果机器人移动没超出world，就直接跳出while循环。总而言之，就是
# 保证机器人做且仅做一次有效的移动。
while not r.move(
dx, dy
): #r.move(dx, dy)：当机器人移动dx、dy不超出world时，更新坐标，并返回True；当超出时，不更新坐标，且返回False。
# if we'd be leaving the robot world, pick instead a new direction
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
# collect/memorize all sensor and motion data
data.append([Z, [dx, dy]])
# we are done when all landmarks were observed; otherwise re-run
complete = (sum(seen) == num_landmarks
) #在N个时间步中完成观测到所有的landmark才算完成任务，否则重新一遍N个时间步。
print(' ')
print('Landmarks: ', r.landmarks)
print(r)
return data #data中总共有N-1个[Z, [dx, dy]]
print depth
time.sleep(1)
pub_data = [1, 0, 0, 0, 0, 0]
m = Int16MultiArray(data=pub_data)
rospy.loginfo(pub_data)
pub.publish(m)
ack_listener()
return vals
N = 250
particles = []
img1 = mapimage.copy()
for i in range(0, N):
particles.append(robot())
cv2.circle(img1, (particles[i].x, particles[i].y), 2, [0, 255, 0], -1)
print "created particles"
cv2.imshow('window', img1)
cv2.waitKey(0)
while mainloop < 2 or particles.length == 0:
vals = senseWorld()
print vals
weights = []
count = 0
img2 = mapimage.copy()
for particle in particles:
sum = 0.0
for i in range(len(p)): # calculate mean error
dx = (p[i].x - r.x +
(world_size / 2.0)) % world_size - (world_size / 2.0)
dy = (p[i].y - r.y +
(world_size / 2.0)) % world_size - (world_size / 2.0)
err = sqrt(dx * dx + dy * dy)
sum += err
return sum / float(len(p))
## MAIN =======================================================================
if __name__ == "__main__":
N = 1000 # number of particles
localizing_robot = rc.robot(world_size, landmarks)
# initialize N random particles
particles = []
x_plot = []
y_plot = []
for i in range(N):
part = rc.robot(world_size, landmarks)
sensor_noise = 3.0
part.set_noise(0.05, 0.05, sensor_noise)
particles.append(part)
x_plot.append(part.x)
y_plot.append(part.y)
# Setup for plotting
robot_x = []
if level==2: mines=3 while mines!=0: c=[n for n in [random.randrange(1,99,1),random.randrange(1,29,1)] if n not in pos1] myscr.addch(c[1],c[0],'M') pos1.append(c) mines-=1 while(codes!=0): c=[n for n in [random.randrange(1,99,1),random.randrange(1,29,1)] if n not in pos1] myscr.addch(c[1],c[0],'C',curses.color_pair(4)) pos1.append(c) codes-=1 c=[n for n in [random.randrange(1,99,1),random.randrange(1,29,1)] if n not in pos1] myscr.addch(c[1],c[0],'B') if level==1: robo=robot_class.robot(myscr) elif level==2: robo=robot1(myscr) # x=robo.check_code() # y=robo.check_bomb() # if x!=0: # if y==0: # break while key!=27: flag=0 for_score=robo.check_code(myscr) myscr.addstr(0,2,"ScOrE: " + str((4-(for_score))*10)) myscr.addstr(0,15,"DiFuSe cOdEs LeFt: " + str(for_score)) # x=robo.check(myscr) x=robo.check(myscr)
def make_data(N, num_landmarks, world_size, measurement_range, motion_noise,
measurement_noise, distance):
# N = number of robot positions resp. motion steps
# num_landmarks = number of landmarks in the 2D world
# world_size = size of the squared 2D world
# measurement_range = maximum measurement / visibility range of the robot to perceive landmarks
# motion_noise = motion measurement noise bandwidth
# measurement_noise = landmark measurement noise bandwidth
# distance = distance travelled per motion step
# check that data has been made
try:
check_for_data(num_landmarks, world_size, measurement_range,
motion_noise, measurement_noise)
except ValueError:
print(
'Error: You must implement the sense function in robot_class.py.')
return []
complete = False
# initialize 2D world with a robot at its center coordinates
r = robot(world_size, measurement_range, motion_noise, measurement_noise)
# create random landmarks within the 2D world of the robot
r.make_landmarks(num_landmarks)
# get true landmark positions
true_landmarks = r.landmarks
while not complete:
data = []
seen = [False for row in range(num_landmarks)]
# guess an initial motion
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
for k in range(N - 1):
# collect sensor measurements in a list, Z
Z = r.sense()
# check off all landmarks that were observed
for i in range(len(Z)):
seen[Z[i][0]] = True
# move (returns False when leaving the robot's 2D world)
while not r.move(dx, dy):
# if we'd be leaving the robot world, pick instead a new direction
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
# collect/memorize all sensor and motion data
data.append([Z, [dx, dy]])
# we are done when all landmarks were observed; otherwise re-run
complete = (sum(seen) == num_landmarks)
# get true robot positions (without motion noise) for reference
true_positions = r.true_positions
print(' ')
print('Landmarks: ', r.landmarks)
print(r)
# return noise-affected measurement data containing [estimted_landmarks, estimated_position]
# for each motion step plus true landmark positions and true robot positions for reference
return data, true_landmarks, true_positions
