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
