class SpacehawksHokuyoLXWrapper:
def __init__():
self.lidar = HokuyoLX()
def get_dists():
return self.lidar.get_filtered_dist().tolist()
def get_intens():
return self.lidar.get_filtered_intens().tolist()
class Wrapper:
def __init__(self):
self.lidar = HokuyoLX()
def get_dists(self):
return self.lidar.get_filtered_dist()[1].tolist()
def get_intens(self):
return self.lidar.get_filtered_intens()[1].tolist()
def run():
plt.ion()
laser = HokuyoLX()
ax = plt.subplot(111, projection='polar')
plot = ax.plot([], [], '.')[0]
text = plt.text(0, 1, '', transform=ax.transAxes)
ax.set_rmax(DMAX)
ax.grid(True)
plt.show()
while plt.get_fignums():
update(laser, plot, text)
laser.close()
def run():
plt.ion()
laser = HokuyoLX()
ax = plt.subplot(111, projection='polar')
plot = ax.plot([], [], '.')[0]
text = plt.text(0, 1, '', transform=ax.transAxes)
ax.set_rmax(DMAX)
ax.grid(True)
plt.show()
while plt.get_fignums():
update(laser, plot, text)
laser.close()
def run():
plt.ion()
laser = HokuyoLX()
ax = plt.subplot(111, projection='polar')
plot = ax.scatter([0, 1], [0, 1], s=5, c=[IMIN, IMAX], cmap=plt.cm.Greys_r, lw=0)
text = plt.text(0, 1, '', transform=ax.transAxes)
ax.set_rmax(DMAX)
ax.grid(True)
plt.show()
while plt.get_fignums():
update(laser, plot, text)
laser.close()
def run():
plt.ion()
laser = HokuyoLX(tsync = False)
laser.convert_time = False
ax = plt.subplot(111, projection='polar')
plot = ax.scatter([0, 1], [0, 1], s=5, c=[IMIN, IMAX], cmap=plt.cm.Reds, lw=0)
text = plt.text(0, 1, '', transform=ax.transAxes)
ax.set_rmax(DMAX)
ax.grid(True)
plt.show()
while plt.get_fignums():
update(laser, plot, text)
laser.close()
def run():
plt.ion()
laser = HokuyoLX(tsync=False)
laser.convert_time = False
ax = plt.subplot(111, projection='polar')
plot = ax.scatter([0, 1], [0, 1], s=10, c='b', lw=0)
text = plt.text(0, 1, '', transform=ax.transAxes)
ax.set_rmax(DMAX)
ax.grid(True)
images.append((plot, ))
plt.show()
while plt.get_fignums():
update(laser, plot, text)
laser.close()
class LIDAR:
def __init__(self):
self.lidar = HokuyoLX()
self.kalman = KalmanFilter(adaptive=False, dt=0.025)
def scan(self) -> list[tuple[float, float, float]]:
return self.lidar.get_filtered_intens()[1].tolist()
class LIDAR:
def __init__(self, ip="192.168.1.10", port=10940):
self.lidar = HokuyoLX(addr=(ip, port))
self.kalman = KalmanFilter(adaptive=False, dt=0.025)
def scan(self):
return self.lidar.get_filtered_intens()[1].tolist()
def generateSampleDist():
lidar = HokuyoLX()
_, data = lidar.get_filtered_dist()
data = data.tolist()
fileName = input("Enter a file name: ")
with open(str(fileName) + ".txt", 'w') as pw:
for i in range(len(data)):
pw.write(str(data[i][0]) + "," + str(data[i][1]))
if i != len(data) - 1:
pw.write("\n")
pw.close()
lidar.close()
def __init__(self, lidar_on=True,small=True):
sensors_number=6
self.sensor_range = 20
self.collision_avoidance = False
if small:
self.sensors_map = {0:(0, np.pi/3),1: (np.pi*0.5, np.pi*1.5),2: (5/3.*np.pi,2*np.pi),3: [(7/4.*np.pi,2*np.pi),(0,np.pi*1/4.)]}
#self.sensors_map= {0: (0, np.pi/3), 1: (np.pi/4, np.pi*7/12), 2: (np.pi*0.5, np.pi*1.5), 3: (17/12.*np.pi, 7/4.*np.pi), 4: (5/3.*np.pi,2*np.pi), 5: [(7/4.*np.pi,2*np.pi),(0,np.pi*1/4.)]} # can be problem with 2pi and 0
self.lidar_on = lidar_on
self.map = np.load('npmap.npy')
if lidar_on:
logging.debug('lidar is connected')
# add check for lidar connection
try:
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
except:
self.lidar_on = False
logging.warning('lidar is not connected')
#self.x = 170 # mm
#self.y = 150 # mm
#self.angle = 0.0 # pi
if small:
self.coords = Array('d',[850, 170, 3*np.pi / 2])
else:
self.coords = Array('d', [170, 170, 0])
self.localisation = Value('b', True)
self.input_queue = Queue()
self.loc_queue = Queue()
self.fsm_queue = Queue()
self.PF = pf.ParticleFilter(particles=1500, sense_noise=25, distance_noise=45, angle_noise=0.2, in_x=self.coords[0],
in_y=self.coords[1], in_angle=self.coords[2],input_queue=self.input_queue, out_queue=self.loc_queue)
# driver process
self.dr = driver.Driver(self.input_queue,self.fsm_queue,self.loc_queue)
self.p = Process(target=self.dr.run)
self.p.start()
self.p2 = Process(target=self.PF.localisation,args=(self.localisation,self.coords,self.get_raw_lidar))
logging.info(self.send_command('echo','ECHO'))
logging.info(self.send_command('setCoordinates',[self.coords[0] / 1000., self.coords[1] / 1000., self.coords[2]]))
self.p2.start()
time.sleep(0.1)
class robot():
#lidar,x,y
def __init__(self):
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
self.x = 0
self.y = 0
def update_pos(self):
timestamp, scan = self.lidar.get_intens()
beacons = get_beacons(scan)
circle1 = (field_x, field_y, beacons[0][1])
circle2 = (0, field_y, beacons[1][1])
print(circle1)
print(circle2)
answer = circle_intersection(circle1, circle2)
if (answer[0][0] > 0 and answer[0][0] < 2000 and answer[0][1] > 0
and answer[0][1] < 3000):
return answer[0]
else:
return answer[1]
parser.add_argument("-l", "--log", help="Print debug info ")
parser.add_argument("-m", "--map", help="Send osc msg in map mode")
args = parser.parse_args()
if args.log == 'ON':
debug_ON = True
if args.map == 'OFF':
map_ON = False
# Start the system.
osc_startup()
# Make client channels to send packets.
osc_udp_client(osc_host_ip, int(osc_host_port), "osc")
try:
laser = HokuyoLX(addr=(str(sensor_ip), int(sensor_port)),
info=False,
buf=1024,
timeout=300,
time_tolerance=1000,
convert_time=False)
print("Connect " + str(sensor_ip) + "\n")
except Exception as e:
print(e)
print("[ERR]Sensor connect failed " + time.strftime("%X") + "@" +
sensor_ip + "\n")
errFlag = True
while True:
update()
def __init__(self,
lidar_on=True,
color='yellow',
sen_noise=20,
angle_noise=0.2,
dist_noise=45,
init_coord=[170, 170, 0]):
self.color = color
# collision settings
self.collision_avoidance = True
self.sensor_range = 60
self.map = np.load('npmap.npy')
self.sensors_places = [
np.pi / 2, np.pi / 2, 0, np.pi, 3 * np.pi / 2, 3 * np.pi / 2, 0,
np.pi, 0, 0
] # DIRECTION OF SENSORS
self.sensors_map = {
0: (np.pi / 6, np.pi / 2 + np.pi / 3),
1: (np.pi / 2 - np.pi / 3, np.pi * 5 / 6),
2: (0.0, 0.0 + np.pi / 3),
3: (np.pi - np.pi / 3, np.pi + np.pi / 3),
4: (3 * np.pi / 2 - np.pi / 3, 11 * np.pi / 6),
5: (np.pi - np.pi / 6, 3 * np.pi / 2 + np.pi / 3),
6: (0.0, 0.0 + np.pi / 3),
7: (np.pi - np.pi / 3, np.pi + np.pi / 3), # 8 IR sensors
8: (2 * np.pi - np.pi / 3, 2 * np.pi),
9: (2 * np.pi - np.pi / 3, 2 * np.pi)
} # doubling values
#6: (3*np.pi/2 + np.pi/4, 2*np.pi), 7:(np.pi/2-np.pi/3, np.pi*5/6): (np.pi)# 6 ir sensors ENDED
#7:(0, np.pi/4), 8:(3*np.pi/2 + np.pi/4, 2*np.pi), 9:(np.pi - np.pi/3, np.pi + np.pi/3)} # 2 us sensors
#can be problems ith 2*np.pi and 0
self.lidar_on = lidar_on
self.collision_d = 9
self.coll_go = False
# localisation settings
self.localisation = Value('b', True)
if lidar_on:
logging.debug('lidar is connected')
# add check for lidar connection
try:
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
except:
self.lidar_on = False
self.localisation = Value('b', False)
logging.warning('lidar is not connected')
#self.x = 170 # mm
#self.y = 150 # mm
#self.angle = 0.0 # pi
driver.PORT_SNR = '325936843235' # need change
# for test
x1, x2, y1, y2 = 1000, 2500, 600, 1100
dx, dy = x2 - x1, y2 - y1
angle = np.pi / 2
start = [x1, y1, angle]
#
self.coords = Array('d', rev_field(init_coord, self.color)) # 170, 170
#self.coords = Array('d',rev_field([1000, 1100, np.pi],self.color))
self.input_queue = Queue()
self.loc_queue = Queue()
self.fsm_queue = Queue()
self.PF = pf.ParticleFilter(particles=1500,
sense_noise=sen_noise,
distance_noise=dist_noise,
angle_noise=angle_noise,
in_x=self.coords[0],
in_y=self.coords[1],
in_angle=self.coords[2],
input_queue=self.input_queue,
out_queue=self.loc_queue,
color=self.color)
# driver process
self.dr = driver.Driver(self.input_queue,
self.fsm_queue,
self.loc_queue,
device=get_device_name())
self.p = Process(target=self.dr.run)
self.p.start()
self.p2 = Process(target=self.PF.localisation,
args=(self.localisation, self.coords,
self.get_raw_lidar))
logging.info(self.send_command('echo', 'ECHO'))
logging.info(
self.send_command('setCoordinates', [
self.coords[0] / 1000., self.coords[1] / 1000., self.coords[2]
]))
self.p2.start()
self.init_manipulators()
time.sleep(0.1)
logging.info("Robot __init__ done!")
class Robot:
def __init__(self,
lidar_on=True,
color='yellow',
sen_noise=20,
angle_noise=0.2,
dist_noise=45,
init_coord=[170, 170, 0]):
self.color = color
# collision settings
self.collision_avoidance = True
self.sensor_range = 60
self.map = np.load('npmap.npy')
self.sensors_places = [
np.pi / 2, np.pi / 2, 0, np.pi, 3 * np.pi / 2, 3 * np.pi / 2, 0,
np.pi, 0, 0
] # DIRECTION OF SENSORS
self.sensors_map = {
0: (np.pi / 6, np.pi / 2 + np.pi / 3),
1: (np.pi / 2 - np.pi / 3, np.pi * 5 / 6),
2: (0.0, 0.0 + np.pi / 3),
3: (np.pi - np.pi / 3, np.pi + np.pi / 3),
4: (3 * np.pi / 2 - np.pi / 3, 11 * np.pi / 6),
5: (np.pi - np.pi / 6, 3 * np.pi / 2 + np.pi / 3),
6: (0.0, 0.0 + np.pi / 3),
7: (np.pi - np.pi / 3, np.pi + np.pi / 3), # 8 IR sensors
8: (2 * np.pi - np.pi / 3, 2 * np.pi),
9: (2 * np.pi - np.pi / 3, 2 * np.pi)
} # doubling values
#6: (3*np.pi/2 + np.pi/4, 2*np.pi), 7:(np.pi/2-np.pi/3, np.pi*5/6): (np.pi)# 6 ir sensors ENDED
#7:(0, np.pi/4), 8:(3*np.pi/2 + np.pi/4, 2*np.pi), 9:(np.pi - np.pi/3, np.pi + np.pi/3)} # 2 us sensors
#can be problems ith 2*np.pi and 0
self.lidar_on = lidar_on
self.collision_d = 9
self.coll_go = False
# localisation settings
self.localisation = Value('b', True)
if lidar_on:
logging.debug('lidar is connected')
# add check for lidar connection
try:
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
except:
self.lidar_on = False
self.localisation = Value('b', False)
logging.warning('lidar is not connected')
#self.x = 170 # mm
#self.y = 150 # mm
#self.angle = 0.0 # pi
driver.PORT_SNR = '325936843235' # need change
# for test
x1, x2, y1, y2 = 1000, 2500, 600, 1100
dx, dy = x2 - x1, y2 - y1
angle = np.pi / 2
start = [x1, y1, angle]
#
self.coords = Array('d', rev_field(init_coord, self.color)) # 170, 170
#self.coords = Array('d',rev_field([1000, 1100, np.pi],self.color))
self.input_queue = Queue()
self.loc_queue = Queue()
self.fsm_queue = Queue()
self.PF = pf.ParticleFilter(particles=1500,
sense_noise=sen_noise,
distance_noise=dist_noise,
angle_noise=angle_noise,
in_x=self.coords[0],
in_y=self.coords[1],
in_angle=self.coords[2],
input_queue=self.input_queue,
out_queue=self.loc_queue,
color=self.color)
# driver process
self.dr = driver.Driver(self.input_queue,
self.fsm_queue,
self.loc_queue,
device=get_device_name())
self.p = Process(target=self.dr.run)
self.p.start()
self.p2 = Process(target=self.PF.localisation,
args=(self.localisation, self.coords,
self.get_raw_lidar))
logging.info(self.send_command('echo', 'ECHO'))
logging.info(
self.send_command('setCoordinates', [
self.coords[0] / 1000., self.coords[1] / 1000., self.coords[2]
]))
self.p2.start()
self.init_manipulators()
time.sleep(0.1)
logging.info("Robot __init__ done!")
def send_command(self, name, params=None):
self.input_queue.put({'source': 'fsm', 'cmd': name, 'params': params})
return self.fsm_queue.get()
def is_start(self):
return self.send_command('start_flag')['data']
def get_raw_lidar(self):
# return np.load('scan.npy')[::-1]
timestamp, scan = self.lidar.get_intens()
return scan
# return scan[::-1] our robot(old)
def check_lidar(self):
try:
state = self.lidar.laser_state()
except:
self.lidar_on = False
logging.warning('Lidar off')
def go_to_coord_rotation(self, parameters):
# beta version of clever go_to
direct_random = [np.pi, np.pi / 2, -np.pi / 2]
distance = 200
# gomologization version and change timer in go_to
self.coll_go = False
ok = self.go_to(parameters)
#return
#
#ok = self.go_to(parameters)
while not ok:
logging.critical("Collision, go back")
angle = (self.coords[2] + self.sensors_places[self.coll_ind] +
random.choice(direct_random)) % (np.pi * 2)
direction = (np.cos(angle), np.sin(angle))
pm = [
self.coords[0] + direction[0] * distance,
self.coords[1] + direction[1] * distance, self.coords[2],
parameters[3]
]
self.go_to(pm)
logging.critical("go to correct")
self.coll_go = True
ok = self.go_to(parameters)
self.coll_go = False
def go_to(self, parameters): # parameters [x,y,angle,speed]
parameters = rev_field(parameters, self.color)
if self.PF.warning:
time.sleep(1)
pm = [
self.coords[0] / 1000., self.coords[1] / 1000.,
float(self.coords[2]), parameters[0] / 1000.,
parameters[1] / 1000.,
float(parameters[2]), parameters[3]
]
x = parameters[0] - self.coords[0]
y = parameters[1] - self.coords[1]
tm = 90 # intstead of 7, to turn off bypass logic after 7 seconds collision occurance
if self.coll_go == True:
tm = 1
logging.info("Go to coordinates: " +
str(parameters[:2] + [np.rad2deg(parameters[2])] +
[parameters[3]]))
logging.info(self.send_command('go_to_with_corrections', pm))
#self.PF.debug_info = [time.time() - self.PF.start_time, parameters[:2]
# +[np.rad2deg(parameters[2])] , [0]]
# After movement
stamp = time.time()
pids = True
time.sleep(
0.100001
) # sleep because of STM interruptions (Maybe add force interrupt in STM)
while not self.send_command('is_point_was_reached')['data']:
time.sleep(0.05)
if self.collision_avoidance:
direction = (float(x), float(y))
while self.check_collisions(direction):
logging.critical("Collision occured!")
if pids:
self.send_command('stopAllMotors')
pids = False
time.sleep(0.5 / 5)
if (time.time() - stamp) > tm:
self.send_command('cleanPointsStack')
cur = [
self.coords[0] / 1000., self.coords[1] / 1000.,
float(self.coords[2])
]
self.send_command('setCoordinates', cur)
logging.info(self.send_command('switchOnPid'))
return False
if not pids:
logging.critical("After collision. GO!")
pids = True
self.send_command('cleanPointsStack')
cur = [
self.coords[0] / 1000., self.coords[1] / 1000.,
float(self.coords[2])
]
self.send_command('setCoordinates', cur)
logging.info(self.send_command('switchOnPid'))
pm[0] = cur[0]
pm[1] = cur[1]
pm[2] = cur[2]
logging.info(
self.send_command('go_to_with_corrections', pm))
time.sleep(0.10000001)
#return False
# add Collision Avoidance there
if self.localisation.value == 0:
self.PF.move_particles([
parameters[0] - self.coords[0], parameters[1] - self.coords[1],
parameters[2] - self.coords[2]
])
self.coords[0] = parameters[0]
self.coords[1] = parameters[1]
self.coords[2] = parameters[2]
logging.info('point reached')
return True
def check_collisions(self, direction):
angle = np.arctan2(direction[1], direction[0]) % (np.pi * 2)
sensor_angle = (angle - self.coords[2]) % (np.pi * 2)
#### switch on sensor_angle
collisions = self.sensor_data()
for index, i in enumerate(collisions):
if (i == True and sensor_angle <= self.sensors_map[index][1]
and sensor_angle >= self.sensors_map[index][0]):
logging.info("Collision at index " + str(index))
angle_map = (self.coords[2] +
self.sensors_places[index]) % (np.pi * 2)
#if self.check_map2(angle_map):
# continue
self.coll_ind = index
return True
return False
def receive_sensors_data(self):
data = self.send_command('sensors_data')['data']
answer = []
for i in range(6):
answer.append((data & (1 << i)) != 0)
return answer
def sensor_data(self):
data = self.send_command('ir_sensors')['data']
#data_us = self.send_command('us_sensors')['data']
#logging.info("Collision data len " + str(len(data)))
#data_us1 = [data_us[0] < UD_THRESHOLD, data_us[1] < UD_THRESHOLD]
data += [data[2], data[6]]
logging.info(data)
#data.append(data_us1)
return data
def check_map(self, direction): # probably can be optimized However O(1)
direction = (direction[0] / np.sum(np.abs(direction)),
direction[1] / np.sum(np.abs(direction)))
for i in range(0, self.sensor_range, 2):
for dx in range(-8, 8):
x = int(self.coords[0] / 10 + direction[0] * i + dx)
y = int(self.coords[1] / 10 + direction[1] * i)
if x >= pf.WORLD_X / 10 or x < 0 or y >= pf.WORLD_Y / 10 or y < 0:
return True
# Or maybe Continue
if self.map[x][y]:
return True
return False
def check_map2(self, angle):
direction = (np.cos(angle), np.sin(angle))
for i in range(0, self.sensor_range, 2):
for dx in range(-self.collision_d, self.collision_d):
x = int(self.coords[0] / 10 + direction[0] * i + dx)
y = int(self.coords[1] / 10 + direction[1] * i)
#logging.info("x = "+str(x)+" y = " + str(y))
if x >= pf.WORLD_X / 10 or x < 0 or y >= pf.WORLD_Y / 10 or y < 0:
return True
# Or maybe Continue
if self.map[x][y]:
return True
return False
def go_last(self, parameters, dur=None):
tmstmp = None
if dur is not None:
tmstmp = time.time()
while abs(parameters[0] -
self.coords[0]) > 10 or abs(parameters[1] -
self.coords[1]) > 10:
logging.info('Calibrate')
self.go_to_coord_rotation(parameters)
if tmstmp is not None and time.time() - tmstmp > dur:
break
##########################################################
################# BIG Robot ############################
##########################################################
def left_ball_down(self, dur=1, angle=angle_left_down, speed=100.0):
self.collision_avoidance = False
self.send_command('left_ball_down', [angle, speed])
logging.info("left_ball_down")
time.sleep(dur)
#self.collision_avoidance = True
def left_ball_up(self, dur=1, angle=angle_left_up, speed=100.0):
self.collision_avoidance = False
self.send_command('left_ball_up', [angle, speed])
logging.info("left_ball_up")
time.sleep(dur)
#self.collision_avoidance = True
def left_ball_drop(self,
dur=1,
angle=angle_left_up + 33. + 5.,
speed=100.0):
self.collision_avoidance = False
self.send_command('left_ball_drop', [angle, speed])
logging.info("left_ball_drop")
time.sleep(dur)
#self.collision_avoidance = True
def right_ball_down(self, dur=1, angle=angle_right_down, speed=100.):
self.collision_avoidance = False
self.send_command('right_ball_down', [angle, speed])
logging.info("right_ball_down")
time.sleep(dur)
#self.collision_avoidance = True
def right_ball_up(self, dur=1, angle=angle_right_up, speed=100.):
self.collision_avoidance = False
self.send_command('right_ball_up', [angle, speed])
logging.info("right_ball_up")
time.sleep(dur)
#self.collision_avoidance = True
def right_ball_drop(self,
dur=1,
angle=angle_right_up + 32. + 5,
speed=100.0): # + 35
rselfcollision_avoidance = False
self.send_command('right_ball_drop', [angle, speed])
logging.info("right_ball_drop")
time.sleep(dur)
#self.collision_avoidance = True
### servos for cylinder take
def front_down_cylinder_no(self, dur=1):
self.collision_avoidance = False
self.send_command('front_down_cylinder_no')
time.sleep(dur)
#self.collision_avoidance = True
def front_up_cylinder_yes(self, dur=1):
self.collision_avoidance = False
self.send_command('front_up_cylinder_yes')
time.sleep(dur)
#self.collision_avoidance = True
def front_drop_cylinder_yes(self, dur=1):
self.collision_avoidance = False
self.send_command('front_drop_cylinder_yes')
time.sleep(dur)
#self.collision_avoidance = True
def back_down_cylinder_no(self, dur=1):
self.collision_avoidance = False
self.send_command('back_down_cylinder_no')
time.sleep(dur)
#self.collision_avoidance = True
def back_up_cylinder_yes(self, dur=1):
self.collision_avoidance = False
self.send_command('back_up_cylinder_yes')
time.sleep(dur)
#self.collision_avoidance = True
def back_drop_cylinder_yes(self, dur=1):
self.collision_avoidance = False
self.send_command('back_drop_cylinder_yes')
time.sleep(dur)
#self.collision_avoidance = True
# sticks to use
def both_sticks_open(self, dur=1):
self.collision_avoidance = False
self.send_command('both_sticks_open')
time.sleep(dur)
#self.collision_avoidance = True
def both_sticks_close(self, dur=1):
self.collision_avoidance = False
self.send_command('both_sticks_close')
time.sleep(dur)
#self.collision_avoidance = True
### seesaw manipulator
def seesaw_hand_down(self, dur=1):
self.collision_avoidance = False
self.send_command('seesaw_hand_down')
logging.info("seesaw_hand_down")
time.sleep(dur)
#self.collision_avoidance = True
def seesaw_hand_up(self, dur=1):
self.collision_avoidance = False
self.send_command('seesaw_hand_up')
logging.info("seesaw_hand_up")
time.sleep(dur)
#self.collision_avoidance = True
# funny action
def funny_action(self, signum, frame):
#self.send_command('stopAllMotors')
#self.send_command('funny_action_open')
funny_done = 1
logging.info('FUNNY ACTION')
raise Exception
def lin_interpol_traj(x1, y1, x2, y2, ratio):
return [x1 + (x2 - x1) * ratio, y1 + (y2 - y1) * ratio]
############################################################################
######## HIGH LEVEL FUNCTIONS ##############################################
############################################################################
def init_manipulators(self, dur=1):
self.left_ball_up(dur=dur)
self.right_ball_up(dur=dur)
self.seesaw_hand_up(dur=0.1)
def trajectory(self, speed=4, mode=3):
self.collision_avoidance = False
if True:
if mode == 1:
speed = 1
angle = np.pi / 2
parameters = [950, 200, angle, speed]
self.go_to_coord_rotation(parameters)
elif mode == 2:
self.collision_avoidance = False
speed = 1
angle = np.pi / 6
if self.color == "yellow":
angle = np.pi / 6
else:
angle = 2 * np.pi - np.pi / 6
parameters = [800, 280, angle, speed] # after seesaw
self.go_to_coord_rotation(parameters)
angle = np.pi / 2
parameters = [875, 200, angle, speed] # after seesaw
self.go_last(parameters, dur=2)
elif mode == 3:
speed = 1
angle = np.pi / 2
parameters = [875, 200, angle, speed]
#self.go_to_coord_rotation(parameters)
else:
speed = 1
angle = np.pi / 2
parameters = [950, 200, angle, speed]
self.go_to_coord_rotation(parameters)
self.collision_avoidance = True
#if mode == 3 or mode == 2: # normal speed 0 usage
angle = np.pi / 2
if self.color == "yellow":
parameters = [875, 580, angle, 0] # ,0]
self.go_to_coord_rotation(parameters)
speed = 1
parameters = [600, 1800, angle, speed]
self.go_to_coord_rotation(parameters)
else:
parameters = [875, 680, angle, 1] # ,0]
self.go_to_coord_rotation(parameters)
speed = 1
parameters = [500, 1800, angle, speed]
self.go_to_coord_rotation(parameters)
#else:
#angle = np.pi/2
#speed = 0
#parameters = [900, 700, angle, speed]
#self.go_to_coord_rotation(parameters)
#parameters = [740, 1000, angle, speed]
#self.go_to_coord_rotation(parameters)
########## Near corner crater, First time drop
speed = 1
if self.color == "yellow":
angle = np.pi / 2
parameters = [640, 1800, angle, speed]
self.go_to_coord_rotation(parameters)
self.collision_avoidance = False
speed = 4
parameters = [500, 1800, angle, speed]
self.go_to_coord_rotation(parameters)
else:
if False:
parameters = [640, 2000 - 380, angle, speed]
self.go_to_coord_rotation(parameters)
self.collision_avoidance = False
speed = 4
angle = np.pi / 4
parameters = [450, 2000 - 380, angle, speed]
self.go_to_coord_rotation(parameters)
else:
angle = np.pi / 2
parameters = [500, 1800, angle, speed] # 450
self.go_to_coord_rotation(parameters)
self.collision_avoidance = False
dur = 1.5
if self.color == "yellow":
self.left_ball_down(dur=dur)
y = 1800
x = 640
else:
self.right_ball_down(dur=dur)
y = 2000 - 300 # -260
x = 550 # 500
angle = np.pi / 2 - np.pi / 6
speed = 4
parameters = [x, y, angle, speed]
self.go_to_coord_rotation(parameters)
y = None
x = None
dur = 0.3
if self.color == "yellow":
self.left_ball_up(dur=dur)
else:
self.right_ball_up(dur=dur)
speed = 1
angle = np.pi / 2
self.collision_avoidance = True
parameters = [640, 2000 - 450, angle, speed]
self.go_to_coord_rotation(parameters)
######### Near corner crater, Second time drop
## TODO: add new point to take cylinder front! Then go as usually
#time.sleep(5)
## End TODO
speed = 1
angle = np.pi + np.pi / 3
parameters = [640, 2000 - 450, angle, speed]
self.go_to_coord_rotation(parameters)
self.collision_avoidance = False
speed = 1
parameters = [350, 2000 - 450, angle, speed]
self.go_to_coord_rotation(parameters)
dur = 1.5
if self.color == "yellow":
self.right_ball_down(dur=dur)
else:
self.left_ball_down(dur=dur)
speed = 1
angle = np.pi + np.pi / 3 + np.pi / 6
parameters = [500, 2000 - 450, angle, speed]
self.go_to_coord_rotation(parameters)
#self.right_ball_up(dur = 0.5)
dur = 0.3
if self.color == "yellow":
self.right_ball_up(dur=dur)
else:
self.left_ball_up(dur=dur)
angle = np.pi / 2
speed = 1
parameters = [500, 2000 - 450, angle, speed]
self.go_to_coord_rotation(parameters)
self.collision_avoidance = True
angle = np.pi / 2
speed = 1
parameters = [640, 2000 - 450, angle, speed]
self.go_to_coord_rotation(parameters)
time.sleep(3)
############# Go to start area 2
## TODO: place another point to drop clinder in lateral slot, or place it near DESAs robot somwhere
#time.sleep(7s)
##
speed = 1
#angle = 6*np.pi/7
#parameters = [965, 1100, angle,0] # 865 1100
#self.go_to_coord_rotation(parameters)
angle = np.pi / 2
parameters = [900, 800, angle, speed]
self.go_last(parameters, dur=3)
speed = 1
angle = np.pi / 2
parameters = [870, 230, angle, speed]
self.go_to_coord_rotation(parameters) # deleted
self.collision_avoidance = False # Need here to turn off without collision_avoidance
self.go_last(parameters, dur=2) # dur = 4
if self.color == "yellow":
angle = np.pi / 10
else:
angle = 2 * np.pi - np.pi / 10
speed = 1
parameters = [860, 200, angle, speed] # 250 # 870
self.go_last(parameters, dur=4)
#self.go_to_coord_rotation(parameters)
self.seesaw_hand_down()
for i in xrange(3):
logging.info("Time passed: " + str(time.time() - tmstmp))
######## From start area 2, via seesaw, to start area 1
speed = 4
#self.localisation.value = False
parameters = [100, 300, angle, speed] # 235, # 200,
self.go_to_coord_rotation(parameters)
self.seesaw_hand_up()
speed = 1
angle = 0.0
#time.sleep(1)
parameters = [185, 250, angle, speed]
self.go_last(parameters, dur=2)
## BOARDER localisation
angle = 0.0
parameters = [185, 300, angle, speed] # 225, 280
self.go_last(parameters, dur=2)
#corrections
cur = rev_field(parameters, self.color)
cur = [cur[0] / 1000., cur[1] / 1000., cur[2]]
self.send_command('setCoordinates', cur)
time.sleep(0.1)
## end
if self.color == "yellow":
self.right_ball_drop()
self.right_ball_up()
else:
self.left_ball_drop()
self.left_ball_up()
##### Revert in starting area 1
speed = 1
angle = 0.0
parameters = [185, 200, angle, speed]
self.go_last(parameters, dur=2)
speed = 4
angle = np.pi
parameters = [185, 200, angle, speed]
if self.color == "blue":
self.go_last(parameters, dur=3) # go_to_coord
else:
self.go_to_coord_rotation(parameters)
speed = 1
## BOARDER localisation
parameters = [185, 300, angle, speed] # 225, 280
self.go_last(parameters, dur=2.5)
#corrections
cur = rev_field(parameters, self.color)
cur = [cur[0] / 1000., cur[1] / 1000., cur[2]]
self.send_command('setCoordinates', cur)
time.sleep(0.1)
# end
if self.color == "yellow":
self.left_ball_drop()
self.left_ball_up()
else:
self.right_ball_drop()
self.right_ball_up()
for i in xrange(3):
logging.info("Time passed: " + str(time.time() - tmstmp))
####### From start area 1, via seesaw, to start area 2
# TODO: go once more
if False: # previous waya to go from start 1 to start 2
speed = 1
angle = np.pi + np.pi / 8
if self.color == "yellow":
angle = np.pi + np.pi / 8 #np.pi/6
else:
angle = 2 * np.pi - np.pi - np.pi / 8
parameters = [200, 200, angle, speed] # 200, 200
rb.go_last(parameters, dur=2)
speed = 1
parameters = [850, 300, angle, speed] # 800, 280
self.go_to_coord_rotation(parameters)
else:
speed = 1
angle = np.pi + np.pi / 8
if self.color == "yellow":
angle = np.pi + np.pi / 8 #np.pi/6
x = 180
y = 270
else:
angle = 2 * np.pi - np.pi - np.pi / 8 #
x = 140
y = 160
parameters = [x, y, angle, speed] # 200, 200
rb.go_last(parameters, dur=3)
speed = 4
if self.color == "yellow":
angle = np.pi + np.pi / 8 #np.pi/6
else:
angle = 2 * np.pi - np.pi - np.pi / 8 #
if self.color == "yellow":
x = 850
else:
x = 900
parameters = [x, 300, angle, speed] # 800, 280
self.go_to_coord_rotation(parameters)
speed = 1
angle = 3 * np.pi / 2 # need to place here before collision_avoidance
parameters = [870, 250, angle, speed]
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters) # need twice
###### Go to for back crater
self.collision_avoidance = True
#angle = 3*np.pi/2
parameters = [880, 580, angle, speed]
self.go_last(parameters, dur=1.5)
if self.color == "yellow":
self.right_ball_down(dur=1.5, angle=angle_right_down + 10)
self.right_ball_up(dur=0.2)
else:
self.left_ball_down(dur=1.5, angle=angle_left_down + 10)
self.left_ball_up(dur=0.2)
for i in xrange(3):
logging.info("Time passed: " + str(time.time() - tmstmp))
##################### Drop to start area 1
if True:
speed = 1
angle = 3 * np.pi / 2
parameters = [860, 250, angle, speed]
self.go_to_coord_rotation(parameters)
#self.right_ball_drop()
#self.right_ball_up()
if self.color == "yellow":
self.right_ball_drop()
self.right_ball_up()
else:
self.left_ball_drop()
self.left_ball_up()
return
##################### Drop to cargo bay, if you have time
if False: # No time for games :)) Should be tested
parameters = [1000, 280, angle, speed]
self.go_to_coord_rotation(parameters)
angle = np.pi / 18
if self.color == "yellow":
angle = np.pi / 18
else:
angle = 2 * np.pi - np.pi / 18
parameters = [880, 230, angle, speed]
self.go_last(parameters, dur=4)
self.seesaw_hand_down()
self.collision_avoidance = False
#self.localisation.value = False
parameters = [235, 230, angle, speed]
self.go_last(parameters)
self.seesaw_hand_up()
#self.localisation.value = True
#self.collision_avoidance = True
angle = 0.0
parameters = [235, 250, angle, speed]
self.go_to_coord_rotation(parameters)
self.go_last(parameters, dur=1)
return
## BOARDER localisation can be pasted
## end
if self.color == "yellow":
self.right_ball_drop()
self.right_ball_up()
else:
self.left_ball_drop()
self.left_ball_up()
return
def loc_test2(self,
x1x2y1y2=[500, 1500, 1000, 1000],
speed=4,
angle=np.pi,
n_times=3,
deltas=1):
x1, x2, y1, y2 = x1x2y1y2 #1000, 2500, 600, 1100
dx, dy = x2 - x1, y2 - y1
if angle is None:
angle = np.arctan2(dy, dx)
start = [x1, y1, angle]
for i in xrange(n_times):
for j in xrange(deltas + 1):
parameters = [
start[0] + j * dx / deltas, start[1] + j * dy / deltas,
start[2], speed
]
self.go_to_coord_rotation(parameters)
parameters = [start[0], start[1], start[2], speed]
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
def loc_test(self, speed=4, angle=np.pi / 2, n_times=3):
x1, x2, y1, y2 = 1000, 2500, 600, 1100
dx, dy = x2 - x1, y2 - y1
#angle = np.pi/2
start = [x1, y1, angle]
for i in xrange(n_times):
parameters = [start[0], start[1], start[2] + 0 * np.pi / 2, speed]
self.go_to_coord_rotation(parameters)
parameters = [start[0], start[1], start[2] + 1 * np.pi / 2, speed]
self.go_to_coord_rotation(parameters)
parameters = [start[0], start[1], start[2] + 2 * np.pi / 2, speed]
self.go_to_coord_rotation(parameters)
parameters = [start[0], start[1], start[2] + 3 * np.pi / 2, speed]
self.go_to_coord_rotation(parameters)
#parameters = [start[0], start[1], start[2] + 4*np.pi/2, speed]
#self.go_to_coord_rotation(parameters)
parameters = [start[0], start[1], start[2], speed]
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
def localisation_test(self,
point_lst=[[500, 1100]],
angle_lst=[0],
speed_lst=[4],
n_times=1):
odometry_coords = []
#self.coords = point_lst[0] + [angle_lst[0]]
#self.send_command('setCoordinates',
# [self.coords[0] / 1000.,
# self.coords[1] / 1000.,
# self.coords[2]])
for i in xrange(n_times):
a = []
for angle in angle_lst:
b = []
for speed in speed_lst:
c = []
for point in point_lst + point_lst[-2:0:-1]:
parameters = point + [angle, speed]
self.go_to_coord_rotation(parameters)
coords = self.send_command(
'getCurrentCoordinates')['data']
# check for invalid
coords[0] = coords[0] * 1000
coords[1] = coords[1] * 1000
c += [coords]
b += [c]
a += [b]
odometry_coords += [a]
return odometry_coords
def collision_test(self, speed=1, mode=1):
angle = np.pi / 2
while True:
if mode == "ci":
parameters = [1145, 400, angle, speed]
t = self.go_to_coord_rotation(parameters)
parameters = [945, 600, angle, speed]
t = self.go_to_coord_rotation(parameters)
parameters = [1145, 800, angle, speed]
t = self.go_to_coord_rotation(parameters)
parameters = [1345, 1000, angle, speed]
t = self.go_to_coord_rotation(parameters)
parameters = [1545, 800, angle, speed]
t = self.go_to_coord_rotation(parameters)
parameters = [1745, 600, angle, speed]
t = self.go_to_coord_rotation(parameters)
parameters = [1545, 400, angle, speed]
t = self.go_to_coord_rotation(parameters)
if mode == "sq":
parameters = [1145, 400, angle, speed]
t = self.go_to_coord_rotation(parameters)
parameters = [1145, 800, angle, speed]
t = self.go_to_coord_rotation(parameters)
parameters = [1545, 800, angle, speed]
t = self.go_to_coord_rotation(parameters)
parameters = [1545, 400, angle, speed]
t = self.go_to_coord_rotation(parameters)
if mode == "fb":
parameters = [1145, 400, angle, speed]
t = self.go_to_coord_rotation(parameters)
parameters = [1145, 800, angle, speed]
t = self.go_to_coord_rotation(parameters)
if mode == 'lr':
parameters = [1145, 600, angle, speed]
t = self.go_to_coord_rotation(parameters)
parameters = [1545, 600, angle, speed]
t = self.go_to_coord_rotation(parameters)
def __init__(self):
self.lidar = HokuyoLX()
self.kalman = KalmanFilter(adaptive=False, dt=0.025)
from hokuyolx import HokuyoLX # For UST10LX
laser = HokuyoLX(addr=('192.168.0.10', 10940))
timestamps, scan = laser.get_filtered_dist()
scan = scan.tolist()
keys = [0] * len(scan)
for i in range(len(scan)):
keys[i] = scan[i][0]
print keys
def __init__(self):
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
self.x = 0
self.y = 0
def __init__():
self.lidar = HokuyoLX()
class Robot:
def __init__(self, lidar_on=True,small=True):
sensors_number=6
self.sensor_range = 20
self.collision_avoidance = False
if small:
self.sensors_map = {0:(0, np.pi/3),1: (np.pi*0.5, np.pi*1.5),2: (5/3.*np.pi,2*np.pi),3: [(7/4.*np.pi,2*np.pi),(0,np.pi*1/4.)]}
#self.sensors_map= {0: (0, np.pi/3), 1: (np.pi/4, np.pi*7/12), 2: (np.pi*0.5, np.pi*1.5), 3: (17/12.*np.pi, 7/4.*np.pi), 4: (5/3.*np.pi,2*np.pi), 5: [(7/4.*np.pi,2*np.pi),(0,np.pi*1/4.)]} # can be problem with 2pi and 0
self.lidar_on = lidar_on
self.map = np.load('npmap.npy')
if lidar_on:
logging.debug('lidar is connected')
# add check for lidar connection
try:
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
except:
self.lidar_on = False
logging.warning('lidar is not connected')
#self.x = 170 # mm
#self.y = 150 # mm
#self.angle = 0.0 # pi
if small:
self.coords = Array('d',[850, 170, 3*np.pi / 2])
else:
self.coords = Array('d', [170, 170, 0])
self.localisation = Value('b', True)
self.input_queue = Queue()
self.loc_queue = Queue()
self.fsm_queue = Queue()
self.PF = pf.ParticleFilter(particles=1500, sense_noise=25, distance_noise=45, angle_noise=0.2, in_x=self.coords[0],
in_y=self.coords[1], in_angle=self.coords[2],input_queue=self.input_queue, out_queue=self.loc_queue)
# driver process
self.dr = driver.Driver(self.input_queue,self.fsm_queue,self.loc_queue)
self.p = Process(target=self.dr.run)
self.p.start()
self.p2 = Process(target=self.PF.localisation,args=(self.localisation,self.coords,self.get_raw_lidar))
logging.info(self.send_command('echo','ECHO'))
logging.info(self.send_command('setCoordinates',[self.coords[0] / 1000., self.coords[1] / 1000., self.coords[2]]))
self.p2.start()
time.sleep(0.1)
def send_command(self,name,params=None):
self.input_queue.put({'source': 'fsm','cmd': name,'params': params})
return self.fsm_queue.get()
def get_raw_lidar(self):
# return np.load('scan.npy')[::-1]
timestamp, scan = self.lidar.get_intens()
return scan
# return scan[::-1] our robot(old)
def check_lidar(self):
try:
state = self.lidar.laser_state()
except:
self.lidar_on = False
logging.warning('Lidar off')
def go_to_coord_rotation(self, parameters): # parameters [x,y,angle,speed]
if self.PF.warning:
time.sleep(1)
pm = [self.coords[0]/1000.,self.coords[1]/1000.,float(self.coords[2]),parameters[0] / 1000., parameters[1] / 1000., float(parameters[2]), parameters[3]]
x = parameters[0] - self.coords[0]
y = parameters[1] - self.coords[1]
sm = x+y
logging.info(self.send_command('go_to_with_corrections',pm))
# After movement
stamp = time.time()
time.sleep(0.100001) # sleep because of STM interruptions (Maybe add force interrupt in STM)
while not self.send_command('is_point_was_reached')['data']:
time.sleep(0.05)
if self.collision_avoidance:
direction = (float(x) / sm, float(y) / sm)
if self.check_collisions(direction):
self.send_command('stopAllMotors')
# check untill ok and then move!
# add Collision Avoidance there
if (time.time() - stamp) > 30:
return False # Error, need to handle somehow (Localize and add new point maybe)
if self.localisation.value == 0:
self.PF.move_particles([parameters[0]-self.coords[0],parameters[1]-self.coords[1],parameters[2]-self.coords[2]])
self.coords[0] = parameters[0]
self.coords[1] = parameters[1]
self.coords[2] = parameters[2]
logging.info('point reached')
return True
def check_collisions(self, direction):
angle = np.arctan2(direction[1],direction[0]) % (np.pi*2)
sensor_angle = (angle-self.coords[2]) % (np.pi*2)
#### switch on sensor_angle
collisions = self.sensor_data()
for index,i in enumerate(collisions):
if i and sensor_angle<=self.sensors_map[index][1] and sensor_angle>=self.sensors_map[index][0]:
logging.info("Collision at index "+str(index))
if self.check_map(direction):
continue
return True
return False
def receive_sensors_data(self):
data = self.send_command('sensors_data')['data']
answer = []
for i in range(6):
answer.append((data & (1 << i)) != 0)
return answer
def sensor_data(self):
data = self.send_command('sensors_data')['data']
return data
def check_map(self,direction): # probably can be optimized However O(1)
for i in range(0,self.sensor_range,2):
for dx in range(-2,2):
for dy in range(-2,2):
x = int(self.coords[0]/10+direction[0]*i+dx)
y = int(self.coords[1]/10+direction[1]*i+dy)
if x > pf.WORLD_X/10 or x < 0 or y > pf.WORLD_Y/10 or y < 0:
return True
# Or maybe Continue
if self.map[x][y]:
return True
return False
def go_last(self,parameters):
while abs(parameters[0]-self.coords[0]) > 10 or abs(parameters[1]-self.coords[1]) > 10:
print 'calibrate'
self.go_to_coord_rotation(parameters)
def left_ball_down(self):
self.send_command('left_ball_down')
time.sleep(1)
def left_ball_up(self):
self.send_command('left_ball_up')
time.sleep(1)
def left_ball_drop(self):
self.send_command('left_ball_drop')
time.sleep(1)
def right_ball_down(self):
self.send_command('right_ball_down')
time.sleep(1)
def right_ball_up(self):
self.send_command('right_ball_up')
time.sleep(1)
def right_ball_drop(self):
self.send_command('right_ball_drop')
time.sleep(1)
def funny(self):
self.send_command('funny_action')
time.sleep(1)
def on_sucker(self):
self.send_command('on_sucker')
def off_sucker(self):
self.send_command('off_sucker')
def rotate_cylinder_horizonal(self):
logging.info(self.send_command('rotate_cylinder_horizonal'))
time.sleep(0.3)
def rotate_cylinder_vertical(self):
logging.info(self.send_command('rotate_cylinder_vertical'))
time.sleep(0.3)
def take_cylinder_outside(self):
logging.info(self.send_command('take_cylinder_outside'))
time.sleep(0.5)
def take_cylinder_inside(self):
logging.info(self.send_command('take_cylinder_inside'))
time.sleep(0.5)
def lift_up(self):
logging.info(self.send_command('lift_up'))
time.sleep(0.5)
def store(self):
logging.info(self.send_command('store'))
# time.sleep(0.5)
def out_cylinders(self):
logging.info(self.send_command('out_cylinders'))
time.sleep(0.5)
def pick_up(self):
self.rotate_cylinder_vertical()
self.take_cylinder_inside()
self.lift_up()
self.off_sucker()
self.store()
self.rotate_cylinder_horizonal()
def pick_up2(self):
self.rotate_cylinder_vertical()
self.take_cylinder_inside()
self.lift_up()
self.off_sucker()
self.rotate_cylinder_horizonal()
def cyl_test(self):
self.pick_up()
self.pick_up()
self.pick_up()
self.out_cylinders()
############################################################################
######## HIGH LEVEL FUNCTIONS ##############################################
############################################################################
def demo(self, speed=1):
"""robot Demo, go to coord and take cylinder"""
signal.signal(signal.SIGALRM, self.funny_action)
signal.alarm(90)
# TODO take cylinder
self.rotate_cylinder_horizonal()
angle = np.pi
parameters = [850, 150, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1000, 500, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1000, 700, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [650, 1360, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [400, 1360, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [250, 1360, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [250, 1360, angle, speed]
self.go_to_coord_rotation(parameters)
# return
self.on_sucker()
self.take_cylinder_outside()
parameters = [160, 1360, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [320, 1360, angle, speed]
self.go_to_coord_rotation(parameters)
self.pick_up()
self.on_sucker()
self.take_cylinder_outside()
parameters = [160, 1360, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [320, 1360, angle, speed]
self.go_to_coord_rotation(parameters)
self.pick_up()
self.on_sucker()
self.take_cylinder_outside()
parameters = [160, 1360, angle, speed]
self.go_to_coord_rotation(parameters)
def demo_r2(self, speed=1):
angle = np.pi
parameters = [400, 1200, angle, speed]
self.go_to_coord_rotation(parameters)
self.pick_up2()
parameters = [150, 1140, angle, speed]
self.go_to_coord_rotation(parameters)
self.out_cylinders()
parameters = [150, 950, angle, speed]
self.go_to_coord_rotation(parameters)
self.out_cylinders()
parameters = [150, 800, angle, speed]
self.go_to_coord_rotation(parameters)
self.out_cylinders()
parameters = [600, 800, angle, speed]
self.go_to_coord_rotation(parameters)
def big_robot_trajectory(self,speed=1):
angle = np.pi*0.1
self.left_ball_up()
self.localisation.value = False
parameters = [900, 150, angle, speed]
self.go_to_coord_rotation(parameters)
self.localisation.value = True
angle = np.pi/2
parameters = [950, 400, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [950, 1000, angle, speed]
self.go_to_coord_rotation(parameters)
angle = 0.0
parameters = [250, 1750, angle, speed]
self.go_to_coord_rotation(parameters)
self.left_ball_down()
self.left_ball_up()
def big_robot_trajectory_r(self,speed=1):
angle = np.pi/2
parameters = [900, 1000, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [950, 400, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [950, 250, angle, speed]
self.go_to_coord_rotation(parameters)
angle = np.pi * 0.1
self.localisation.value = False
parameters = [170, 180, angle, speed]
self.go_to_coord_rotation(parameters)
self.localisation.value = True
self.left_ball_drop()
self.funny()
def loc_test(self):
while True:
angle = np.pi
parameters = [900, 200, angle, 6]
self.go_to_coord_rotation(parameters)
parameters = [900, 400, angle, 6]
self.go_to_coord_rotation(parameters)
parameters = [900, 600, angle, 6]
self.go_to_coord_rotation(parameters)
parameters = [900, 400, angle, 6]
self.go_to_coord_rotation(parameters)
def small_robot_trajectory(self,speed=1):
angle = 3*np.pi / 2
self.rotate_cylinder_horizonal()
parameters = [1100, 300, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1145, 250, angle, speed]
self.go_to_coord_rotation(parameters)
self.on_sucker()
self.take_cylinder_outside()
parameters = [1145, 160, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1145, 320, angle, speed]
self.go_to_coord_rotation(parameters)
self.pick_up()
self.on_sucker()
self.take_cylinder_outside()
parameters = [1145, 160, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1145, 320, angle, speed]
self.go_to_coord_rotation(parameters)
self.pick_up()
self.on_sucker()
self.take_cylinder_outside()
parameters = [1145, 160, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1145, 320, angle, speed]
self.go_to_coord_rotation(parameters)
self.pick_up2()
def small_robot_trajectory_r(self, speed=1):
angle = np.pi
parameters = [1150, 1000, angle, speed]
self.go_to_coord_rotation(parameters)
angle = np.pi-np.pi/4
parameters = [1320, 1520, angle, speed]
self.go_to_coord_rotation(parameters)
speed = 6
parameters = [1320, 1690, angle, speed]
self.go_to_coord_rotation(parameters)
self.out_cylinders()
parameters = [1217, 1590, angle, speed]
self.go_to_coord_rotation(parameters)
self.out_cylinders()
parameters = [1117, 1490, angle, speed]
self.go_to_coord_rotation(parameters)
self.out_cylinders()
speed = 4
parameters = [1150, 1000, angle, speed]
self.go_to_coord_rotation(parameters)
def funny_action(self, signum, frame):
print 'Main functionaly is off'
print 'FUNNNY ACTION'
def sens_test(self):
def wall_position(debug=False):
# if debugging is active then create a tkinter client and display
# the laser scan and calculations
if debug:
top = tk.Tk()
# Create label to show the image on
label = tk.Label(top)
label.pack()
# Create a figure to plot the results on
f = Figure(figsize=(5, 4), dpi=100)
# Create the plot
ax = f.add_subplot(111)
ax.plot([])
# Add the plot to the client
canvas = FigureCanvasTkAgg(f, master=top)
canvas.get_tk_widget().pack()
canvas.draw()
# If not debugging create a dummy ax and canvas
else:
ax = None
canvas = None
# Create the hokuyo laser object
laser = HokuyoLX()
# Create the publisher
pub = rospy.Publisher('wall_position', Float32MultiArray, queue_size=1)
ang_pub = rospy.Publisher('ang_pub', PoseStamped, queue_size=1)
wall_pub = rospy.Publisher('wall_pub', PoseStamped, queue_size=1)
pos_pub = rospy.Publisher('pos_pub', PointStamped, queue_size=1)
est_pub = rospy.Publisher('est_pub', PoseStamped, queue_size=1)
# Create the node
rospy.init_node('wall', anonymous=True)
# Set the rate. Not sure what this should be
rate = rospy.Rate(50)
# Keep the loop alive
while not rospy.is_shutdown():
# Obtain a laser measurement
x, y, angle, ax, canvas, x_est, y_est, yaw_est, valid = main_laser(
laser, ax, canvas, debug)
# Convert it to the ros array
msg = Float32MultiArray()
msg.data = [x, y, (angle), valid]
# Publish the data
pub.publish(msg)
# Est msg
est_msg = PoseStamped()
est_msg.header.stamp = rospy.Time.now()
est_msg.header.frame_id = "drone"
est_msg.pose.position.x = y_est * 0.001
est_msg.pose.position.y = x_est * 0.001
est_msg.pose.position.z = 0
q = tf.transformations.quaternion_from_euler(0, 0, (yaw_est) *
3.14159 / 180.0)
est_msg.pose.orientation.x = q[0]
est_msg.pose.orientation.y = q[1]
est_msg.pose.orientation.z = q[2]
est_msg.pose.orientation.w = q[3]
est_pub.publish(est_msg)
# Ang msg
ang_msg = PoseStamped()
ang_msg.header.stamp = rospy.Time.now()
ang_msg.header.frame_id = "drone"
ang_msg.pose.position.x = 0
ang_msg.pose.position.y = 0
ang_msg.pose.position.z = 0
q = tf.transformations.quaternion_from_euler(0, 0, (-angle) * 3.14159 /
180.0)
ang_msg.pose.orientation.x = q[0]
ang_msg.pose.orientation.y = q[1]
ang_msg.pose.orientation.z = q[2]
ang_msg.pose.orientation.w = q[3]
ang_pub.publish(ang_msg)
# Wall msg
wall_msg = PoseStamped()
wall_msg.header.stamp = rospy.Time.now()
wall_msg.header.frame_id = "drone"
wall_msg.pose.position.x = y * 0.001
wall_msg.pose.position.y = x * 0.001
wall_msg.pose.position.z = 0
q = tf.transformations.quaternion_from_euler(0, 0, (-angle - 90) *
3.14159 / 180.0)
wall_msg.pose.orientation.x = q[0]
wall_msg.pose.orientation.y = q[1]
wall_msg.pose.orientation.z = q[2]
wall_msg.pose.orientation.w = q[3]
wall_pub.publish(wall_msg)
# Pos msg
pos_msg = PointStamped()
pos_msg.header.stamp = rospy.Time.now()
pos_msg.header.frame_id = "drone"
pos_msg.point.x = y * 0.001
pos_msg.point.y = x * 0.001
pos_msg.point.z = 0
pos_pub.publish(pos_msg)
# If debug is active then update the client
if debug:
try:
top.update()
# Break if the tk client is no longer there
except tk.TclError:
break
# Keep the rate of the loop
rate.sleep()
class Robot:
def __init__(self,
lidar_on=True,
small=True,
init_coord=[900, 200, np.pi / 2],
color='yellow'):
# Cylinder Staff
self.coll_go = False
##################
self.color = color
self.status = False
self.cur_state = 0 # 0-neutral,1-suck,2-throw
self.sensor_range = 35
self.collision_d = 16
self.coll_ind = -1
self.collision_avoidance = True
self.localisation = Value('b', True)
self.sensors_places = [
np.pi / 2, 0, 3 * np.pi / 2, np.pi, 3 * np.pi / 2, 0, np.pi,
np.pi / 2, 0, 0
]
self.filter_queue = [np.array([False] * len(self.sensors_places))] * 3
self.sensors_map = {
1: (0, np.pi / 4),
5: (0, np.pi / 4),
7: (np.pi / 4, 3 * np.pi / 4),
0: (np.pi / 4, 3 * np.pi / 4),
6: (3 * np.pi / 4, 5 * np.pi / 4),
3: (3 * np.pi / 4, 5 * np.pi / 4),
2: (5 * np.pi / 4, 7 * np.pi / 4),
4: (5 * np.pi / 4, 7 * np.pi / 4),
8: (7 * np.pi / 4, 2 * np.pi),
9: (7 * np.pi / 4, 2 * np.pi)
}
self.lidar_on = lidar_on
self.map = np.load('npmap.npy')
if lidar_on:
logging.debug('lidar is connected')
# add check for lidar connection
try:
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
except:
self.lidar_on = False
self.localisation = Value('b', False)
logging.warning('lidar is not connected')
# self.x = 170 # mm
# self.y = 150 # mm
# self.angle = 0.0 # pi
#850 170 3p/2
# 900 200 np.pi/2
# 400, 850, 0.
self.coords = Array('d', rev_field(init_coord, self.color))
#else:
#driver.PORT_SNR = '325936843235' # need change
#self.coords = Array('d', rev_field([170, 170, 0], self.color))
self.input_queue = Queue()
self.loc_queue = Queue()
self.fsm_queue = Queue() # 2000,25,25,0.1
self.PF = pf.ParticleFilter(particles=2000,
sense_noise=25,
distance_noise=30,
angle_noise=0.15,
in_x=self.coords[0],
in_y=self.coords[1],
in_angle=self.coords[2],
input_queue=self.input_queue,
out_queue=self.loc_queue,
color=self.color)
# coords sharing procces
self.p3 = Process(target=app.run, args=("0.0.0.0", ))
# driver process
print "Paricle filter On"
self.dr = driver.Driver(self.input_queue, self.fsm_queue,
self.loc_queue)
print "Driver On"
self.p = Process(target=self.dr.run)
print "Process driver Init"
self.p.start()
print "Process driver Start"
self.p2 = Process(target=self.PF.localisation,
args=(self.localisation, self.coords,
self.get_raw_lidar))
print "Process npParticle Init"
logging.info(self.send_command('echo', 'ECHO'))
logging.info(
self.send_command('setCoordinates', [
self.coords[0] / 1000., self.coords[1] / 1000., self.coords[2]
]))
if self.lidar_on:
self.p2.start()
print "Process npParticle Start"
else:
self.localisation = Value('b', False)
print "Robot __init__ done!"
def send_command(self, name, params=None):
self.input_queue.put({'source': 'fsm', 'cmd': name, 'params': params})
return self.fsm_queue.get()
def get_raw_lidar(self):
# return np.load('scan.npy')[::-1]
timestamp, scan = self.lidar.get_intens()
return scan
# return scan[::-1] our robot(old)
def second_robot_cords(self):
try:
r = requests.get("http://192.168.1.213:5000/coords", timeout=1)
return ([float(i) for i in r.content.split()])
except:
logging.critical("Connection refused!")
return None
def get_status(self):
try:
r = requests.get("http://192.168.1.213:5000/status", timeout=1)
return bool(r.content)
except:
logging.critical("Connection refused!")
return True
def check_lidar(self):
try:
state = self.lidar.laser_state()
except:
self.lidar_on = False
logging.warning('Lidar off')
def go_to_coord_rotation(self, parameters):
# beta version of clever go_to
direct_random = [np.pi, np.pi / 2, -np.pi / 2]
distance = 200
# gomologization version and change timer in go_to
self.coll_go = False
ok = self.go_to(parameters)
#return
#
#ok = self.go_to(parameters)
while not ok:
logging.critical("Collision, go back")
angle = (self.coords[2] + self.sensors_places[self.coll_ind] +
random.choice(direct_random)) % (np.pi * 2)
direction = (np.cos(angle), np.sin(angle))
pm = [
self.coords[0] + direction[0] * distance,
self.coords[1] + direction[1] * distance, self.coords[2],
parameters[3]
]
logging.critical("New point " + str(pm))
self.go_to(pm)
logging.critical("go to correct")
self.coll_go = True
ok = self.go_to(parameters)
self.coll_go = False
def go_to(self, parameters): # parameters [x,y,angle,speed]
parameters = rev_field(parameters, self.color)
if self.PF.warning:
time.sleep(1)
pm = [
self.coords[0] / 1000., self.coords[1] / 1000.,
float(self.coords[2]), parameters[0] / 1000.,
parameters[1] / 1000.,
float(parameters[2]), parameters[3]
]
x = parameters[0] - self.coords[0]
y = parameters[1] - self.coords[1]
tm = 7
if self.coll_go == True:
tm = 1
logging.info(self.send_command('go_to_with_corrections', pm))
# After movement
stamp = time.time()
pids = True
time.sleep(
0.100001
) # sleep because of STM interruptions (Maybe add force interrupt in STM)
while not self.send_command('is_point_was_reached')['data']:
time.sleep(0.05)
if self.collision_avoidance:
direction = (float(x), float(y))
while self.check_collisions(direction):
if pids:
self.send_command('stopAllMotors')
pids = False
time.sleep(0.5)
if (time.time() - stamp) > tm:
self.send_command('cleanPointsStack')
cur = [
self.coords[0] / 1000., self.coords[1] / 1000.,
float(self.coords[2])
]
self.send_command('setCoordinates', cur)
logging.info(self.send_command('switchOnPid'))
return False
if not pids:
pids = True
self.send_command('cleanPointsStack')
cur = [
self.coords[0] / 1000., self.coords[1] / 1000.,
float(self.coords[2])
]
self.send_command('setCoordinates', cur)
logging.info(self.send_command('switchOnPid'))
pm[0] = cur[0]
pm[1] = cur[1]
pm[2] = cur[2]
logging.info(
self.send_command('go_to_with_corrections', pm))
time.sleep(0.10000001)
# return False
if self.localisation.value == 0:
self.PF.move_particles([
parameters[0] - self.coords[0], parameters[1] - self.coords[1],
parameters[2] - self.coords[2]
])
self.coords[0] = parameters[0]
self.coords[1] = parameters[1]
self.coords[2] = parameters[2]
logging.info('point reached')
return True
def check_collisions(self, direction):
angle = np.arctan2(direction[1], direction[0]) % (np.pi * 2)
sensor_angle = (angle - self.coords[2]) % (np.pi * 2)
#### switch on sensor_angle
collisions = self.sensor_data()
for index, i in enumerate(collisions):
if (i == True and sensor_angle <= self.sensors_map[index][1]
and sensor_angle >= self.sensors_map[index][0]):
logging.info("Collision at index " + str(index))
angle_map = (self.coords[2] +
self.sensors_places[index]) % (np.pi * 2)
if self.check_map2(angle_map):
continue
self.coll_ind = index
return True
return False
def receive_sensors_data(self):
data = self.send_command('sensors_data')['data']
answer = []
for i in range(6):
answer.append((data & (1 << i)) != 0)
return answer
def pre_sensor_data(self):
data = self.send_command('sensors_data')['data']
if type(data) == type('ok'):
return np.array([False] * len(self.sensors_places))
data.append(data[1])
data.append(data[5])
return np.array(data)
def sensor_data(self):
self.filter_queue = self.filter_queue[1:] + [self.pre_sensor_data()]
return np.prod(self.filter_queue, axis=0).astype(bool)
def check_map2(self, angle):
direction = (np.cos(angle), np.sin(angle))
for i in range(0, self.sensor_range, 2):
for dx in range(-self.collision_d, self.collision_d):
x = int(self.coords[0] / 10 + direction[0] * i + dx)
y = int(self.coords[1] / 10 + direction[1] * i)
#logging.info("x = "+str(x)+" y = " + str(y))
if x >= pf.WORLD_X / 10 or x <= 0 or y >= pf.WORLD_Y / 10 or y <= 0:
return True
# Or maybe Continue
if self.map[x][y]:
return True
return False
def check_map(self, direction): # probably can be optimized However O(1)
direction = (direction[0] / np.sum(np.abs(direction)),
direction[1] / np.sum(np.abs(direction)))
for i in range(0, self.sensor_range, 2):
for dx in range(-self.collision_d, self.collision_d):
x = int(self.coords[0] / 10 + direction[0] * i + dx)
y = int(self.coords[1] / 10 + direction[1] * i)
#logging.info("x = "+str(x)+" y = " + str(y))
if x > pf.WORLD_X / 10 or x < 0 or y > pf.WORLD_Y / 10 or y < 0:
return True
# Or maybe Continue
if self.map[x][y]:
return True
return False
def go_last(self, parameters):
pm = rev_field(parameters, self.color)
while abs(pm[0] - self.coords[0]) > 10 or abs(pm[1] -
self.coords[1]) > 10:
print 'calibrate'
self.go_to_coord_rotation(parameters)
time.sleep(0.1)
##########################################################
################# Sucker Robot ############################
##########################################################
def on_coolers_suck(self):
logging.info(self.send_command('on_coolers_suck'))
def on_coolers_throw(self):
logging.info(self.send_command('on_coolers_throw'))
def off_coolers(self):
logging.info(self.send_command('off_coolers'))
def on_mixer(self, direction=1):
logging.info(self.send_command('on_mixer', [direction]))
def off_mixer(self):
logging.info(self.send_command('off_mixer'))
def up_front_seasaw(self):
logging.info(self.send_command('up_front_seasaw'))
def up_back_seasaw(self):
logging.info(self.send_command('up_back_seasaw'))
def down_front_seasaw(self):
logging.info(self.send_command('down_front_seasaw'))
def down_back_seasaw(self):
logging.info(self.send_command('down_back_seasaw'))
def open_door(self):
logging.info(self.send_command('open_door'))
def close_door(self):
logging.info(self.send_command('close_door'))
def is_start(self):
return self.send_command('start_flag')['data']
############################################################################
######## HIGH LEVEL FUNCTIONS ##############################################
############################################################################
def first(self, speed=4):
angle = np.pi / 2
self.on_mixer()
self.suck()
parameters = [950, 550, angle, speed]
self.go_to_coord_rotation(parameters)
self.status = False
parameters = [900, 900, angle, speed]
self.go_to_coord_rotation(parameters)
self.stop()
angle = np.pi / 4
parameters = [500, 1500, angle, speed]
self.go_to_coord_rotation(parameters)
self.suck()
time.sleep(2)
parameters = [300, 1700, angle, speed]
self.go_to_coord_rotation(parameters)
angle = 0.0
parameters = [300, 1700, angle, speed]
self.go_to_coord_rotation(parameters)
angle = np.pi / 2
parameters = [300, 1700, angle, speed]
self.go_to_coord_rotation(parameters)
angle = 3 * np.pi / 2
parameters = [300, 1700, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [920, 1800, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [920, 1800, angle, speed]
self.go_to_coord_rotation(parameters)
time.sleep(1)
#parameters = [600,1600, angle, speed]
#self.go_to_coord_rotation(parameters)
angle = 0.0
self.status = False
parameters = [600, 1300, angle, speed]
self.go_to_coord_rotation(parameters)
self.stop()
return
def first_back(self, speed=4, mode=''):
# 500 1500 np.pi/2
self.status = False
angle = 0.0
while not self.get_status():
continue
parameters = [920, 900, angle, speed]
self.go_to_coord_rotation(parameters)
self.status = True
if mode == 'steal' and time.time() - tmstmp > 5:
pr_speed = speed
speed = 4
parameters = [900, 550, angle, speed]
self.go_to_coord_rotation(parameters)
angle = 3 * np.pi / 2
parameters = [1000, 550, angle, speed]
self.go_to_coord_rotation(parameters)
self.stop()
parameters = [1500, 550, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [2120, 550, angle, speed]
self.go_to_coord_rotation(parameters)
self.suck()
time.sleep(1.5)
self.stop()
angle = 0.0
parameters = [1000, 550, angle, speed]
self.go_to_coord_rotation(parameters)
speed = pr_speed
angle = 0.0
parameters = [920, 250, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [850, 210, angle, speed]
self.go_to_coord_rotation(parameters)
if self.color == "yellow":
self.down_back_seasaw()
else:
self.down_front_seasaw()
time.sleep(1)
parameters = [400, 210, angle, speed]
self.go_to_coord_rotation(parameters)
if self.color == "yellow":
self.up_back_seasaw()
else:
self.up_front_seasaw()
parameters = [200, 210, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [215, 170, angle, speed]
self.go_to_coord_rotation(parameters)
self.throw()
time.sleep(3)
self.stop()
time.sleep(0.2)
self.on_mixer(0)
self.suck()
time.sleep(2)
self.stop()
time.sleep(0.2)
self.throw()
time.sleep(3)
self.stop()
self.off_mixer()
return
def steal_strategy(self, speed=4):
angle = np.pi / 2
self.on_mixer()
self.suck()
parameters = [880, 500, angle, 4]
self.go_to_coord_rotation(parameters)
if False:
angle = 3 * np.pi / 2
parameters = [880, 500, angle, 4]
self.go_to_coord_rotation(parameters)
self.stop()
parameters = [1500, 500, angle, 4]
self.go_to_coord_rotation(parameters)
parameters = [2120, 550, angle, 4]
self.go_to_coord_rotation(parameters)
self.suck()
time.sleep(1)
self.stop()
parameters = [1000, 550, angle, 4]
self.go_to_coord_rotation(parameters)
angle = np.pi / 2
parameters = [900, 900, angle, 4]
self.go_to_coord_rotation(parameters)
self.stop()
angle = np.pi / 4
parameters = [500, 1500, angle, speed]
self.go_to_coord_rotation(parameters)
self.suck()
time.sleep(2)
parameters = [300, 1700, angle, speed]
self.go_to_coord_rotation(parameters)
angle = 0.0
parameters = [300, 1700, angle, speed]
self.go_to_coord_rotation(parameters)
angle = np.pi / 2
parameters = [300, 1700, angle, speed]
self.go_to_coord_rotation(parameters)
angle = 3 * np.pi / 2
parameters = [300, 1700, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [920, 1800, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [920, 1800, angle, speed]
self.go_to_coord_rotation(parameters)
time.sleep(1)
#parameters = [600,1600, angle, speed]
#self.go_to_coord_rotation(parameters)
angle = 0.0
parameters = [600, 1300, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [600, 1300, angle, speed]
self.go_to_coord_rotation(parameters)
self.stop()
return
def steal_strategy2(self, speed=4):
angle = np.pi / 2
self.on_mixer()
self.suck()
parameters = [900, 550, angle, speed]
self.go_to_coord_rotation(parameters)
self.status = False
parameters = [900, 900, angle, speed]
self.go_to_coord_rotation(parameters)
self.stop()
self.status = True
angle = np.pi / 4
parameters = [500, 1500, angle, speed]
self.go_to_coord_rotation(parameters)
##
self.stop()
self.suck()
time.sleep(0.2)
parameters = [300, 2000 - 630, angle, 1]
self.go_to_coord_rotation(parameters)
#self.stop()
parameters = [225, 2000 - 440, angle, speed]
self.go_to_coord_rotation(parameters)
angle = 0.0
parameters = [225, 2000 - 300, angle, speed]
self.go_to_coord_rotation(parameters)
angle = 2 * np.pi - np.pi / 6
parameters = [190, 2000 - 310, angle, speed]
self.go_to_coord_rotation(parameters)
angle = 2 * np.pi - np.pi / 2
parameters = [265, 2000 - 230, angle, speed]
self.go_to_coord_rotation(parameters)
angle = 2 * np.pi - np.pi / 2 - np.pi / 6
parameters = [265, 2000 - 230, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [650, 2000 - 330, angle, speed]
self.go_to_coord_rotation(parameters)
# suck front crater
angle = 2 * np.pi - np.pi / 2
parameters = [850, 1800, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [880, 1800, angle, speed]
self.go_to_coord_rotation(parameters)
time.sleep(1)
#parameters = [600,1600, angle, speed]
#self.go_to_coord_rotation(parameters)
self.status = False
angle = 0.0
parameters = [600, 1300, angle, speed]
self.go_to_coord_rotation(parameters)
self.stop()
return
def cube(self, speed=4):
angle = 0.0
#parameters = [920, 200, angle, speed]
#self.go_to_coord_rotation(parameters)
parameters = [920, 600, angle, speed]
self.go_to_coord_rotation(parameters)
return
while True:
parameters = [1200, 500, angle, speed]
self.go_to_coord_rotation(parameters)
angle = np.pi
parameters = [1200, 1000, angle, speed]
self.go_to_coord_rotation(parameters)
continue
parameters = [1700, 1000, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1700, 500, angle, speed]
self.go_to_coord_rotation(parameters)
def throw(self):
self.cur_state = 2
self.close_door()
self.on_coolers_throw()
time.sleep(0.2)
self.off_coolers()
time.sleep(0.2)
self.on_coolers_throw()
def suck(self):
self.cur_state = 1
self.off_coolers()
time.sleep(0.1)
self.on_coolers_suck()
self.open_door()
def stop(self):
self.cur_state = 0
self.close_door()
time.sleep(0.4)
self.off_coolers()
def funny_action(self, signum, frame):
global funny_done
logging.critical('FUNNNY ACTION start')
logging.critical('CHECK if not throwing')
if self.cur_state == 1:
self.stop()
#time.sleep(0.2)
tms = time.time()
logging.critical('Adjust position')
#try:
self.collision_avoidance = False ## Not to be stucked in while
self.localisation.value = False ## Not to correct
x, y, ang = self.coords[:3]
throw_x, throw_y = 210, 110
ang_dir = np.arctan2(y - throw_y, x - throw_x) # wil be from 0 to pi/2
new_ang = (2 * np.pi -
(np.pi / 2 + np.pi / 2 - ang_dir) + np.pi / 2) % (
2 * np.pi
) #(... + np.pi/2 because of have hole at right)
#if (new_ang - ang)*180/np.pi > 1 and not 190 <= x <= 220 and not 150 <= y <= 190:
new_x = x
new_y = y
speed = 4 # isn't too slow??
## TODO if collision or bad place to rotate -> go away first: x->new_x, y->new_y
pm = [new_x, new_y, new_ang, speed]
logging.info("New coords: " + str(pm))
parameters = rev_field(pm, self.color)
pm = [
self.coords[0] / 1000., self.coords[1] / 1000.,
float(self.coords[2]), parameters[0] / 1000.,
parameters[1] / 1000.,
float(parameters[2]), parameters[3]
]
logging.info(self.send_command('go_to_with_corrections', pm))
#self.go_to_coord_rotation(pm)
#logging.critical('Start Throw')
if self.cur_state == 0:
self.throw()
#logging.critical("Throw")
tms_n = time.time()
while tms_n - tmstmp <= time_stop:
#time.sleep(0.1)
#logging.critical("Here in While!")
tms_n = time.time()
#logging.critical("Stopping")
self.stop()
self.off_mixer()
#logging.critical('Stop Propeller. Stop mixer.')
#logging.info(self.send_command('stopAllMotors'))
#logging.critical('Stop All Motors')
tms_n = time.time()
while tms_n - tmstmp <= time_funny:
#time.sleep(0.1)
#logging.critical("Here in While!")
tms_n = time.time()
logging.info(self.send_command('funny_action_open'))
funny_done = 1
raise Exception
class Robot:
def __init__(self, lidar_on=True, small=True, color='yellow'):
# Cylinder Staff
self.cylinders = 0
self.cyl_prepare = [0.215, 0.143, 0.143]
self.cyl_up = [0.523, 0.64, 0.66]
self.cyl_down = [-0.79, -0.67, -0.72]
self.coll_go = False
self.useLift = False
self.collision_belts = False
##################
self.color = color
self.sensor_range = 35
self.collision_d = 9
self.coll_ind = -1
self.collision_avoidance = True
self.localisation = Value('b', True)
if small:
self.sensors_places = [
0, 0, 0, np.pi, np.pi / 2, 3 * np.pi / 2, 0, 0, 0
]
self.sensors_map = {
0: (0, np.pi / 3),
7: (7 * np.pi / 4, 2 * np.pi),
3: (np.pi * 0.7, np.pi * 1.3),
1: (5 / 3. * np.pi, 2 * np.pi),
2: (0, np.pi * 1 / 4.),
6: (7 / 4. * np.pi, 2 * np.pi),
8: (0, np.pi / 4),
4: (np.pi / 4, 3 * np.pi / 4),
5: (np.pi * 5 / 4, 7 * np.pi / 4)
}
self.lidar_on = lidar_on
self.map = np.load('npmap.npy')
if lidar_on:
logging.debug('lidar is connected')
# add check for lidar connection
try:
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
except:
self.lidar_on = False
self.localisation = Value('b', False)
logging.warning('lidar is not connected')
# self.x = 170 # mm
# self.y = 150 # mm
# self.angle = 0.0 # pi
if small:
#850 170 3p/2
#
self.coords = Array(
'd', rev_field([170, 170, 3 * np.pi / 2], self.color))
else:
driver.PORT_SNR = '325936843235' # need change
self.coords = Array('d', rev_field([170, 170, 0], self.color))
self.input_queue = Queue()
self.loc_queue = Queue()
self.fsm_queue = Queue() # 2000,25,25,0.1
self.PF = pf.ParticleFilter(particles=2000,
sense_noise=25,
distance_noise=25,
angle_noise=0.1,
in_x=self.coords[0],
in_y=self.coords[1],
in_angle=self.coords[2],
input_queue=self.input_queue,
out_queue=self.loc_queue,
color=self.color)
# driver process
print "Paricle filter On"
self.dr = driver.Driver(self.input_queue, self.fsm_queue,
self.loc_queue)
print "Driver On"
self.p = Process(target=self.dr.run)
print "Process driver Init"
self.p.start()
print "Process driver Start"
self.p2 = Process(target=self.PF.localisation,
args=(self.localisation, self.coords,
self.get_raw_lidar))
print "Process npParticle Init"
logging.info(self.send_command('echo', 'ECHO'))
logging.info(
self.send_command('setCoordinates', [
self.coords[0] / 1000., self.coords[1] / 1000., self.coords[2]
]))
if self.lidar_on:
self.p2.start()
print "Process npParticle Start"
else:
self.localisation = Value('b', False)
#time.sleep(0.1)
logging.info(self.send_command('rotate_cylinder_vertical', [146.0]))
#if self.color == "yellow":
# self.rotate_cylinder_vertical()
#else:
# self.rotate_cylinder_horizonal()
print "Robot __init__ done!"
def send_command(self, name, params=None):
self.input_queue.put({'source': 'fsm', 'cmd': name, 'params': params})
return self.fsm_queue.get()
def get_raw_lidar(self):
# return np.load('scan.npy')[::-1]
timestamp, scan = self.lidar.get_intens()
return scan
# return scan[::-1] our robot(old)
def check_lidar(self):
try:
state = self.lidar.laser_state()
except:
self.lidar_on = False
logging.warning('Lidar off')
def go_to_coord_rotation(self, parameters):
# beta version of clever go_to
direct_random = [np.pi, np.pi / 2, -np.pi / 2]
distance = 200
# gomologization version and change timer in go_to
self.coll_go = False
ok = self.go_to(parameters)
#return
#
#ok = self.go_to(parameters)
while not ok:
logging.critical("Collision, go back")
angle = (self.coords[2] + self.sensors_places[self.coll_ind] +
random.choice(direct_random)) % (np.pi * 2)
direction = (np.cos(angle), np.sin(angle))
pm = [
self.coords[0] + direction[0] * distance,
self.coords[1] + direction[1] * distance, self.coords[2],
parameters[3]
]
self.go_to(pm)
logging.critical("go to correct")
self.coll_go = True
ok = self.go_to(parameters)
self.coll_go = False
def go_to(self, parameters): # parameters [x,y,angle,speed]
parameters = rev_field(parameters, self.color)
if self.PF.warning:
time.sleep(1)
pm = [
self.coords[0] / 1000., self.coords[1] / 1000.,
float(self.coords[2]), parameters[0] / 1000.,
parameters[1] / 1000.,
float(parameters[2]), parameters[3]
]
x = parameters[0] - self.coords[0]
y = parameters[1] - self.coords[1]
tm = 7
if self.coll_go == True:
tm = 1
logging.info(self.send_command('go_to_with_corrections', pm))
# After movement
stamp = time.time()
pids = True
time.sleep(
0.100001
) # sleep because of STM interruptions (Maybe add force interrupt in STM)
while not (self.send_command('is_point_was_reached')['data']):
time.sleep(0.05)
if self.collision_avoidance:
direction = (float(x), float(y))
while self.check_collisions(direction):
if pids:
self.send_command('stopAllMotors')
pids = False
time.sleep(0.5)
if (time.time() - stamp) > tm:
self.send_command('cleanPointsStack')
cur = [
self.coords[0] / 1000., self.coords[1] / 1000.,
float(self.coords[2])
]
self.send_command('setCoordinates', cur)
logging.info(self.send_command('switchOnPid'))
return False
if not pids:
pids = True
self.send_command('cleanPointsStack')
cur = [
self.coords[0] / 1000., self.coords[1] / 1000.,
float(self.coords[2])
]
self.send_command('setCoordinates', cur)
logging.info(self.send_command('switchOnPid'))
pm[0] = cur[0]
pm[1] = cur[1]
pm[2] = cur[2]
logging.info(
self.send_command('go_to_with_corrections', pm))
time.sleep(0.10000001)
# return False
if self.localisation.value == 0:
self.PF.move_particles([
parameters[0] - self.coords[0], parameters[1] - self.coords[1],
parameters[2] - self.coords[2]
])
self.coords[0] = parameters[0]
self.coords[1] = parameters[1]
self.coords[2] = parameters[2]
logging.info('point reached')
return True
def check_collisions(self, direction):
angle = np.arctan2(direction[1], direction[0]) % (np.pi * 2)
sensor_angle = (angle - self.coords[2]) % (np.pi * 2)
#### switch on sensor_angle
collisions = self.sensor_data()
for index, i in enumerate(collisions):
if (i == True and sensor_angle <= self.sensors_map[index][1]
and sensor_angle >= self.sensors_map[index][0]):
logging.info("Collision at index " + str(index))
angle_map = (self.coords[2] +
self.sensors_places[index]) % (np.pi * 2)
if self.check_map2(angle_map):
continue
self.coll_ind = index
return True
return False
def receive_sensors_data(self):
data = self.send_command('sensors_data')['data']
answer = []
for i in range(6):
answer.append((data & (1 << i)) != 0)
return answer
def pre_sensor_data(self):
data = self.send_command('sensors_data')['data']
return data
data.append(data[2])
data.append(data[0])
data.append(data[1])
if self.collision_belts:
data[0] = False
data[7] = False
data[1] = False
data[8] = False
return np.array(data)
def sensor_data(self):
return self.pre_sensor_data() * self.pre_sensor_data(
) * self.pre_sensor_data()
def check_map2(self, angle):
direction = (np.cos(angle), np.sin(angle))
for i in range(0, self.sensor_range, 2):
for dx in range(-self.collision_d, self.collision_d):
x = int(self.coords[0] / 10 + direction[0] * i + dx)
y = int(self.coords[1] / 10 + direction[1] * i)
#logging.info("x = "+str(x)+" y = " + str(y))
if x >= pf.WORLD_X / 10 or x <= 0 or y >= pf.WORLD_Y / 10 or y <= 0:
return True
# Or maybe Continue
if self.map[x][y]:
return True
return False
def check_map(self, direction): # probably can be optimized However O(1)
direction = (direction[0] / np.sum(np.abs(direction)),
direction[1] / np.sum(np.abs(direction)))
for i in range(0, self.sensor_range, 2):
for dx in range(-self.collision_d, self.collision_d):
x = int(self.coords[0] / 10 + direction[0] * i + dx)
y = int(self.coords[1] / 10 + direction[1] * i)
#logging.info("x = "+str(x)+" y = " + str(y))
if x > pf.WORLD_X / 10 or x < 0 or y > pf.WORLD_Y / 10 or y < 0:
return True
# Or maybe Continue
if self.map[x][y]:
return True
return False
def go_last(self, parameters):
pm = rev_field(parameters, self.color)
while abs(pm[0] - self.coords[0]) > 10 or abs(pm[1] -
self.coords[1]) > 10:
print 'calibrate'
self.go_to_coord_rotation(parameters)
time.sleep(0.1)
##########################################################
################# SMALL Robot ############################
##########################################################
def is_cylinder_taken(self):
return self.send_command('cylinder_taken')['data']
def on_sucker(self):
self.send_command('on_sucker')
def off_sucker(self):
self.send_command('off_sucker')
time.sleep(0.3)
def rotate_cylinder_horizonal(self):
if self.color == 'yellow':
logging.info(
self.send_command('rotate_cylinder_horizonal', [249.0]))
else:
logging.info(self.send_command('rotate_cylinder_vertical',
[146.0]))
time.sleep(0.1)
def rotate_cylinder_vertical(self):
if self.color == 'yellow':
logging.info(self.send_command('rotate_cylinder_vertical',
[146.0]))
else:
logging.info(
self.send_command('rotate_cylinder_horizonal', [249.0]))
time.sleep(0.1)
def take_cylinder_outside(self):
logging.info(self.send_command('take_cylinder_outside'))
time.sleep(0.3)
def take_cylinder_inside(self, rotation='l'):
if self.color == 'yellow':
if rotation == 'l':
#rb.rotate_cylinder_vertical()
logging.info(
self.send_command('take_cylinder_inside_l', [249.0]))
else:
#rb.rotate_cylinder_horizonal()
logging.info(
self.send_command('take_cylinder_inside_r', [146.0]))
else:
if rotation == 'l':
#rb.rotate_cylinder_vertical()
logging.info(
self.send_command('take_cylinder_inside_r', [146.0]))
else:
#rb.rotate_cylinder_horizonal()
logging.info(
self.send_command('take_cylinder_inside_l', [249.0]))
time.sleep(0.5)
def lift_up(self):
logging.info(
self.send_command('lift_up', [self.cyl_up[self.cylinders]]))
time.sleep(self.cyl_up[self.cylinders])
def store(self):
logging.info(
self.send_command('lift_up', [self.cyl_prepare[self.cylinders]]))
time.sleep(self.cyl_prepare[self.cylinders])
# time.sleep(0.5)
def out_cylinders(self):
logging.info(
self.send_command('lift_up', [self.cyl_down[self.cylinders - 1]]))
time.sleep(0.5)
self.cylinders -= 1
def is_start(self):
return self.send_command('start_flag')['data']
def off_wheels(self):
logging.info(self.send_command('off_wheels'))
def on_wheels(self):
logging.info(self.send_command('on_wheels'))
def pick_up(self, version=False):
#self.rotate_cylinder_vertical()
if (version == False):
self.take_cylinder_inside()
else:
self.take_cylinder_inside('r')
self.lift_up()
self.off_sucker()
self.store()
if (version == False):
self.rotate_cylinder_vertical()
else:
self.rotate_cylinder_horizonal()
self.cylinders += 1
############################################################################
######## HIGH LEVEL FUNCTIONS ##############################################
############################################################################
def loc_test(self):
while True:
angle = 0.0
parameters = [900, 200, angle, 4]
self.go_to_coord_rotation(parameters)
parameters = [900, 400, angle, 4]
self.go_to_coord_rotation(parameters)
parameters = [900, 600, angle, 4]
self.go_to_coord_rotation(parameters)
parameters = [900, 400, angle, 4]
self.go_to_coord_rotation(parameters)
def small_robot_trajectory(self, speed=1):
signal.signal(signal.SIGALRM, rb.funny_action)
signal.alarm(90)
angle = 3 * np.pi / 2
#self.rotate_cylinder_vertical()
parameters = [870, 160, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1150, 300, angle, 4]
self.go_last(parameters)
parameters = [1150, 290, angle, 4]
self.go_to_coord_rotation(parameters)
#self.go_to_coord_rotation(parameters)
#parameters = [1150, 290, angle, speed]
#self.go_to_coord_rotation(parameters)
self.collision_avoidance = False
self.sensor_range = 60
#parameters = [1150, 250, angle, speed]
#self.go_to_coord_rotation(parameters)
self.collision_avoidance = True
self.on_sucker()
self.take_cylinder_outside()
self.collision_avoidance = False
parameters = [1150, 160, angle, speed]
self.go_to_coord_rotation(parameters)
self.collision_avoidance = True
#time.sleep(0.2)
parameters = [1150, 340, angle, 1]
self.go_to_coord_rotation(parameters)
self.pick_up()
#time.sleep(3)
#parameters = [1150, 330, angle, speed]
#self.go_to_coord_rotation(parameters)
self.on_sucker()
self.take_cylinder_outside()
self.collision_avoidance = False
parameters = [1150, 160, angle, speed]
self.go_to_coord_rotation(parameters)
self.collision_avoidance = True
parameters = [1150, 340, angle, 1]
self.go_to_coord_rotation(parameters)
self.pick_up()
self.on_sucker()
self.take_cylinder_outside()
self.collision_avoidance = False
parameters = [1150, 160, angle, speed]
self.go_to_coord_rotation(parameters)
self.collision_avoidance = True
speed = 4
parameters = [1150, 340, angle, 1]
self.go_to_coord_rotation(parameters)
self.pick_up()
speed = 4
# cylinder corrections
self.on_sucker()
self.take_cylinder_outside()
parameters = [1250, 300, angle, 1]
self.go_to_coord_rotation(parameters)
parameters = [1250, 210, angle, 1]
self.go_to_coord_rotation(parameters)
parameters = [1190, 210, angle, 1]
self.go_to_coord_rotation(parameters)
parameters = [1190, 300, angle, 1]
self.go_to_coord_rotation(parameters)
parameters = [1050, 300, angle, 1]
self.go_to_coord_rotation(parameters)
parameters = [1050, 210, angle, 1]
self.go_to_coord_rotation(parameters)
parameters = [1105, 210, angle, 1]
self.go_to_coord_rotation(parameters)
parameters = [1105, 300, angle, 1]
self.go_to_coord_rotation(parameters)
#parameters = [1150, 300, angle, speed]
#self.go_to_coord_rotation(parameters)
###########
parameters = [1150, 160, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1150, 340, angle, speed]
self.go_to_coord_rotation(parameters)
self.rotate_cylinder_horizonal()
self.sensor_range = 35
def small_robot_trajectory_r(self, speed=1):
angle = 3 * np.pi / 2
speed = 4
parameters = [1150, 1000, angle, speed]
self.go_to_coord_rotation(parameters)
angle = 3 * np.pi / 4
parameters = [1320, 1520, angle, speed]
self.go_to_coord_rotation(parameters)
speed = 6
#return
self.collision_belts = True
parameters = [1350, 1650, angle, speed]
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.off_sucker()
self.take_cylinder_inside()
parameters = [1230, 1540, angle,
speed] # [1250, 1560, angle, speed] no push
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.out_cylinders()
self.take_cylinder_outside()
self.take_cylinder_inside()
### PUSHING
parameters = [1180, 1485, angle, speed]
self.go_to_coord_rotation(parameters)
self.take_cylinder_outside()
parameters = [1300, 1600, angle, speed]
self.go_to_coord_rotation(parameters)
self.take_cylinder_inside()
#########
parameters = [1210, 1510, angle,
speed] # [1180, 1485, angle, speed] no push
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.out_cylinders()
self.take_cylinder_outside()
self.take_cylinder_inside()
parameters = [1120, 1440, angle, speed] # [1095, 1410, angle, speed]
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.out_cylinders()
self.take_cylinder_outside()
self.take_cylinder_inside()
speed = 4
self.collision_belts = False
#logging.info(self.send_command('lift_up',[30000]))
self.useLift = True
parameters = [875, 1150, angle, speed]
self.go_to_coord_rotation(parameters)
### multicolor
angle = 0.2
parameters = [760, 1390, angle, speed]
self.go_to_coord_rotation(parameters)
time.sleep(0.5)
parameters = [780, 1380, angle, speed]
self.go_to_coord_rotation(parameters)
#if self.color == 'yellow': # maybe change
# self.rotate_cylinder_horizonal()
#else:
# self.rotate_cylinder_vertical()
self.on_sucker()
self.take_cylinder_outside()
parameters = [850, 1410, angle, speed]
self.go_to_coord_rotation(parameters)
#rb.rotate_cylinder_vertical()
parameters = [700, 1360, angle, speed]
self.go_to_coord_rotation(parameters)
angle = np.pi
self.rotate_cylinder_vertical()
#if self.color == 'yellow': # maybe change
# self.rotate_cylinder_vertical()
#else:
# self.rotate_cylinder_horizonal()
#self.take_cylinder_inside()
parameters = [600, 1000, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [500, 1000, angle, speed]
self.go_to_coord_rotation(parameters)
speed = 4
parameters = [120, 1000, angle, speed]
self.go_to_coord_rotation(parameters)
#self.take_cylinder_inside()
#parameters = [130, 1050, angle, speed]
#self.go_to_coord_rotation(parameters)
self.off_sucker()
parameters = [500, 1000, angle, speed]
self.go_to_coord_rotation(parameters)
angle = 3 * np.pi / 4 + np.pi / 2
parameters = [410, 780, angle, speed]
self.go_to_coord_rotation(parameters)
rb.rotate_cylinder_horizonal()
parameters = [410, 780, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [210, 580, angle, speed]
self.go_to_coord_rotation(parameters)
self.rotate_cylinder_vertical()
#if self.color == "yellow":
# self.rotate_cylinder_vertical()
####self.on_sucker()
####self.take_cylinder_outside()
parameters = [130, 500, angle, speed]
self.go_to_coord_rotation(parameters)
#parameters = [210, 580, angle, speed]
#self.go_to_coord_rotation(parameters)
self.off_wheels()
self.on_sucker()
self.take_cylinder_outside()
self.on_wheels()
#parameters = [100, 470, angle, speed]
#self.go_to_coord_rotation(parameters)
parameters = [210, 580, angle, speed]
self.go_to_coord_rotation(parameters)
angle = np.pi
self.rotate_cylinder_horizonal()
#if self.color == "yellow":
# self.rotate_cylinder_horizonal()
#else:
# self.rotate_cylinder_vertical()
parameters = [300, 800, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [120, 800, angle, speed]
self.go_to_coord_rotation(parameters)
self.off_sucker()
return
####
speed = 4
#parameters = [750, 1300, angle, speed]
#self.go_to_coord_rotation(parameters)
#parameters = [830, 1380, angle, speed]
#self.go_to_coord_rotation(parameters)
angle = np.pi
parameters = [200, 600, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [200, 800, angle, speed]
self.go_to_coord_rotation(parameters)
if self.color == "yellow":
self.rotate_cylinder_horizonal()
else:
self.rotate_cylinder_vertical()
self.off_sucker()
return
#parameters = [350, 600, angle, speed]
#self.go_to_coord_rotation(parameters)
#parameters = [340, 600, angle, speed]
#self.go_to_coord_rotation(parameters)
#parameters = [350, 600, angle, speed]
#self.go_to_coord_rotation(parameters)
#return
#self.on_sucker()
#self.take_cylinder_outside()
speed = 4
parameters = [120, 600, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [170, 600, angle, speed] # 250,600 cylinder
self.go_to_coord_rotation(parameters)
parameters = [170, 900, angle, speed]
self.go_to_coord_rotation(parameters)
#parameters = [300, 800, angle, speed]
#self.go_to_coord_rotation(parameters)
#parameters = [200, 800, angle, speed]
#self.go_to_coord_rotation(parameters)
return
### One cylinder more
#parameters = [1150, 1000, angle, speed]
#self.go_to_coord_rotation(parameters)
##
#parameters = [950, 270, 3*np.pi / 2,speed]
#self.go_last(parameters)
def second_trajectory_r(self, speed=1):
angle = 3 * np.pi / 2
speed = 4
parameters = [1250, 1000, angle, speed]
self.go_to_coord_rotation(parameters)
angle = 3 * np.pi / 4
parameters = [1320, 1520, angle, speed]
self.go_to_coord_rotation(parameters)
speed = 6
#return
self.collision_belts = True
parameters = [1350, 1650, angle, speed]
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.off_sucker()
self.take_cylinder_inside()
parameters = [1230, 1540, angle,
speed] # [1250, 1560, angle, speed] no push
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.out_cylinders()
self.take_cylinder_outside()
#self.take_cylinder_inside()
### PUSHING
#parameters = [1180, 1485, angle, speed]
#self.go_to_coord_rotation(parameters)
#self.take_cylinder_outside()
parameters = [1300, 1600, angle, speed]
self.go_to_coord_rotation(parameters)
self.take_cylinder_inside()
#########
parameters = [1210, 1510, angle,
speed] # [1180, 1485, angle, speed] no push
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.out_cylinders()
self.take_cylinder_outside()
self.take_cylinder_inside()
parameters = [1120, 1440, angle, speed] # [1095, 1410, angle, speed]
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.out_cylinders()
self.take_cylinder_outside()
self.take_cylinder_inside()
speed = 4
self.collision_belts = False
logging.info(self.send_command('lift_up', [2.55]))
#self.useLift = True
parameters = [875, 1150, angle, speed]
self.go_to_coord_rotation(parameters)
angle = np.pi
parameters = [300, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.on_sucker()
self.take_cylinder_outside()
self.rotate_cylinder_horizonal()
parameters = [160, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [350, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
self.take_cylinder_inside('r')
parameters = [350, 1080, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [180, 1080, angle, speed]
self.go_to_coord_rotation(parameters)
self.take_cylinder_outside()
self.off_sucker()
parameters = [300, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.on_sucker()
self.take_cylinder_outside()
self.rotate_cylinder_vertical()
parameters = [160, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [350, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
self.take_cylinder_inside('l')
parameters = [350, 960, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [180, 960, angle, speed]
self.go_to_coord_rotation(parameters)
self.take_cylinder_outside()
self.off_sucker()
parameters = [300, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.on_sucker()
self.take_cylinder_outside()
self.rotate_cylinder_horizonal()
parameters = [160, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [350, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
self.take_cylinder_inside('r')
parameters = [350, 840, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [180, 840, angle, speed]
self.go_to_coord_rotation(parameters)
self.take_cylinder_outside()
self.off_sucker()
def without_last(self, speed=1):
signal.signal(signal.SIGALRM, rb.funny_action)
signal.alarm(90)
angle = 3 * np.pi / 2
#self.rotate_cylinder_vertical()
parameters = [870, 160, angle, 6]
self.go_to_coord_rotation(parameters)
parameters = [1150, 300, angle, 4]
self.go_last(parameters)
time.sleep(0.4)
parameters = [1150, 290, angle, 4]
self.go_to_coord_rotation(parameters)
self.sensor_range = 60
#parameters = [1150, 250, angle, speed]
#self.go_to_coord_rotation(parameters)
self.on_sucker()
self.take_cylinder_outside()
parameters = [1150, 160, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1150, 340, angle, 1]
self.go_to_coord_rotation(parameters)
### New concevik
if (self.is_cylinder_taken() == 0):
parameters = [1150, 160, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1150, 340, angle, 1]
self.go_to_coord_rotation(parameters)
self.pick_up(self.color == 'blue')
self.on_sucker()
self.take_cylinder_outside()
parameters = [1150, 160, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1150, 340, angle, 1]
self.go_to_coord_rotation(parameters)
### New concevik
if (self.is_cylinder_taken() == 0):
parameters = [1150, 160, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1150, 340, angle, 1]
self.go_to_coord_rotation(parameters)
self.pick_up(self.color == 'blue')
self.on_sucker()
self.take_cylinder_outside()
self.rotate_cylinder_vertical()
parameters = [1150, 160, angle, speed]
self.go_to_coord_rotation(parameters)
speed = 4
parameters = [1150, 340, angle, 1]
self.go_to_coord_rotation(parameters)
### New concevik
if (self.is_cylinder_taken() == 0):
parameters = [1150, 160, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1150, 340, angle, 1]
self.go_to_coord_rotation(parameters)
#self.rotate_cylinder_horizonal()
###### FIELD ADDITION
self.take_cylinder_inside()
#self.pick_up()
speed = 4
def without_last_r(self, speed=1):
angle = 3 * np.pi / 2
speed = 4
parameters = [1250, 1000, angle, speed]
self.go_to_coord_rotation(parameters)
angle = 3 * np.pi / 4
parameters = [1320, 1520, angle, speed]
self.go_to_coord_rotation(parameters)
speed = 6
self.collision_belts = True
parameters = [1350, 1650, angle, speed]
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.take_cylinder_outside()
self.off_sucker()
self.take_cylinder_inside()
parameters = [1230, 1540, angle,
speed] # [1250, 1560, angle, speed] no push
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.out_cylinders()
#KL#self.take_cylinder_outside()
#self.take_cylinder_inside()
### PUSHING
#parameters = [1180, 1485, angle, speed]
#self.go_to_coord_rotation(parameters)
#self.take_cylinder_outside()
#parameters = [1300, 1600, angle, speed]
#self.go_to_coord_rotation(parameters)
#KL#self.take_cylinder_inside()
#########
parameters = [1120, 1440, angle, speed] # [1095, 1410, angle, speed]
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.out_cylinders()
#KL#self.take_cylinder_outside()
#KL#self.take_cylinder_inside()
speed = 4
self.collision_belts = False
logging.info(self.send_command('lift_up', [2.55]))
parameters = [875, 1150, angle, speed]
self.go_to_coord_rotation(parameters)
angle = np.pi
parameters = [300, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.on_sucker()
self.take_cylinder_outside()
self.rotate_cylinder_horizonal()
### New concevik
while (self.is_cylinder_taken() == 0):
#self.take_cylinder_outside()
parameters = [145, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [350, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
self.take_cylinder_inside('r')
parameters = [350, 1080, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [175, 1080, angle, speed]
self.go_to_coord_rotation(parameters)
self.take_cylinder_outside()
self.off_sucker()
parameters = [300, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.on_sucker()
#self.take_cylinder_outside()
self.rotate_cylinder_vertical()
### New concevik
while (self.is_cylinder_taken() == 0):
#self.take_cylinder_outside()
parameters = [145, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [350, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
self.take_cylinder_inside('l')
parameters = [350, 960, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [175, 960, angle, speed]
self.go_to_coord_rotation(parameters)
self.take_cylinder_outside()
self.off_sucker()
parameters = [300, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
self.go_to_coord_rotation(parameters)
self.on_sucker()
#self.take_cylinder_outside()
self.rotate_cylinder_horizonal()
### New concevik
while (self.is_cylinder_taken() == 0):
#self.take_cylinder_outside()
parameters = [145, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [350, 1350, angle, speed]
self.go_to_coord_rotation(parameters)
self.take_cylinder_inside('r')
parameters = [350, 840, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [175, 840, angle, speed]
self.go_to_coord_rotation(parameters)
self.take_cylinder_outside()
self.off_sucker()
def bad_move(self, speed=1):
angle = 0.0
parameters = [1800, 1000, angle, speed]
self.go_to_coord_rotation(parameters)
def funny_action(self, signum, frame):
self.off_sucker()
logging.info(self.send_command('stopAllMotors'))
logging.critical('FUNNNY ACTION')
exit()
def collisionTest(self, speed=1):
angle = 3 * np.pi / 2
while True:
parameters = [1145, 400, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [945, 600, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1145, 800, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1345, 1000, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1545, 800, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1745, 600, angle, speed]
self.go_to_coord_rotation(parameters)
parameters = [1545, 300, angle, speed]
self.go_to_coord_rotation(parameters)
def cylinder_test(self):
#### Cylinder test
self.on_sucker()
self.take_cylinder_outside()
time.sleep(2)
print "Before " + str(self.is_cylinder_taken())
if (True):
self.pick_up()
print self.is_cylinder_taken()
self.on_sucker()
self.take_cylinder_outside()
time.sleep(2)
print "Before " + str(self.is_cylinder_taken())
if (True):
self.pick_up()
print self.is_cylinder_taken()
self.on_sucker()
self.take_cylinder_outside()
time.sleep(2)
print "Before " + str(self.is_cylinder_taken())
if (True):
self.pick_up()
print self.is_cylinder_taken()
time.sleep(4)
self.out_cylinders()
self.out_cylinders()
self.out_cylinders()
return
save_svg()
save_csv()
# plt.ion()
pos_x = 0
pos_y = 0
step_index = 2
color_index = 0
orientation = 0
dataset = []
plots = []
laser = HokuyoLX()
fig, ax = plt.subplots()
arrow_obj = Arrow(pos_x,
pos_y,
1000 * math.sin(orientation),
1000 * math.cos(orientation),
width=200,
color="red")
arrow = ax.add_patch(arrow_obj)
text = plt.text(0,
1,
'x: %d y: %d step = %d' % (pos_x, pos_y, STEPS[step_index]),
transform=ax.transAxes)
ax.set_xlim(-DMAX, DMAX)
ax.set_ylim(-DMAX, DMAX)
ax.grid(True)
"""
def __init__(self,
lidar_on=True,
small=True,
init_coord=[900, 200, np.pi / 2],
color='yellow'):
# Cylinder Staff
self.coll_go = False
##################
self.color = color
self.status = False
self.cur_state = 0 # 0-neutral,1-suck,2-throw
self.sensor_range = 35
self.collision_d = 16
self.coll_ind = -1
self.collision_avoidance = True
self.localisation = Value('b', True)
self.sensors_places = [
np.pi / 2, 0, 3 * np.pi / 2, np.pi, 3 * np.pi / 2, 0, np.pi,
np.pi / 2, 0, 0
]
self.filter_queue = [np.array([False] * len(self.sensors_places))] * 3
self.sensors_map = {
1: (0, np.pi / 4),
5: (0, np.pi / 4),
7: (np.pi / 4, 3 * np.pi / 4),
0: (np.pi / 4, 3 * np.pi / 4),
6: (3 * np.pi / 4, 5 * np.pi / 4),
3: (3 * np.pi / 4, 5 * np.pi / 4),
2: (5 * np.pi / 4, 7 * np.pi / 4),
4: (5 * np.pi / 4, 7 * np.pi / 4),
8: (7 * np.pi / 4, 2 * np.pi),
9: (7 * np.pi / 4, 2 * np.pi)
}
self.lidar_on = lidar_on
self.map = np.load('npmap.npy')
if lidar_on:
logging.debug('lidar is connected')
# add check for lidar connection
try:
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
except:
self.lidar_on = False
self.localisation = Value('b', False)
logging.warning('lidar is not connected')
# self.x = 170 # mm
# self.y = 150 # mm
# self.angle = 0.0 # pi
#850 170 3p/2
# 900 200 np.pi/2
# 400, 850, 0.
self.coords = Array('d', rev_field(init_coord, self.color))
#else:
#driver.PORT_SNR = '325936843235' # need change
#self.coords = Array('d', rev_field([170, 170, 0], self.color))
self.input_queue = Queue()
self.loc_queue = Queue()
self.fsm_queue = Queue() # 2000,25,25,0.1
self.PF = pf.ParticleFilter(particles=2000,
sense_noise=25,
distance_noise=30,
angle_noise=0.15,
in_x=self.coords[0],
in_y=self.coords[1],
in_angle=self.coords[2],
input_queue=self.input_queue,
out_queue=self.loc_queue,
color=self.color)
# coords sharing procces
self.p3 = Process(target=app.run, args=("0.0.0.0", ))
# driver process
print "Paricle filter On"
self.dr = driver.Driver(self.input_queue, self.fsm_queue,
self.loc_queue)
print "Driver On"
self.p = Process(target=self.dr.run)
print "Process driver Init"
self.p.start()
print "Process driver Start"
self.p2 = Process(target=self.PF.localisation,
args=(self.localisation, self.coords,
self.get_raw_lidar))
print "Process npParticle Init"
logging.info(self.send_command('echo', 'ECHO'))
logging.info(
self.send_command('setCoordinates', [
self.coords[0] / 1000., self.coords[1] / 1000., self.coords[2]
]))
if self.lidar_on:
self.p2.start()
print "Process npParticle Start"
else:
self.localisation = Value('b', False)
print "Robot __init__ done!"
def write_conf():
global sensor_ip
global sensor_port
global osc_host_ip
global osc_host_port
global area_left
global area_right
global map_left
global map_right
global blob_size_threshold
global marker_angual_interval
global marker_distance_interval
global laser
global errFlag
cf = configparser.ConfigParser()
sensor_ip = sVar_sensor_ip.get()
sensor_port = sVar_sensor_port.get()
osc_host_ip = sVar_osc_host_ip.get()
osc_host_port = sVar_osc_host_port.get()
area_left = sVar_area_left.get()
area_right = sVar_area_right.get()
area_far = sVar_area_far.get()
area_near = sVar_area_near.get()
map_left = sVar_map_left.get()
map_right = sVar_map_right.get()
map_near = sVar_map_near.get()
map_far = sVar_map_far.get()
marker_angual_interval = sVar_marker_angual_interval.get()
marker_distance_interval = sVar_marker_distance_interval.get()
blob_size_threshold = sVar_blob_size_threshold.get()
cf.read("config.conf")
cf.set("SENSOR", "sensor_ip", sensor_ip)
cf.set("SENSOR", "sensor_port", str(sensor_port))
cf.set("OSC", "OSC_host_ip", osc_host_ip)
cf.set("OSC", "OSC_host_port", str(osc_host_port))
cf.set("AREA", "area_left", str(area_left))
cf.set("AREA", "area_right", str(area_right))
cf.set("AREA", "area_near", str(area_near))
cf.set("AREA", "area_far", str(area_far))
cf.set("AREA", "map_near", str(map_near))
cf.set("AREA", "map_far", str(map_far))
cf.set("AREA", "map_left", str(map_left))
cf.set("AREA", "map_right", str(map_right))
cf.set("MARKER", "marker_distance_interval", str(marker_distance_interval))
cf.set("MARKER", "marker_angual_interval", str(marker_angual_interval))
cf.set("MARKER", "blob_size_threshold", str(blob_size_threshold))
# TO DO
#osc_terminate()
#osc_startup()
#osc_udp_client(str(osc_host_ip), int(osc_host_port), "osc")
try:
laser.close()
laser = HokuyoLX(addr=(str(sensor_ip), int(sensor_port)),
info=False,
buf=1024,
time_tolerance=1000,
convert_time=False)
msg_text.insert(1.0,
"[MSG]Sensor connected " + time.strftime("%X") + "\n")
msg_text.tag_remove("RED", "1.0")
errFlag = False
except Exception as e:
msg_text.insert(
1.0, "[ERR]Sensor connect failed " + time.strftime("%X") + "\n")
msg_text.tag_add("RED", "1.0")
errFlag = True
with open('config.conf', 'w') as configfile:
cf.write(configfile)
read_conf()
def __init__(self, lidar_on=True, small=True, color='yellow'):
# Cylinder Staff
self.cylinders = 0
self.cyl_prepare = [0.215, 0.143, 0.143]
self.cyl_up = [0.523, 0.64, 0.66]
self.cyl_down = [-0.79, -0.67, -0.72]
self.coll_go = False
self.useLift = False
self.collision_belts = False
##################
self.color = color
self.sensor_range = 35
self.collision_d = 9
self.coll_ind = -1
self.collision_avoidance = True
self.localisation = Value('b', True)
if small:
self.sensors_places = [
0, 0, 0, np.pi, np.pi / 2, 3 * np.pi / 2, 0, 0, 0
]
self.sensors_map = {
0: (0, np.pi / 3),
7: (7 * np.pi / 4, 2 * np.pi),
3: (np.pi * 0.7, np.pi * 1.3),
1: (5 / 3. * np.pi, 2 * np.pi),
2: (0, np.pi * 1 / 4.),
6: (7 / 4. * np.pi, 2 * np.pi),
8: (0, np.pi / 4),
4: (np.pi / 4, 3 * np.pi / 4),
5: (np.pi * 5 / 4, 7 * np.pi / 4)
}
self.lidar_on = lidar_on
self.map = np.load('npmap.npy')
if lidar_on:
logging.debug('lidar is connected')
# add check for lidar connection
try:
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
except:
self.lidar_on = False
self.localisation = Value('b', False)
logging.warning('lidar is not connected')
# self.x = 170 # mm
# self.y = 150 # mm
# self.angle = 0.0 # pi
if small:
#850 170 3p/2
#
self.coords = Array(
'd', rev_field([170, 170, 3 * np.pi / 2], self.color))
else:
driver.PORT_SNR = '325936843235' # need change
self.coords = Array('d', rev_field([170, 170, 0], self.color))
self.input_queue = Queue()
self.loc_queue = Queue()
self.fsm_queue = Queue() # 2000,25,25,0.1
self.PF = pf.ParticleFilter(particles=2000,
sense_noise=25,
distance_noise=25,
angle_noise=0.1,
in_x=self.coords[0],
in_y=self.coords[1],
in_angle=self.coords[2],
input_queue=self.input_queue,
out_queue=self.loc_queue,
color=self.color)
# driver process
print "Paricle filter On"
self.dr = driver.Driver(self.input_queue, self.fsm_queue,
self.loc_queue)
print "Driver On"
self.p = Process(target=self.dr.run)
print "Process driver Init"
self.p.start()
print "Process driver Start"
self.p2 = Process(target=self.PF.localisation,
args=(self.localisation, self.coords,
self.get_raw_lidar))
print "Process npParticle Init"
logging.info(self.send_command('echo', 'ECHO'))
logging.info(
self.send_command('setCoordinates', [
self.coords[0] / 1000., self.coords[1] / 1000., self.coords[2]
]))
if self.lidar_on:
self.p2.start()
print "Process npParticle Start"
else:
self.localisation = Value('b', False)
#time.sleep(0.1)
logging.info(self.send_command('rotate_cylinder_vertical', [146.0]))
#if self.color == "yellow":
# self.rotate_cylinder_vertical()
#else:
# self.rotate_cylinder_horizonal()
print "Robot __init__ done!"
def __init__(self, ip="192.168.1.10", port=10940):
self.lidar = HokuyoLX(addr=(ip, port))
self.kalman = KalmanFilter(adaptive=False, dt=0.025)
