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) ] rot_angle = (-1) * (HAL.getPose3d().yaw + math.pi / 2) target_vector = [ target_vector[0] * math.cos(rot_angle) - target_vector[1] * math.sin(rot_angle), target_vector[0] * math.sin(rot_angle) + target_vector[1] * math.cos(rot_angle) ]
from GUI import GUI from HAL import HAL 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