Unlock 30% off on Manual Testing Annual Plans this Holiday Season.

Copied to Clipboard!

How to use path_config method in tox

Best Python code snippet using tox_python

pathFinder.py

Source:pathFinder.py

1#!/usr/bin/env python2import rospy3import math4import autobot5import operator6from autobot.msg import drive_param7from sensor_msgs.msg import LaserScan8from autobot.msg import pid_input9from autobot.msg import wall_dist10from autobot.msg import pathFinderState11from autobot.srv import *12from autobot.msg import gps_direc13from autobot.msg import enable_lidar14"""15TODO:16- [x] Decide if you want to hug right/left/or closest wall17    - Right wall hugged for now to simulate right side of road18    - Update: wall decision to be made by image processing node19- [x] Send error left to hug left wall20- [ ] Use a command line argument to enable/disable debug msgs21"""22class PathConfig(object):23    __slots__ = ('wallToWatch', 'desiredTrajectory', 'velocity', 'pubRate',24                 'minFrontDist', 'enabled', 'rev', 'count', 'switch_to_gps', 'neutral', 'count_nf', 'neutral_forw', 'enlid')25    """26    wallToWatch: Set which wall to hug27    options: autobot.msg.wall_dist.WALL_LEFT28             autobot.msg.wall_dist.WALL_RIGHT29             autobot.msg.wall_dist.WALL_FRONT  #< probably won't be used30    """31    def __init__(self):32        self.wallToWatch = autobot.msg.wall_dist.WALL_RIGHT33        self.desiredTrajectory = 0.5  # desired distance from the wall34        self.minFrontDist = 1.4       # minimum required distance in front of car35        self.velocity = 10070          # velocity of drive36        self.pubRate = 0              # publish rate of node37        self.enabled = True          # enable/disable state of wall hugging38        self.rev = False39        self.count = 040        self.switch_to_gps = False41        self.neutral = False42        self.count_nf = 043        self.neutral_forw = False44        self.enlid = 145        46PATH_CONFIG = PathConfig()47errorPub = rospy.Publisher('error', pid_input, queue_size=10)48motorPub = rospy.Publisher('drive_parameters', drive_param, queue_size=10)49statePub = rospy.Publisher('pathFinderStatus', pathFinderState, queue_size=10)50eLidarPub = rospy.Publisher('e_lidar', enable_lidar, queue_size=10)51def HandleTogglePathFinderService(req):52    """ Handler for enabling/disabling path finder53    Responds with ack msg (bool)54    """55    global PATH_CONFIG56    PATH_CONFIG.enabled = True   #req.state57    return TogglePathFinderResponse(True)58def HandleAdjustWallDist(req):59    """ Handler for adjusting wall hugging parameters60    Responds with wall_dist msg and a bool to verify that the61    service command has been accepted62    """63    global PATH_CONFIG64    # print " wall {}".format(req.cmd.wall)65    # print " dist {}\n".format(req.cmd.dist)66    resp = wall_dist()67    isValid = req.cmd.dist >= 068    if isValid is True and req.cmd.wall != autobot.msg.wall_dist.WALL_FRONT:69        """ only accept WALL_LEFT or WALL_RIGHT70        Service client can send an invalid wall or distance71        query current settings72        """73        if req.cmd.wall is not wall_dist.WALL_UNDEF:74            PATH_CONFIG.wallToWatch = req.cmd.wall75        PATH_CONFIG.desiredTrajectory = req.cmd.dist76    else:77        isValid = False78    resp.wall = PATH_CONFIG.wallToWatch79    resp.dist = PATH_CONFIG.desiredTrajectory80    return AdjustWallDistResponse(resp, isValid)81def publishCurrentState(event):82    global PATH_CONFIG83    msg = pathFinderState()84    msg.velocity = PATH_CONFIG.velocity85    msg.hug.wall = PATH_CONFIG.wallToWatch86    msg.hug.dist = PATH_CONFIG.desiredTrajectory87    msg.enabled = True   #PATH_CONFIG.enabled88    statePub.publish(msg)89def getRange(data, theta):90    """ Find the index of the array that corresponds to angle theta.91    Return the lidar scan value at that index92    Do some error checking for NaN and absurd values93    data: the LidarScan data94    theta: the angle to return the distance for95    """96    car_theta = math.radians(theta) - math.pi / 297    if car_theta > 3 * math.pi / 4:98        car_theta = 3 * math.pi / 499    elif car_theta < -3 * math.pi / 4:100        car_theta = -3 * math.pi / 4101    float_index = (car_theta + 3 * math.pi / 4) / data.angle_increment102    index = int(float_index)103    return data.ranges[index]104def max_list(list):105    maxelem = []106    highest = 0107    for x in list:108        if highest < x[2]:109            highest = x[2]110            maxelem = x111    return maxelem112# Function will return the biggest free available segment113def far_see(data):114    i_rl = 0115    direc = 90116    # checking for broad angle, from starting from center, if front is completely blocked then the segment starts turning sideways117    while direc < 120 and direc > 60:118        near_vision = [getRange(data, direc-3+i) for i in range(6)]119        if any(value < 5 for value in near_vision):120            i_rl += 1121            if i_rl % 2:122                direc = direc+i_rl123            else:124                direc = direc-i_rl125        else:126            return direc127    return -1 128def short_see(direc, data):129    i_rl = 0130    while direc < 120 and direc > 60:131        near_vision = [getRange(data, direc-15+i) for i in range(30)]132        if any(value < 0.9 for value in near_vision):133            i_rl += 1134            if i_rl % 2:135                direc = direc+i_rl136            else:137                direc = direc-i_rl138        else:139            return direc140    return -1     141def check_2m_and_run(data):142    global PATH_CONFIG143    farFrontDist = [getRange(data, 80+i) for i in range(20)]			# creates the list of all the FAR-FRONT values in range 80-to-100144    frontDistance = [getRange(data, 70+i) for i in range(40)]			# creates the list of all the FRONT 	 values in range 70-to-110145    #theta = 50  # PICK THIS ANGLE TO BE BETWEEN 0 AND 70 DEGREES146    #thetaDistRight = getRange(data, theta)  # a147    rightDist = [getRange(data, 50+i) for i in range(30)]  # b148    #thetaDistLeft = getRange(data, 180-theta)  # aL149    leftDist = [getRange(data, 100+i) for i in range(30)]  # bL150    left_b = False																		# INDICATES IF LEFT  is blocked151    right_b = False																		# INDICATES IF RIGHT is blocked152    minLeftDist = 1				# threshold for LEFt distance153    minRightDist = 1				# threshold for RIGHT  distance154    minFrontDist = 0.5			# threshold for FRONT distance155    minFarFrontDist = 2			# threshold for FarFront distance156    #driveParam = drive_param()157 158    #looking for closer objest, go to blocked state159    velocity = PATH_CONFIG.velocity160    angle = 11161    if any(front_fd < minFarFrontDist for front_fd in farFrontDist):		# CHECKs IF FAR-front is blocked162        print "Far Front Blocked!"163        164#        driveParam.velocity = PATH_CONFIG.velocity165#        smallest = 100;166#        for d in rightDist:167#            if smallest > d:168#                smallest = d169        fd = min(farFrontDist)		#returns the MINIMUM distance from the list containing all the FAR-FRONT DISTANCE values170        if any(front_d < minFrontDist for front_d in frontDistance):		# CHECKs IF Front is blocked171            print "Front Blocked!"172            if not PATH_CONFIG.rev and not PATH_CONFIG.neutral_forw:173                velocity = 6600174                PATH_CONFIG.neutral_forw = False175                PATH_CONFIG.neutral = False176                PATH_CONFIG.count += 1177                if PATH_CONFIG.count > 10:178                    PATH_CONFIG.rev = True179                    PATH_CONFIG.count = 0180            else:181                velocity = 9830182                PATH_CONFIG.neutral = True183                184            if PATH_CONFIG.neutral_forw:185                velocity = 10070186                PATH_CONFIG.count_nf += 1         #count neutral forward to check for any reverse right after neutral and followed forward187                PATH_CONFIG.neutral = False188                if PATH_CONFIG.count_nf > 25:189                    PATH_CONFIG.neutral_forw = False190                    velocity = 6600191                    PATH_CONFIG.rev = False192                    PATH_CONFIG.count_nf = 0193            angle = 0194            195        elif all(left_d > min(rightDist) for left_d in leftDist):196            angle = -90 + (fd-1)*60197            print "Turning left"198            PATH_CONFIG.rev = False199            PATH_CONFIG.count = 0200            PATH_CONFIG.count_nf = 0201        elif all(right_d > min(leftDist) for right_d in rightDist):202            angle = 90 - (fd-1)*60203            print "Turning right"204            PATH_CONFIG.rev = False205            PATH_CONFIG.count = 0206            PATH_CONFIG.count_nf = 0207        else:208            velocity = 10070 209            angle = 0210            PATH_CONFIG.rev = False211            #PATH_CONFIG.neutral = True212            #PATH_CONFIG.neutral_forw = False213            PATH_CONFIG.count = 0214            PATH_CONFIG.count_nf = 0215    else:216        PATH_CONFIG.rev = False217        PATH_CONFIG.count = 0218        PATH_CONFIG.count_nf = 0219        220    221    if velocity is PATH_CONFIG.velocity:222        if PATH_CONFIG.neutral is True:223            PATH_CONFIG.neutral_forw = True224            PATH_CONFIG.neutral = False225            PATH_CONFIG.rev = False226            PATH_CONFIG.count_nf = 0227            PATH_CONFIG.count = 0228    else:229        PATH_CONFIG.count_nf = 0230        PATH_CONFIG.neutral_forw = False231            232            233    vel_ang = []234    vel_ang.append(velocity)235    vel_ang.append(angle)236    return vel_ang237def callback(data):238    global PATH_CONFIG239    # Do not attempt to hug wall if disabled240    241    print(PATH_CONFIG.enlid)242    243    if PATH_CONFIG.enabled is False or PATH_CONFIG.enlid == 0:244        return245    #frontDistance = getRange(data, 90)246    #checking for far front distance247    farFrontDist = [getRange(data, 85+i) for i in range(10)]	248    # if frontDist is less than 5m, then go in249    driveParam = drive_param()250    direc = 90251    fd = min(farFrontDist)252    #initially setting default velocity and angle, change in appropriate conditions253    driveParam.velocity = PATH_CONFIG.velocity254    driveParam.angle = 0255    #if fd < 5:256    minSegDist = 5257    obstSeg = [getRange(data, 75) for i in range(30)] 258    fdseg = min(obstSeg)259    260    if fd < 5:261    # if fdseg < 5:262        #PATH_CONFIG.switch_to_gps = False263        if fd < 2:264            vel_ang = check_2m_and_run(data)265            driveParam.velocity = vel_ang[0]266            driveParam.angle = vel_ang[1] 267        else:268             #check for available largest segment and get the direction269            direc = far_see(data)270        271            if direc is -1:  #checking for return value from far_see272                #driveParam.velocity = 9830273                vel_ang = check_2m_and_run(data)274                driveParam.velocity = vel_ang[0]275                driveParam.angle = vel_ang[1]          276            else:277                #turn the car to the free space direction, then automatically farFrontDist goes higher than 5, will go out of this main if loop278    279                # LiDAR direc is from 0 to 180 in front semicircle, for angle pwm its from 90 to 0 to -90280                if direc < 90:   #if far_see output direction is towards right side281                    # turning car slowly from, 30' to 90' by checking the min front dist from 5m to 2m. so Instantly it turns to 30 and then turns to slowly to 90 where 90 is full right282                    # direc is in LiDAR angle bandwidth, converting it to pwm angle bandwidth as well283                    driveParam.angle = (90 - direc) - (fd-2)*11.25284                    PATH_CONFIG.rev = False285                    PATH_CONFIG.count = 0286                else:287                    # turning car slowly from, -30' to -90' by checking the min front dist from 5m to 2m.288                    driveParam.angle = (90 - direc) + (fd-2)*11.25289                    PATH_CONFIG.rev = False290                    PATH_CONFIG.count = 0291 292        motorPub.publish(driveParam)293    else:294        frontDistance = [getRange(data, 70+i) for i in range(40)]			# creates the list of all the FRONT 	 values in range 70-to-110295        if any(front_d < 2 for front_d in frontDistance):296            vel_ang = check_2m_and_run(data)297            PATH_CONFIG.switch_to_gps = False298            driveParam.velocity = vel_ang[0]299            driveParam.angle = vel_ang[1] 300            motorPub.publish(driveParam)301        else:302            PATH_CONFIG.switch_to_gps = True303    304    if driveParam.velocity is PATH_CONFIG.velocity:305        if PATH_CONFIG.neutral is True:306            PATH_CONFIG.neutral_forw = True307            PATH_CONFIG.neutral = False308            PATH_CONFIG.rev = False309            PATH_CONFIG.count_nf = 0310            PATH_CONFIG.count = 0311    else:312        PATH_CONFIG.count_nf = 0313        PATH_CONFIG.neutral_forw = False314         315    print("velocity = ", driveParam.velocity)316    print("angle = ", driveParam.angle)317    print("far direc = ", direc)318   319    return320def gps_callback(gps_direc):321    global PATH_CONFIG322    if PATH_CONFIG.switch_to_gps:323        print("gps_controll_on")   324        driveParam = drive_param()325        driveParam.velocity = gps_direc.velocity326        driveParam.angle = gps_direc.angle 327        motorPub.publish(driveParam)328        329    return330    331def lidar_enable_callback(enb_lid):332    global PATH_CONFIG333    334    '''    335    if enb_lid == 0:336        print(enb_lid)337     338    el_decrement_msg = enable_lidar()339    340    if 50 > enb_lid.enable_lidar >= 1:341        PATH_CONFIG.enlid = 0342        el_decrement_msg.enable_lidar = enb_lid.enable_lidar+1343        eLidarPub.publish(el_decrement_msg)344    345    elif enb_lid.enable_lidar >= 50:346        el_decrement_msg.enable_lidar = 0347        eLidarPub.publish(el_decrement_msg)348    349    350    '''351    if enb_lid.enable_lidar == 0:352        PATH_CONFIG.enlid = 1353    else:354	PATH_CONFIG.enlid = 0355	''' 356	    print("en: ", PATH_CONFIG.enlid)357	    rospy.sleep(0.2)358	    359	    PATH_CONFIG.enlid = 1360        '''361    #print("en: ", PATH_CONFIG.enlid)362if __name__ == '__main__':363    print("Path finding node started", )364    rospy.Service('adjustWallDist', AdjustWallDist, HandleAdjustWallDist)365    rospy.Service('togglePathFinder', TogglePathFinder, HandleTogglePathFinderService)366    rospy.init_node('pathFinder', anonymous=True)367    rospy.Subscriber('e_lidar', enable_lidar, lidar_enable_callback)368    rospy.Subscriber("scan", LaserScan, callback)369    #rospy.Subscriber("GPS", gps_direc, gps_callback)370    rospy.Timer(rospy.Duration(0.5), callback=publishCurrentState)...

config.py

Source:config.py

1# -*- coding: utf-8 -*-2import logging3from os import path4import yaml5logger = logging.getLogger(__name__)6def load_config(path_config):7    if not path.isfile(path_config):8        logger.error("No exists config file %s" % (path_config,))9    f = open(path_config, 'r')10    config = yaml.load(f)11    f.close()12    return config13def load_config_device(device_name, path_config='/etc/buoy/buoy.yaml'):14    config = load_config(path_config=path_config)15    return config['device'][device_name]16def load_config_device_serial(device_name, path_config='/etc/buoy/device.yaml'):17    config = load_config_device(device_name, path_config=path_config)18    return config['device'][device_name]['serial']19def load_config_logger(path_config='/etc/buoy/logging.yaml'):20    config = load_config(path_config=path_config)...

Automation Testing Tutorials

Learn to execute automation testing from scratch with LambdaTest Learning Hub. Right from setting up the prerequisites to run your first automation test, to following best practices and diving deeper into advanced test scenarios. LambdaTest Learning Hubs compile a list of step-by-step guides to help you be proficient with different test automation frameworks i.e. Selenium, Cypress, TestNG etc.