def __init__(self, performance: Performance,
safety_module: SafetyControlModule, simulation: Simulation):
self._performance = performance
self._safety_module = safety_module
self._Sim = simulation
self.wheel1a = self._Sim.wheel1a
self.wheel2a = self._Sim.wheel2a
self.wheel3a = self._Sim.wheel3a
self.wheel1b = self._Sim.wheel1b
self.wheel2b = self._Sim.wheel2b
self.wheel3b = self._Sim.wheel3b
self.slider1 = self._Sim.slider1
#time.sleep(6)
self.slider2 = self._Sim.slider2
#time.sleep(6)
self.slider3 = self._Sim.slider3
#time.sleep(6)
self.sonar1a2a3a = Sonar("1a2a3a")
self.sonar1a2b3a = Sonar("1a2b3a")
self.sonar1a2b3b = Sonar("1a2b3b")
self.sonar1a2a3b = Sonar("1a2a3b")
self.sonar1b2a3a = Sonar("1b2a3a")
self.sonar1b2b3a = Sonar("1b2b3a")
self.sonar1b2b3b = Sonar("1b2b3b")
self.sonar1b2a3b = Sonar("1b2a3b")
self.inertiaMeter = self._Sim.inertiaMeter
self._experience_memory = deque()
def main(robotIP, port=9559):
sonar = Sonar(robotIP, port)
controler = Controller()
motionProxy = ALProxy("ALMotion", robotIP, port)
postureProxy = ALProxy("ALRobotPosture", robotIP, port)
frame = motion.FRAME_ROBOT # maybe test TORSO?
last_theta = 0
memory_factor = 0.3
# Get ready, NAO!
motionProxy.wakeUp()
postureProxy.goToPosture("StandInit", 0.5)
start_t = time.time()
# Start walking (sonar appears to start reading correct values only after this)
motionProxy.moveToward(0.01, 0, 0)
time.sleep(0.5)
# Main loop
while (time.time() - start_t <= 180):
# Sensor reading step
x_velocity = 1
y_velocity = 0
theta_correction = 0
lread, rread = sonar.getSampleReading(n_readings=10)
# Course correction decision step (theta is positive counterclockwise)
print("Sonar readings (lread, rread) = (%.2f, %.2f)" % (lread, rread))
theta_correction, v_correction = controler.compute(lread=lread, rread=rread, last_theta=last_theta)
print("Theta %.2f; Velocity %.2f" % (theta_correction, v_correction))
# Course correction execution step
#motionProxy.moveToward(v_correction, y_velocity, theta_correction)
motionProxy.moveToward(v_correction, y_velocity, (memory_factor * last_theta + (1 - memory_factor) * theta_correction))
last_theta = theta_correction
time.sleep(0.5)
# Stop motion
motionProxy.moveToward(0, 0, 0) # I found this to be less motion breaking
motionProxy.stopMove()
postureProxy.goToPosture("StandInit", 0.2) # make him stand again
# Just to indicate we're finished
motionProxy.rest()
print("NAOvigation script finished.")
def main(robotIP, port=9559):
sonar = Sonar(robotIP, port)
controler = Controller()
motionProxy = ALProxy("ALMotion", robotIP, port)
postureProxy = ALProxy("ALRobotPosture", robotIP, port)
frame = motion.FRAME_ROBOT # maybe test TORSO?
# Get ready, NAO!
motionProxy.wakeUp()
postureProxy.goToPosture("StandInit", 0.5)
start_t = time.time()
# Start walking (sonar appears to start reading correct values only after this)
motionProxy.moveToward(0.01, 0, 0)
time.sleep(0.5)
# Main loop
while (time.time() - start_t <= 180):
# make course corrections
# async walk
# little sleep
# Sensor reading step
x_velocity = 1
y_velocity = 0
theta_correction = 0
lread, rread = sonar.getSampleReading(n_readings=10)
# Course correction decision step (theta is positive counterclockwise)
print("Sonar readings (lread, rread) = (%.2f, %.2f)" % (lread, rread))
theta_correction = controler.compute(lread=lread, rread=rread)
print("Theta %.2f" % theta_correction)
# Course correction execution step
motionProxy.moveToward(x_velocity, y_velocity, theta_correction)
time.sleep(0.5)
# Stop motion
motionProxy.stopMove()
# Just to indicate we're finished
motionProxy.rest()
print("NAOvigation script finished.")
def __init__(self, logger=None):
"""
Build monitor with GPS and Sonar
:param logger:
"""
# Object level holders for data
self._rmc = None
self._dpt = None
self._mtw = None
# Option to use test logger or recording logger
self._logger = self.logger
if logger is not None:
self._logger = logger
# self._tur = Turbidity()
# Get file for logging data
self._file_name = self.getFileName()
self._file = self.openFile()
self._GPS = None
self._Sonar = None
# Get list of ports and check who is using
# avoids problems when switch USB location
port_list = getPorts()
if 'GPS' in port_list.keys():
self._GPS = GPS(self._logger, port_list['GPS'])
else:
# Fail gracefully
self._GPS = GPS(self._logger)
if 'Sonar' in port_list.keys():
self._Sonar = Sonar(self._logger, port_list['Sonar'])
else:
# Fail gracefully
self._Sonar = Sonar(self._logger)
print("Serial Port Usage: ", port_list)
# Start data looping
while True:
self._GPS.get()
self._Sonar.get()
def analyseSonar(self):
# Start SonarQube and API for it
sonar = Sonar(sonarScannerImg='sonar-maven')
if not sonar.isSonarQubeRunning():
sonar.startSonarQube()
api = API()
# Get the save dir and make the git path with it
save_dir = self.sonar_save_dir_input.text()
if os.path.isabs(save_dir):
git_full_path = os.path.join(save_dir,
self.active_repo['created_in'])
else:
git_full_path = os.path.join(os.getcwd(),
self.active_repo['created_in'])
print("active_repo['created_in']:", git_full_path)
wait('sonarqube', sonar.isSonarQubeRunning, timeout=5)
# Running Sonar scanner(with sanity check)
before = set(api.projects())
print(before)
sonar.runSonarScanner(git_full_path, 'clean', 'verify', 'sonar:sonar')
wait('analysis',
lambda: before.symmetric_difference(api.projects()),
timeout=1)
project_key = before.symmetric_difference(api.projects()).pop()
print('project_key:', project_key)
wait('issues', lambda: any(api.issues(project=project_key)), timeout=1)
# Get the issues from API, parse and save as csv
issues = list(api.issues(project=project_key))
issue_file = os.path.join(
save_dir, self.active_repo['created_in'] + '_issues.csv')
analysisParseList(
issues, issue_file) # NOTE: NOT TESTED! Parse and write to file
# Get the git commit data
commit_file = os.path.join(
save_dir, self.active_repo['created_in'] + '_commits.csv')
# TODO: get commit data for each commit associated with an issue
# and write to file
print(len(issues))
print(issue_file)
print(commit_file)
api.delete_project(project_key)
def handleMouse(self, player, gun):
"""
Test for a mouse press, and handle a press exists.
@param player: The player's object.
@param gun: The gun handler.
"""
(b1, b2, b3) = pygame.mouse.get_pressed()
if b1 and gun.canShoot(gun.currentState):
self.bulletList.add(gun.shoot())
elif b2:
print "stub"
elif b3:
if int(pygame.time.get_ticks()
) >= self.sonarTimer + self.sonarTimerDelay:
self.sonarList.add(Sonar(self.screen, player, self.sfx))
self.sonarTimer = int(pygame.time.get_ticks())
import RPi.GPIO as GPIO
from Motor import Motor
from Encoder import Encoder
from Sonar import Sonar
from Supervisor import Supervisor
from test import *
if __name__ == '__main__':
# MCP23017
mcp = Adafruit_MCP230XX(busnum=1, address=0x20, num_gpios=16)
# Objects declaration
m = Motor(mcp)
e = Encoder()
s = Sonar(mcp)
sup = Supervisor(m, e, s)
# Initialisation
m.init_motor()
e.initEncoder()
s.initSonar()
# Ultrasonic sensors activation
s.activate_us()
# Vacuum cleaner starting
m.vacuum_cleaner_start()
m.change_speed_vacuum(50)
# Temporising before starting
from Robot import Robot
from Simulation import Simulation
#from INS import INS
from Sonar import Sonar
import dill
start = time.time()
environment = Environment("Environment_data/area_4/map.npz")
mission = Mission(environment)
x_start = environment.cloud[0, 0, 0]
y_start = environment.cloud[0, 0, 1]
sonar = Sonar(environment, [200, 200], [100, 100])
robot = Robot([x_start, y_start, 0], sonar=sonar)
simulation = Simulation(0.5, environment, [mission], robot)
simulation.run()
print("Time elapsed : {} seconds.".format(time.time() - start))
filename = "Environment_data/area_4/cubic_200x200_100x100_without_ins.pkl"
dill.dump_session(filename)
#robot.plot_history()
#robot.plot_uncertainty()
import time
import RPi.GPIO as GPIO
from Servo import Servo
from Sonar import Sonar
if __name__ == "__main__":
GPIO.setmode(GPIO.BOARD)
sonar1 = Sonar(11, 13)
servo1 = Servo(15, 50, 17)
# move to 45 degrees
servo1.move(45)
time.sleep(1)
# take a reading
distance45 = sonar1.getDistance()
print(distance45)
# move to 45 degrees
servo1.move(90)
time.sleep(1)
# take a reading
distance90 = sonar1.getDistance()
print(distance90)
# move to 45 degrees
servo1.move(135)
time.sleep(1)
# take a reading
distance135 = sonar1.getDistance()
print(distance135)
lcd.lcd_clear()
lcd.display_date(1)
if __name__ == "__main__":
try:
# GPIO handler for 1st LCD display transistor
transistor_LCD = Handle_GPIO(12)
# GPIO handler for 2nd LCD display transistor
transistor_LCD_ground = Handle_GPIO(40)
# GPIO handler for green LED
ledAuthorized = Handle_GPIO(13)
# GPIO handler for red led
ledDenied = Handle_GPIO(15)
components_active()
# GPIO handler for ultrasonic range device
sonar = Sonar(16, 18, 300)
# Create an object of the class MFRC522
mfrc = MFRC522.MFRC522()
api = API_Requests()
lcd = LCD_display()
components_idle()
while(True):
time.sleep(2)
distance = sonar.getSonar()
print ("The distance is : %.2f cm"%(distance))
if distance < 50:
components_active()
while(True):
reset()
distance = sonar.getSonar()
if distance > 50 :
def main(args):
sonar = Sonar()
if not sonar.isSonarQubeRunning():
sonar.startSonarQube()
print("SonarQube will be started. (This could take a while)")
api = API()
git_dir, *_ = os.path.basename(args.repo).rpartition('.git')
relative_git_path = os.path.join(args.o, git_dir)
if os.path.exists(relative_git_path):
print(
f'The path `{relative_git_path}` exists, can\'t load the repository',
file=sys.stderr)
print(f'To solve either:', file=sys.stderr)
print(' a) remove the path', file=sys.stderr)
print(' b) choose different save directory', file=sys.stderr)
exit(1)
try:
git = GitRepo(relative_git_path)
ok = git.pullRepoContents(args.repo)
assert ok
if os.path.isabs(args.o):
git_full_path = os.path.join(args.o, git_dir)
else:
git_full_path = os.path.join(os.getcwd(), args.o, git_dir)
print('git_dir:', git_full_path)
wait('sonarqube', sonar.isSonarQubeRunning, timeout=5)
before = set(api.projects())
print(before)
project_key = None
scan_started = False
try:
sonar.runSonarScanner(git_full_path, 'clean', 'verify',
'sonar:sonar')
scan_started = True
# NOTE: this is quite hacky and fragile
wait('analysis',
lambda: before.symmetric_difference(api.projects()),
timeout=1)
project_key = before.symmetric_difference(api.projects()).pop()
print('project_key:', project_key)
# NOTE: breaks if there is no issues
wait('issues',
lambda: any(api.issues(project=project_key)),
timeout=1)
issue_file = os.path.join(args.o, git_dir + '_issues.csv')
git_file = os.path.join(args.o, git_dir + '_git.csv')
issues = api.issues(project=project_key)
analysisParseList(
issues,
issue_file) # NOTE: NOT TESTED! Parse and write to file
git.gitParse(git_file) # NOTE: NOT TESTED!
finally:
if scan_started:
if project_key is None:
wait('analysis',
lambda: before.symmetric_difference(api.projects()),
timeout=0.2)
project_key = before.symmetric_difference(
api.projects()).pop()
# remove analysed project from sonarqube
api.delete_project(project_key)
finally:
# remove git repo
if os.path.isdir(relative_git_path):
shutil.rmtree(relative_git_path)
class Monitor(object):
def __init__(self, logger=None):
"""
Build monitor with GPS and Sonar
:param logger:
"""
# Object level holders for data
self._rmc = None
self._dpt = None
self._mtw = None
# Option to use test logger or recording logger
self._logger = self.logger
if logger is not None:
self._logger = logger
# self._tur = Turbidity()
# Get file for logging data
self._file_name = self.getFileName()
self._file = self.openFile()
self._GPS = None
self._Sonar = None
# Get list of ports and check who is using
# avoids problems when switch USB location
port_list = getPorts()
if 'GPS' in port_list.keys():
self._GPS = GPS(self._logger, port_list['GPS'])
else:
# Fail gracefully
self._GPS = GPS(self._logger)
if 'Sonar' in port_list.keys():
self._Sonar = Sonar(self._logger, port_list['Sonar'])
else:
# Fail gracefully
self._Sonar = Sonar(self._logger)
print("Serial Port Usage: ", port_list)
# Start data looping
while True:
self._GPS.get()
self._Sonar.get()
def getFileName(self):
"""
Create file name for next log
Assumes format 'Log_000.txt'
Will incriment the number for the next file
:return: the filename
"""
import glob
filename = '/home/pi/Data/Log_000.txt'
files = glob.glob("/home/pi/Data/*.txt")
if len(files) > 0:
files.sort()
last_file = files[-1]
next_nbr = int(last_file.split('.')[0].split('_')[1])
next_nbr += 1
filename = "{}{}{}".format('/home/pi/Data/Log_',
format(next_nbr, '0>3'), '.txt')
print("Logging to:", filename)
return filename
def openFile(self):
"""
Open a file for logging data
:return: the file descriptor
"""
return open(self._file_name, 'a')
def save(self, rec):
"""
appandes the characters in rec into the file
:param rec: the characters to be saved
:return: None, but also prints the rec
"""
self._file.write(rec)
self._file.flush()
print(rec)
def logger(self, values):
"""
Callback to get messages, when have all three - save them as one record. A log value is printed
when all measurement values have non None values.
:param values:
:return: None
"""
# print "Values", values
if values['name'] is None:
pass
if values['name'] == 'GPRMC': # Time and location
self._rmc = values['data']
elif values['name'] == 'SDDBT': # Depth
self._dpt = values['data']
elif values['name'] == 'YXMTW': # Temperature
self._mtw = values['data']
# Save record when have all parts
if (self._rmc is not None) and (self._dpt
is not None) and (self._mtw
is not None):
self.finishLogging()
# clear data for next round of sentences
self._rmc = None
self._dpt = None
self._mtw = None
def finishLogging(self):
"""
save the logging data to the file
:return: None
"""
""" Save data to file """
# Get non serial data
# temp1 = getTempC(0)
# temp2 = getTempC(1)
# tb = self._tur.getRaw()
rec = "{}, {}, {}, {}, {}".format(self._rmc['time'], self._rmc['lat'],
self._rmc['lon'], self._dpt['depth'],
self._mtw["temp"])
# print rec
self.save(rec)
matcher = Matcher(environment, "SURF")
print(matcher)
# matcher.plot()
[Gx, Gy] = environment.gradient()
ori_hsv_img = environment.hsv_gradient(Gx, Gy, environment.cloud[:, :, 2])
# =============================================================================
# Test partial view
# =============================================================================
sonar = Sonar(environment, [600, 600])
T = np.array([450103.16, 4951485.2])
# T = np.array([450500, 4951800])
start = time.time()
view, _ = sonar.generateView(T, np.pi/4, True)
print('Time elapsed {}'.format(time.time() - start))
test = view[:,:,2]
# plt.figure("View")
# plt.imshow(test.T)
# plt.show()
import RPi.GPIO as GPIO
from Motor import Motor
from Encoder import Encoder
from Sonar import Sonar
from Supervisor import Supervisor
from test import *
if __name__ == '__main__':
# MCP23017
mcp = Adafruit_MCP230XX(busnum=1, address=0x20, num_gpios=16)
# Objects declaration
m = Motor(mcp)
e = Encoder()
s = Sonar(mcp)
sup = Supervisor(m, e, s)
# Initialisation
m.init_motor()
e.initEncoder()
s.initSonar()
# Ultrasonic sensors activation
s.activate_us()
# Vacuum cleaner starting
m.vacuum_cleaner_start()
m.change_speed_vacuum(50)
# Temporising before starting