def check_target(self, posx, posy, targetx, targety): if (abs(targetx - posx) <= 6 and abs(targety - posy) <= 6): currentTarget.setReached(True) currentTarget = GUI.map.getNextTarget() if (currentTarget != None): return 1 else: return 0 while True: currentTarget = GUI.map.getNextTarget() GUI.map.targetx = currentTarget.getPose().x GUI.map.targety = currentTarget.getPose().y posx = HAL.getPose3d().x posy = HAL.getPose3d().y targetid = currentTarget.getId() # TODO k_obstacle = 0.6 k_car = 0.2 k_target = -0.2 unit_angle = (HAL.getLaserData().maxAngle - HAL.getLaserData().minAngle) / len(HAL.getLaserData().values) unit_range = 1 / (HAL.getLaserData().maxRange - HAL.getLaserData().minRange) target_vector = [ k_target * (GUI.map.targetx - posx), k_target * (GUI.map.targety - posy)
import math import numpy as np # Enter sequential code! threshold_angle = 0.01 threshold_distance = 0.25 threshold = 0.01 kp = 1.0 while True: # Enter iterative code! # creating Objects currentTarget = GUI.map.getNextTarget() laser_data = HAL.getLaserData () # definging Variables pos = [HAL.getPose3d().x, HAL.getPose3d().y] dist = [-1*(currentTarget.getPose().x - pos[0]), -1*(currentTarget.getPose().y - pos[1])] mod_dist = math.sqrt(dist[0]**2 + dist[1]**2) GUI.map.targetx = currentTarget.getPose().x GUI.map.targety = currentTarget.getPose().y if (dist[1] <= 0.001): currentTarget.setReached(True) GUI.map.carx = 1.5*(dist[0] / mod_dist) GUI.map.cary = 1.5*(dist[1] / mod_dist) GUI.map.obsx = 0.0 GUI.map.obsy = 0.0 # Creating the Histogram hist = [] for a in range(len(laser_data.values)):