def delete_account(self, user):
"""
Delete the 'user'.
Clear utmp first, to avoid error.
Removes the /etc/sudoers.d/waagent file.
"""
userentry = None
try:
userentry = pwd.getpwnam(user)
except (EnvironmentError, KeyError):
pass
if userentry is None:
logger.error("DeleteAccount: " + user + " not found.")
return
uidmin = None
try:
if os.path.isfile("/etc/login.defs"):
uidmin = int(
ext_utils.get_line_starting_with("UID_MIN", "/etc/login.defs").split()[1])
except (ValueError, KeyError, AttributeError, EnvironmentError):
pass
if uidmin is None:
uidmin = 100
if userentry[2] < uidmin:
logger.error(
"DeleteAccount: " + user + " is a system user. Will not delete account.")
return
# empty contents of utmp to prevent error if we are the 'user' deleted
ext_utils.run_command_and_write_stdout_to_file(['echo'], '/var/run/utmp')
ext_utils.run(['rmuser', '-y', user], chk_err=False)
try:
os.remove(self.sudoers_dir_base + "/sudoers.d/waagent")
except EnvironmentError:
pass
return
def delete_account(self, user):
"""
Delete the 'user'.
Clear utmp first, to avoid error.
Removes the /etc/sudoers.d/waagent file.
"""
user_entry = None
try:
user_entry = pwd.getpwnam(user)
except (KeyError, EnvironmentError):
pass
if user_entry is None:
logger.error("DeleteAccount: " + user + " not found.")
return
uid_min = None
try:
uid_min = int(ext_utils.get_line_starting_with("UID_MIN", "/etc/login.defs").split()[1])
except (ValueError, KeyError, AttributeError, EnvironmentError):
pass
if uid_min is None:
uid_min = 100
if user_entry[2] < uid_min:
logger.error(
"DeleteAccount: " + user + " is a system user. Will not delete account.")
return
ext_utils.run(['rm', '-f', '/var/run/utmp']) # Delete utmp to prevent error if we are the 'user' deleted
ext_utils.run(['userdel', '-f', '-r', user])
try:
os.remove("/etc/sudoers.d/waagent")
except EnvironmentError:
pass
return
def restart_ssh_service(self):
"""
Service call to re(start) the SSH service
"""
ssh_restart_cmd = [self.service_cmd, self.ssh_service_name, self.ssh_service_restart_option]
ret_code = ext_utils.run(ssh_restart_cmd)
if ret_code != 0:
logger.error("Failed to restart SSH service with return code:" + str(ret_code))
return ret_code
def run_command_get_output(cmd, chk_err=True, log_cmd=True):
"""
Wrapper for subprocess.check_output.
Execute 'cmd'. Returns return code and STDOUT, trapping expected exceptions.
Reports exceptions to Error if chk_err parameter is True
"""
if log_cmd:
logger.log_if_verbose(cmd)
try:
output = subprocess.check_output(cmd,
stderr=subprocess.STDOUT,
shell=False)
except subprocess.CalledProcessError as e:
if chk_err and log_cmd:
logger.error('CalledProcessError. Error Code is ' +
str(e.returncode))
logger.error('CalledProcessError. Command string was ' + str(cmd))
logger.error('CalledProcessError. Command result was ' +
(e.output[:-1]).decode('latin-1'))
return e.returncode, e.output.decode('latin-1')
except EnvironmentError as e:
if chk_err and log_cmd:
logger.error('CalledProcessError. Error message is ' + str(e))
return e.errno, str(e)
# noinspection PyUnboundLocalVariable
return 0, output.decode('latin-1')
def add_extension_event(name, op, is_success, duration=0, version="1.0", message="", extension_type="",
is_internal=False):
event = ExtensionEvent()
event.Name = name
event.Version = version
event.IsInternal = is_internal
event.Operation = op
event.OperationSuccess = is_success
event.Message = message
event.Duration = duration
event.ExtensionType = extension_type
try:
event.save()
except OSError:
logger.error("Error " + traceback.format_exc())
def main():
logger.log("%s started to handle." % ExtensionShortName)
try:
for a in sys.argv[1:]:
if re.match("^([-/]*)(disable)", a):
disable()
elif re.match("^([-/]*)(uninstall)", a):
uninstall()
elif re.match("^([-/]*)(install)", a):
install()
elif re.match("^([-/]*)(enable)", a):
enable()
elif re.match("^([-/]*)(update)", a):
update()
except Exception as e:
err_msg = "Failed with error: {0}, {1}".format(e, traceback.format_exc())
logger.error(err_msg)
def __init__(self, wala_config_file):
self.values = dict()
if not os.path.isfile(wala_config_file):
raise ValueError("Missing configuration in {0}".format(wala_config_file))
try:
for line in ext_utils.get_file_contents(wala_config_file).split('\n'):
if not line.startswith("#") and "=" in line:
parts = line.split()[0].split('=')
value = parts[1].strip("\" ")
if value != "None":
self.values[parts[0]] = value
else:
self.values[parts[0]] = None
# when get_file_contents returns none
except AttributeError:
logger.error("Unable to parse {0}".format(wala_config_file))
raise
return
def getDirectionsFromIntersections(self, intersections):
directions = []
# for each cople of nodes compute cardinal points direction between them
prevNode = intersections[0]
for currentNode in intersections[1:]:
# check if X has changed
if currentNode[0] > prevNode[0]:
directions.append(SOUTH)
elif currentNode[0] < prevNode[0]:
directions.append(NORD)
# check if Y has changed
elif currentNode[1] > prevNode[1]:
directions.append(EAST)
elif currentNode[1] < prevNode[1]:
directions.append(WEST)
else:
logger.error("Invalid intersetions")
# go to next couple of node
prevNode = currentNode
return directions
def run_command_and_write_stdout_to_file(command, output_file):
# meant to replace commands of the nature command > output_file
try:
p = subprocess.Popen(command, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=False)
stdout, stderr = p.communicate()
except OSError as e:
logger.error('CalledProcessError. Error message is ' + str(e))
return e.errno
if p.returncode != 0:
logger.error('CalledProcessError. Error Code is ' + str(p.returncode))
logger.error('CalledProcessError. Command string was ' + ' '.join(command))
logger.error(
'CalledProcessError. Command result was stdout: ' + str(stdout) + ' stderr: ' + str(stderr))
return p.returncode
set_file_contents(output_file, stdout)
return 0
def create_account(self, user, password, expiration, thumbprint):
"""
Create a user account, with 'user', 'password', 'expiration', ssh keys
and sudo permissions.
Returns None if successful, error string on failure.
"""
userentry = None
try:
userentry = pwd.getpwnam(user)
except (EnvironmentError, KeyError):
pass
uidmin = None
try:
uidmin = int(ext_utils.get_line_starting_with("UID_MIN", "/etc/login.defs").split()[1])
except (ValueError, KeyError, AttributeError, EnvironmentError):
pass
if uidmin is None:
uidmin = 100
if userentry is not None and userentry[2] < uidmin and userentry[2] != self.CORE_UID:
logger.error(
"CreateAccount: " + user + " is a system user. Will not set password.")
return "Failed to set password for system user: "******" (0x06)."
if userentry is None:
command = ['useradd', '--create-home', '--password', '*', user]
if expiration is not None:
command += ['--expiredate', expiration.split('.')[0]]
if ext_utils.run(command):
logger.error("Failed to create user account: " + user)
return "Failed to create user account: " + user + " (0x07)."
else:
logger.log("CreateAccount: " + user + " already exists. Will update password.")
if password is not None:
self.change_password(user, password)
try:
if password is None:
ext_utils.set_file_contents("/etc/sudoers.d/waagent", user + " ALL = (ALL) NOPASSWD: ALL\n")
else:
ext_utils.set_file_contents("/etc/sudoers.d/waagent", user + " ALL = (ALL) ALL\n")
os.chmod("/etc/sudoers.d/waagent", 0o440)
except EnvironmentError:
logger.error("CreateAccount: Failed to configure sudo access for user.")
return "Failed to configure sudo privileges (0x08)."
home = self.get_home()
if thumbprint is not None:
ssh_dir = home + "/" + user + "/.ssh"
ext_utils.create_dir(ssh_dir, user, 0o700)
pub = ssh_dir + "/id_rsa.pub"
prv = ssh_dir + "/id_rsa"
ext_utils.run_command_and_write_stdout_to_file(['ssh-keygen', '-y', '-f', thumbprint + '.prv'], pub)
ext_utils.set_file_contents(prv, ext_utils.get_file_contents(thumbprint + ".prv"))
for f in [pub, prv]:
os.chmod(f, 0o600)
ext_utils.change_owner(f, user)
ext_utils.set_file_contents(ssh_dir + "/authorized_keys", ext_utils.get_file_contents(pub))
ext_utils.change_owner(ssh_dir + "/authorized_keys", user)
logger.log("Created user account: " + user)
return None
# Camera:
HAX = 53 # The head angle of the camera (x)
HAY = 40 # The head angle of the camera (y)
# Camera Settings:
brightness = -0.1
saturation = 15
exposure = -1
##################################
# Camera and FPS counter setting #
##################################
if __name__ == "__main__":
if lockFile.is_locked():
logger.error("Lock File Locked!")
raise ValueError
else:
lockFile.lock()
try:
FPSCounter = FPS()
FPSCounter.start()
cam = cv2.VideoCapture(0)
cam.set(3, 640)
cam.set(4, 480)
# cam.set(cv2.cv.CV_CAP_PROP_BRIGHTNESS, brightness)
# cam.set(cv2.cv.CV_CAP_PROP_SATURATION, saturation)
# cam.set(cv2.cv.CV_CAP_PROP_EXPOSURE, exposure) # not working on the old camera
def run_send_stdin(cmd, cmd_input, chk_err=True, log_cmd=True):
"""
Wrapper for subprocess.Popen.
Execute 'cmd', sending 'input' to STDIN of 'cmd'.
Returns return code and STDOUT, trapping expected exceptions.
Reports exceptions to Error if chk_err parameter is True
"""
if log_cmd:
logger.log_if_verbose(str(cmd) + str(cmd_input))
subprocess_executed = False
try:
me = subprocess.Popen(cmd,
shell=False,
stdin=subprocess.PIPE,
stderr=subprocess.STDOUT,
stdout=subprocess.PIPE)
output = me.communicate(cmd_input)
subprocess_executed = True
except EnvironmentError as e:
if chk_err and log_cmd:
logger.error('CalledProcessError. Error Code is ' + str(e.errno))
logger.error('CalledProcessError. Command was ' + str(cmd))
logger.error('CalledProcessError. Command result was ' + str(e))
return 1, str(e)
if subprocess_executed and me.returncode != 0 and chk_err and log_cmd:
logger.error('CalledProcessError. Error Code is ' +
str(me.returncode))
logger.error('CalledProcessError. Command was ' + str(cmd))
logger.error('CalledProcessError. Command result was ' +
output[0].decode('latin-1'))
return me.returncode, output[0].decode('latin-1')
def run(self):
logger.info("Running..")
leftIsEmpty = False # flag used to detect left parking lot
rightIsEmpty = False # flag used to detect right parking lot
leftStartingPosition = 0.0 # length of the left parking lot
rightStartingPosition = 0.0 # length of the right parking lot
sideOfParkingLot = 0 # indicates the side of the parking lot found: -1 left, 1 right, 0 not found yet
actualTurn = 0
while self.driver.step() != -1:
## here goes code that should be executed each step ##
# get actual compass value
#compassValues = self.compass.getValues()
#logger.debug("COMPASS: "******"INIT status")
# set wheel angle
self.setAngle(0.0)
# set starting speed
self.setSpeed(0.5)
# set new status
self.setStatus(Status.TURN)
# skip to next cycle to ensure everything is working fine
continue
# FOLLOW LINE STATUS
if self.status == Status.FOLLOW_LINE:
# set cruise speed
self.setSpeed(0.5)
# compute new angle
newAngle = self.lineFollower.getNewSteeringAngle()
# logger.debug("new steering angle: " + str(newAngle))
# set new steering angle
if newAngle != UNKNOWN:
self.setAngle(newAngle)
else:
self.setStatus(Status.TURN)
# TURN STATUS
if self.status == Status.TURN:
if actualTurn < len(self.turns):
turn = self.turns[actualTurn]
self.setAngle(0.5 * turn * MAX_ANGLE)
else:
self.setStatus(Status.STOP)
# compute new angle
newAngle = self.lineFollower.getNewSteeringAngle()
# if line is found
if newAngle != UNKNOWN:
self.setStatus(Status.FOLLOW_LINE)
actualTurn += 1
continue
# FORWARD STATUS
if self.status == Status.FORWARD:
# logger.debug("FORWARD status")
# set cruise speed
self.setSpeed(0.2)
# avoing obstacles
ds = self.distanceSensors
# get distance sensors values
frontLeftSensor = ds.frontLeft.getValue()
frontRightSensor = ds.frontRight.getValue()
sideLeftSensor = ds.sideLeft.getValue()
sideRightSensor = ds.sideRight.getValue()
# set values of thresholds
tolerance = 10
sideThreshold = 950
# check if front left obstacle, turn right
if frontLeftSensor > frontRightSensor + tolerance:
self.setAngle(RIGHT * frontLeftSensor / 500.0 * MAX_ANGLE)
# logger.debug("Steering angle: " + str(RIGHT * frontLeftSensor / 1000.0 * MAX_ANGLE))
logger.debug("Steering angle: " + str(self.angle))
# check if front right obstacle, turn left
elif frontRightSensor > frontLeftSensor + tolerance:
self.setAngle(LEFT * frontRightSensor / 500.0 * MAX_ANGLE)
# logger.debug("Steering angle: " + str(LEFT * frontRightSensor / 1000.0 * MAX_ANGLE))
logger.debug("Steering angle: " + str(self.angle))
# check if side left obstacle, turn slight right
elif sideLeftSensor > sideThreshold:
self.setAngle(RIGHT * sideLeftSensor / 4000.0 * MAX_ANGLE)
# check if side right obstacle, turn slight left
elif sideRightSensor > sideThreshold:
self.setAngle(LEFT * sideRightSensor / 4000.0 * MAX_ANGLE)
# if no obstacle go straight
else:
self.setAngle(self.angle / 1.5)
# SEARCHING_PARK STATUS
if self.status == Status.SEARCHING_PARK:
# set slow speed
self.setSpeed(0.2)
# set straight wheels
self.setAngle(0.0)
# get distance and position sensors
ds = self.distanceSensors
ps = self.positionSensors
#log info for debug
logger.debug("Left Distance Sensor: " +
str(ds.sideLeft.getValue()))
logger.debug("Left Position Sensor: " +
str(ps.frontLeft.getValue()) + " rad")
logger.debug("Left Wheel Length: " +
str(self.leftWheelDistance) + " m")
logger.debug("Starting position: " +
str(leftStartingPosition) + " m")
logger.debug("Parking Lot Length: " +
str(self.leftWheelDistance -
leftStartingPosition) + " m")
sideThreshold = 650
leftSensorValue = ds.sideLeft.getValue()
rightSensorValue = ds.sideRight.getValue()
# checking parking lot on the LEFT side
if leftSensorValue < sideThreshold and not leftIsEmpty:
leftIsEmpty = True
leftStartingPosition = self.leftWheelDistance # 100
elif leftSensorValue > sideThreshold and leftIsEmpty:
leftIsEmpty = False
elif leftIsEmpty and self.leftWheelDistance - leftStartingPosition > LENGTH + LENGTH / 3:
leftStartingPosition = self.leftWheelDistance # 200 - 100
sideOfParkingLot = LEFT
self.setStatus(Status.FORWARD2)
# checking parking lot on the RIGHT side
if rightSensorValue < sideThreshold and not rightIsEmpty:
rightIsEmpty = True
rightStartingPosition = self.rightWheelDistance
elif rightSensorValue > sideThreshold and rightIsEmpty:
rightIsEmpty = False
elif rightIsEmpty and self.rightWheelDistance - rightStartingPosition > LENGTH + LENGTH / 3:
rightStartingPosition = self.rightWheelDistance
sideOfParkingLot = RIGHT
self.setStatus(Status.FORWARD2)
# this ensure that the parking manoeuvre starts after going forward and not as soon as the parking lot is detected
if self.status == Status.FORWARD2:
distance = 0.19
if sideOfParkingLot == LEFT:
if self.leftWheelDistance - leftStartingPosition > distance:
self.status = Status.PARKING
elif sideOfParkingLot == RIGHT:
if self.rightWheelDistance - rightStartingPosition > distance:
self.status = Status.PARKING
else:
logger.warning(
"Parking lot not found! I don't know if right or left."
)
self.setStatus(Status.SEARCHING_PARK)
# starting the parking manoeuvre
if self.status == Status.PARKING:
if sideOfParkingLot != LEFT and sideOfParkingLot != RIGHT:
logger.error("side of parking lot unknown.")
exit(1)
# stop the vehicle, turn the wheels and go back
self.setSpeed(0.0)
self.setAngle(sideOfParkingLot * MAX_ANGLE)
self.setSpeed(-0.1)
# when should it turn the other way
backThreshold = 400
ds = self.distanceSensors
rear = ds.back.getValue()
logger.debug("Back sensor: " + str(rear))
if rear > backThreshold:
self.status = Status.PARKING2
if self.status == Status.PARKING2:
self.setAngle(-1 * sideOfParkingLot * MAX_ANGLE)
threshold = 945
rear = self.distanceSensors.back.getValue()
if rear > threshold:
self.setStatus(Status.CENTER)
if self.status == Status.CENTER:
self.setAngle(0.0)
self.setSpeed(0.2)
rear = self.distanceSensors.back.getValue()
front = self.distanceSensors.frontCenter.getValue()
if rear - front < 20:
self.setStatus(Status.STOP)
if self.status == Status.STOP:
self.setSpeed(0.0)
self.setAngle(0.0)
# if obstacle is cleared go forward
if not self.avoidObstacle(
) and self.prevStatus == Status.PARKING2:
self.setStatus(Status.FORWARD)
if self.status == Status.MANUAL:
# get current state
speed = self.speed
angle = self.angle
# logger.debug("Current Key: " + str(currentKey))
# keyboard controlls
# accelerate
if currentKey == self.keyboard.UP:
if speed < 0:
speed += 0.02
else:
speed += 0.008
# break
elif currentKey == self.keyboard.DOWN:
if speed > 0:
speed -= 0.02
else:
speed -= 0.008
# turn left
elif currentKey == self.keyboard.LEFT:
angle -= 0.01
# turn right
elif currentKey == self.keyboard.RIGHT:
angle += 0.01
# handbreak
elif currentKey == ord(' '):
speed /= 4
# update state
self.setSpeed(speed)
self.setAngle(angle)
def parse(xml_text, configuration, is_deprovision=False):
"""
Parse xml tree, retrieving user and ssh key information.
Return self.
"""
ofv_env = OvfEnv()
logger.log_if_verbose(re.sub("<UserPassword>.*?<", "<UserPassword>*<", xml_text))
dom = xml.dom.minidom.parseString(xml_text)
if len(dom.getElementsByTagNameNS(ofv_env.OvfNs, "Environment")) != 1:
logger.error("Unable to parse OVF XML.")
section = None
newer = False
for p in dom.getElementsByTagNameNS(ofv_env.WaNs, "ProvisioningSection"):
for n in p.childNodes:
if n.localName == "Version":
verparts = get_node_text_data(n).split('.')
major = int(verparts[0])
minor = int(verparts[1])
if major > ofv_env.MajorVersion:
newer = True
if major != ofv_env.MajorVersion:
break
if minor > ofv_env.MinorVersion:
newer = True
section = p
if newer:
logger.warning(
"Newer provisioning configuration detected. Please consider updating waagent.")
if section is None:
logger.error(
"Could not find ProvisioningSection with major version=" + str(ofv_env.MajorVersion))
return None
ofv_env.ComputerName = get_node_text_data(section.getElementsByTagNameNS(ofv_env.WaNs, "HostName")[0])
ofv_env.UserName = get_node_text_data(section.getElementsByTagNameNS(ofv_env.WaNs, "UserName")[0])
if is_deprovision:
return ofv_env
try:
ofv_env.UserPassword = get_node_text_data(section.getElementsByTagNameNS(ofv_env.WaNs, "UserPassword")[0])
except (KeyError, ValueError, AttributeError, IndexError):
pass
try:
cd_section = section.getElementsByTagNameNS(ofv_env.WaNs, "CustomData")
if len(cd_section) > 0:
ofv_env.CustomData = get_node_text_data(cd_section[0])
if len(ofv_env.CustomData) > 0:
ext_utils.set_file_contents(constants.LibDir + '/CustomData', bytearray(
translate_custom_data(ofv_env.CustomData, configuration)))
logger.log('Wrote ' + constants.LibDir + '/CustomData')
else:
logger.error('<CustomData> contains no data!')
except Exception as e:
logger.error(str(e) + ' occured creating ' + constants.LibDir + '/CustomData')
disable_ssh_passwd = section.getElementsByTagNameNS(ofv_env.WaNs, "DisableSshPasswordAuthentication")
if len(disable_ssh_passwd) != 0:
ofv_env.DisableSshPasswordAuthentication = (get_node_text_data(disable_ssh_passwd[0]).lower() == "true")
for pkey in section.getElementsByTagNameNS(ofv_env.WaNs, "PublicKey"):
logger.log_if_verbose(repr(pkey))
fp = None
path = None
for c in pkey.childNodes:
if c.localName == "Fingerprint":
fp = get_node_text_data(c).upper()
logger.log_if_verbose(fp)
if c.localName == "Path":
path = get_node_text_data(c)
logger.log_if_verbose(path)
ofv_env.SshPublicKeys += [[fp, path]]
for keyp in section.getElementsByTagNameNS(ofv_env.WaNs, "KeyPair"):
fp = None
path = None
logger.log_if_verbose(repr(keyp))
for c in keyp.childNodes:
if c.localName == "Fingerprint":
fp = get_node_text_data(c).upper()
logger.log_if_verbose(fp)
if c.localName == "Path":
path = get_node_text_data(c)
logger.log_if_verbose(path)
ofv_env.SshKeyPairs += [[fp, path]]
return ofv_env
def computeParkingAngleAndSpeed(self):
logger.debug("PARKING STATE: " + str(self.status))
# get distance sensors
ds = self.distanceSensors
# set reference
distanceTraveled = self.positioning.getActualDistance()
side = 0
front = 0
rear = 0
bside = 0
fside = 0
if self.status > SEARCH and self.sideOfParkingLot != UNKNOWN:
# get rear and side sensors values
rear = self.distanceSensors.backCenter.getValue()
front = self.distanceSensors.frontCenter.getValue()
if self.sideOfParkingLot == RIGHT:
side = self.distanceSensors.sideRight.getValue()
bside = self.distanceSensors.backRight.getValue()
fside = self.distanceSensors.frontRight.getValue()
elif self.sideOfParkingLot == LEFT:
side = self.distanceSensors.sideLeft.getValue()
bside = self.distanceSensors.backLeft.getValue()
fside = self.distanceSensors.frontLeft.getValue()
# SEARCH status 2
if self.status == SEARCH:
# set slow speed
self.speed = 0.2
# set straight wheels
self.angle = self.lineFollower.getNewSteeringAngle()
if self.lineFollower.isLineLost():
self.resetParkingPosition()
self.angle = self.lineFollower.getSteeringAngleLineSearching()
logger.debug("PARKING angle from Line Follower: " +
str(self.angle))
if abs(self.angle) > 0.25:
self.resetParkingPosition()
# side threshold, if side sensor is greather than threshold the parking lot if occupied
sideThreshold = 650
# get side left sensors values
leftSensorValue = ds.sideLeft.getValue()
rightSensorValue = ds.sideRight.getValue()
# checking parking lot on the LEFT side
if leftSensorValue < sideThreshold and not self.leftIsEmpty:
self.leftIsEmpty = True
self.leftStartingPosition = distanceTraveled
elif leftSensorValue > sideThreshold and self.leftIsEmpty:
self.leftIsEmpty = False
elif self.leftIsEmpty and distanceTraveled - self.leftStartingPosition > CAR_LENGTH + (
CAR_LENGTH * 2 / 3):
self.leftStartingPosition = distanceTraveled
self.sideOfParkingLot = LEFT
self.status = FORWARD
# checking parking lot on the RIGHT side
if rightSensorValue < sideThreshold and not self.rightIsEmpty:
self.rightIsEmpty = True
self.rightStartingPosition = distanceTraveled
elif rightSensorValue > sideThreshold and self.rightIsEmpty:
self.rightIsEmpty = False
elif self.rightIsEmpty and distanceTraveled - self.rightStartingPosition > CAR_LENGTH + (
CAR_LENGTH * 2 / 3):
self.rightStartingPosition = distanceTraveled
self.sideOfParkingLot = RIGHT
self.status = FORWARD
# FORWARD status 3
# this ensure that the parking manoeuvre starts after going forward and not as soon as the parking lot is detected
elif self.status == FORWARD:
# distance to travel before starting the maneuver
distance = 0.13
# check if distance traveled is greater than distance to travel
if self.sideOfParkingLot == LEFT:
if distanceTraveled - self.leftStartingPosition >= distance:
self.status = PARKING
elif self.sideOfParkingLot == RIGHT:
if distanceTraveled - self.rightStartingPosition >= distance:
self.status = PARKING
else:
# if parking is found but is not set the side
logger.warning(
"Parking lot found! But I don't know if right or left. Looking for anther one."
)
self.status = SEARCH
# starting the parking manoeuvre 4
elif self.status == PARKING:
# check is the side of the parking lot is known
if self.sideOfParkingLot != LEFT and self.sideOfParkingLot != RIGHT:
logger.error("Side of parking lot is unknown.")
# stop the vehicle, turn the wheels and go back slowly
self.angle = self.sideOfParkingLot
self.speed = -0.1
# back thresholds that tells when contursteer
backCenterThreshold = 610
backSideThreshold = 410
if self.sideOfParkingLot == LEFT:
backCenterThreshold = 625
backSideThreshold = 425
# debug log
logger.debug("Rear: " + str(rear) + " Back Park Side: " +
str(bside))
# if true countersteer
if bside > backSideThreshold or rear > backCenterThreshold:
self.status = PARKING2
# PARKING2 status 5
elif self.status == PARKING2:
# set countersteering angle
if self.angle != 0:
self.angle = -1 * self.sideOfParkingLot
self.speed = -0.05
# set side and back threshold
rearThreshold = 985
sideThreshold = 945
backSideThreshold = 915
# debug log
logger.debug("sideOfParkingLot: " + str(self.sideOfParkingLot) +
" side: " + str(side))
# check if the park is completed
if (bside <= fside and side >= 865):
self.status = CENTER
# check if need some orient adjust
if self.distanceSensors.backDistance(
sideThreshold): #and bside > fside:
self.status = ADJUST_POSITION
# ADJUST_POSITION status 6
elif self.status == ADJUST_POSITION:
# debug log
logger.debug("Need to adjust position")
# set angle
self.angle = self.sideOfParkingLot
self.speed = 0.05
# check if the park is completed
if (bside < fside and side > 865):
self.status = CENTER
if front > 925 or fside > 925 or not self.distanceSensors.backDistance(
825):
self.angle = -1 * self.sideOfParkingLot
self.status = PARKING2
# CENTER status 7
elif self.status == CENTER:
if rear > 950 or (rear < 100 and front < 700):
self.speed = 0.05
elif front > 950 or (front < 100 and rear < 700):
self.speed = -0.05
# set angle streight
if abs(bside - fside) < 50 and abs(rear - front) < 80:
self.angle = 0.0
self.status = STOP
logger.debug(
"TRYING TO STAY CENTER: same distance sides and rear and front!"
)
elif abs(bside - fside) < 50 and rear < 100 and front > 700:
self.angle = 0.0
self.status = STOP
logger.debug(
"TRYING TO STAY CENTER: there's nothing behind, stay ahead!"
)
elif abs(bside - fside) < 50 and rear < 100 and front > 700:
self.angle = 0.0
self.status = STOP
logger.debug(
"TRYING TO STAY CENTER: there's nothing behind, stay ahead!"
)
elif abs(bside - fside) < 50 and front < 100 and rear > 700:
self.angle = 0.0
self.status = STOP
logger.debug(
"TRYING TO STAY CENTER: there's nothing ahead, stay behind!"
)
elif abs(bside - fside) < 20:
self.angle = 0.0
logger.debug("TRYING TO STAY CENTER: same distance sides!")
else:
if self.speed > 0:
nextAngle = 0.2
else:
nextAngle = -0.2
if fside < bside:
nextAngle *= self.sideOfParkingLot
else:
nextAngle *= -self.sideOfParkingLot
self.angle = nextAngle
logger.debug("TRYING TO STAY CENTER: NEED TO GO " +
("right" if self.angle > 0 else "left"))
# STOP status 8
elif self.status == STOP:
self.speed = 0.0
self.angle = 0.0
# invalid status
else:
logger.debug("Invalid parcking status")
# Camera:
HAX = 53 # The head angle of the camera (x)
HAY = 40 # The head angle of the camera (y)
# Camera Settings:
brightness = -0.1
saturation = 15
exposure = -1
##################################
# Camera and FPS counter setting #
##################################
if __name__ == "__main__":
if lockFile.is_locked():
logger.error("Lock File Locked!")
raise ValueError
else:
lockFile.lock()
try:
FPSCounter = FPS()
FPSCounter.start()
cam = cv2.VideoCapture(0)
cam.set(3,640)
cam.set(4,480)
#cam.set(cv2.cv.CV_CAP_PROP_BRIGHTNESS, brightness)
#cam.set(cv2.cv.CV_CAP_PROP_SATURATION, saturation)
#cam.set(cv2.cv.CV_CAP_PROP_EXPOSURE, exposure) # not working on the old camera
from Odometry import Odometry
from Utils import DistanceSensors, Position, PositionSensors
from Utils import logger, Status, DEBUG
from LineFollower import LineFollower, UNKNOWN
from Navigation import PathPlanner, NORD, EAST, SOUTH, WEST, LEFT, RIGHT, FORWARD
try:
from vehicle import Driver
except ImportError:
logger.error("Cannot find vehicle. Make sure you run this inside Webots.")
exit(1)
# constants
MAX_SPEED = 1.8
MAX_ANGLE = 0.52 # rads ~ 30°
# LEFT = -1
# RIGHT = 1
UNKNOWN = -2
# vehicle dimensions in meters
WHEEL_RADIUS = 0.020
LENGTH = 0.180
WIDTH = 0.098
HEIGHT = 0.061
class Altino:
def __init__(self):
# initialize devices
self.initializeDevices()
