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)):
