def get_closest_lidar(self, time, guess):
if utils.isempty(guess):
return [0.0, 0.0, 0.0]
idx = utils.binary_search(time, self.lidar_times)
#print "Lidar---> ", self.lidar_data[idx]
points = self.lidar_data[idx]
#print "lidar: ", points
#print idx
best = 10.0
res = [0.0, 0.0, 0.0]
#print points
for elem in points:
d = utils.distance_ang(elem, guess)
#print d
if d < best:
best = d
res = elem
return list(res)
def query_data(table='devops_compile',
keys='id, compile_build_time, compile_build_finish_time',
condition=None,
order=None):
global DEV_OPS_DB
if not DEV_OPS_DB:
DEV_OPS_DB = mysql.connector.connect(host=utils.DB_HOST,
port=utils.DB_PORT,
user=utils.DB_USER,
passwd=utils.DB_PASSWORD,
database=utils.DB_DATABASE)
cursor = DEV_OPS_DB.cursor()
sql = 'select %s from %s ;' % (keys, table)
if not utils.isempty(condition):
sql = sql.replace(';', ' where %s;' % condition)
if not utils.isempty(order):
sql = sql.replace(';', ' order by %s;' % order)
cursor.execute(sql)
return cursor.fetchall()
def get_closest_radar(self, radar_points, coord):
#print coord, utils.isempty(coord)
if utils.isempty(coord):
return [0.0, 0.0, 0.0]
res = [0.0, 0.0, 0.0]
best = 10.0
for elem in radar_points:
#print elem, coord, utils.distance_ang(elem, coord) , best, res
if utils.distance_ang(elem, coord) < best:
best = utils.distance_ang(elem, coord)
res = elem
#print res
return res
def get_best_radar_position_full(self, points, camera_angle, camera_distance, guess = [0.0, 0.0, 0.0], debug=False):
best = -1.0
res = [0.0, 0.0, 0.0]
if len(points) == 0:
return [camera_distance*math.cos(camera_angle), -camera_distance*math.sin(camera_angle), 0.0]
#if len(self.object_positions) == 439:
# print points
# print camera_angle, camera_distance
# print guess
# debug = True
#if debug:
# print "--------------------------"
# print points
# print camera_angle, camera_distance
if abs(camera_angle) > 1.0e-6:
# dif1 = 1000000
# dif2 = 1000000
#
# for j in range(len(points)):
# try:
# ang2 = math.atan2(points[j][1], points[j][0])
# dist2 = abs(math.sqrt(points[j][0]*points[j][0] + points[j][1]*points[j][1]) - camera_distance)
#
# if debug:
# print ang2, dist2, dif1, dif2
#
# if utils.isempty(guess) or utils.distance_2d(guess, points[j]) < 5.0:
# if dif1 - abs(ang2 - camera_angle) > 0.1:
# res =points[j]
# dif1 = abs(ang2 - camera_angle)
# dif2 = dist2
# elif abs(dif1 - abs(ang2 - camera_angle)) < 0.1 and dif2 > dist2:
# res =points[j]
# dif2 = dist2
# except TypeError:
# print "TypeError"
dif1 = 1000000
dif2 = 1000000
for j in range(len(points)):
try:
ang2 = math.atan2(points[j][1], points[j][0])
dist2 = abs(math.sqrt(points[j][0]*points[j][0] + points[j][1]*points[j][1]) - camera_distance)
#if debug:
# print points[j]
# print ang2, dist2, dif1, dif2
#print ang2, dist2
if abs(ang2 - camera_angle) < 0.15 and abs(dist2) < 10:
#if debug:
# print "pasa ", guess
if not utils.isempty(guess):
if utils.distance_2d(guess, points[j]) < dif2:
res =points[j]
dif2 = utils.distance_2d(guess, points[j])
else:
if dif2 > dist2:
res =points[j]
dif2 = dist2
except TypeError:
print "TypeError"
#print camera_angle, math.atan2(res[1], res[0])
if res[0] == 0.0 and res[1]==0.0:
dif1 = 1000000
dif2 = 1000000
for j in range(len(points)):
try:
ang2 = math.atan2(points[j][1], points[j][0])
dist2 = abs(math.sqrt(points[j][0]*points[j][0] + points[j][1]*points[j][1]) - camera_distance)
#if debug:
# print utils.distance_2d(guess, points[j])
# print "-->"
if not utils.isempty(guess):
if utils.distance_2d(guess, points[j]) < 10.0 and utils.distance_2d(guess, points[j]) <dif2:
res =points[j]
dif2 = utils.distance_2d(guess, points[j])
except TypeError:
print "TypeError"
return res
def solve_time(self, time, idx=-1):
real_time = time - int(0.25*1000000000)
real_lidar_time = time #+ int(0.2*1000000000)
if self.debug:
self.out.write(str(real_time) + "\n")
self.out.flush()
#print real_time
# Estimate position
estimated_position = [0.0, 0.0, 0.0]
if not utils.isempty(self.last_position):
estimated_position = self.last_position
# KARMAN-FILTER
if False and not utils.isempty(self.last_velocity):
estimated_position = [estimated_position[i] + self.last_velocity[i]* \
(real_time/MS_IN_SEC - self.last_realtime/MS_IN_SEC) \
for i in range(len(estimated_position))]
#if idx == 350:
# print "------------->", idx
#print estimated_position
# if not utils.isempty(self.last_acceleration):
# estimated_position = [estimated_position[i] + 0.5*self.last_acceleration[i]* \
# math.pow(real_time/MS_IN_SEC - self.last_realtime/MS_IN_SEC, 2) \
# for i in range(len(estimated_position))]
self.mutexCamera.acquire()
try:
camera_angle = self.calc_camera_angle(real_time)
camera_distance = self.calc_camera_distance(real_time)
camera_coordinates = self.calc_camera_coordinates(real_time)
finally:
self.mutexCamera.release()
if self.debug:
self.out.write(str(camera_angle) + " " + str(camera_distance) + "\n")
self.out.flush()
#print camera_angle, camera_distance
if abs(camera_distance) > eps:
camera_coordinates = [camera_distance*math.cos(camera_angle), camera_distance*math.sin(camera_angle), 0.0]
else:
camera_coordinates = [0.0, 0.0, 0.0]
self.mutexRadar.acquire()
try:
prev_radar = self.get_previous_radar(real_time)
nxt_radar = self.get_next_radar(real_time)
prev_radar_time = self.get_previous_radar_time(real_time)
nxt_radar_timer = self.get_next_radar_time(real_time)
finally:
self.mutexRadar.release()
#print real_time
#print nxt_radar
#print prev_radar
# if self.debug:
# self.out.write(str(nxt_radar_timer) + "\n")
# self.out.write(str(nxt_radar) + "\n")
# self.out.write(str(prev_radar_time) + "\n")
# self.out.write(str(prev_radar) + "\n")
# self.out.flush()
radar_coordinates_prev = self.get_closest_radar(prev_radar, estimated_position)
radar_coordinates_next = self.get_closest_radar(nxt_radar, estimated_position)
radar_cam_coordinates_prev = self.get_closest_radar(prev_radar, camera_coordinates)
radar_cam_coordinates_next = self.get_closest_radar(nxt_radar, camera_coordinates)
#print radar_coordinates_prev, radar_coordinates_next
#print radar_cam_coordinates_prev, radar_cam_coordinates_next
radar_coordinates = [0.0, 0.0, 0.0]
radar_cam_coordinates = [0.0, 0.0, 0.0]
if utils.isempty(radar_coordinates_prev):
radar_coordinates = radar_coordinates_next
elif utils.isempty(radar_coordinates_next):
radar_coordinates = radar_coordinates_prev
else:
radar_coordinates = [utils.interpolate_aprox(real_time, prev_radar_time, \
radar_coordinates_prev[i], \
nxt_radar_timer, \
radar_coordinates_next[i]) for i in range(3)]
if utils.isempty(radar_cam_coordinates_prev):
radar_cam_coordinates = radar_cam_coordinates_next
elif utils.isempty(radar_cam_coordinates_next):
radar_cam_coordinates = radar_cam_coordinates_prev
else:
radar_cam_coordinates = [utils.interpolate_aprox(real_time, prev_radar_time, \
radar_cam_coordinates_prev[i], \
nxt_radar_timer, \
radar_cam_coordinates_next[i]) for i in range(3)]
#print "------->", camera_angle, math.atan2(radar_cam_coordinates[1], radar_cam_coordinates[0])
d = utils.distance_2d(radar_cam_coordinates, [0.0, 0.0, 0.0])
radar_cam_coordinates = [d*math.cos(camera_angle), d*math.sin(camera_angle), 0.0]
#print radar_cam_coordinates
#radar_position_prev = self.get_best_radar_position(prev_radar_time, estimated_position)
#radar_position_next = self.get_best_radar_position(nxt_radar_time, estimated_position)
#if len(self.object_positions) == 486:
# print radar_position_prev, radar_position_next, real_time
# print prev_radar_time, nxt_radar_time
#radar_position = []
#if utils.distance_2d(radar_position_prev, radar_position_next) < 10.0:
# radar_position = [utils.interpolate_aprox(real_time, prev_radar_time, \
# radar_position_prev[i], \
# nxt_radar_time, \
# radar_position_next[i]) for i in range(3)]
#else:
# radar_position = self.get_best_radar_position_full([radar_position_prev, radar_position_next], camera_angle, camera_distance, estimated_position)
lidar_position_prev = [0.0, 0.0, 0.0]
lidar_position_next = [0.0, 0.0, 0.0]
self.mutexLidar.acquire()
try:
if len(self.lidar_times) > 0:
prev_lidar_time = self.get_previous_lidar_time(real_lidar_time)
next_lidar_time = self.get_next_lidar_time(real_lidar_time)
guess = estimated_position
if utils.isempty(guess):
guess = camera_coordinates
#if not utils.isempty(estimated_position) and len(self.lidar_times) > 0:
lidar_position_prev = self.get_closest_lidar(prev_lidar_time, guess)
lidar_position_next = self.get_closest_lidar(next_lidar_time, guess)
#elif not utils.isempty(radar_position) and len(self.lidar_times) > 0:
# lidar_position_prev = self.get_closest_lidar(prev_lidar_time, radar_position)
# lidar_position_next = self.get_closest_lidar(next_lidar_time, radar_position)
finally:
self.mutexLidar.release()
if len(self.lidar_times) > 0:
lidar_position = [utils.interpolate_aprox(real_lidar_time, prev_lidar_time, \
lidar_position_prev[i], \
next_lidar_time, \
lidar_position_next[i]) for i in range(3)]
else:
lidar_position = [0.0, 0.0, 0.0]
#if len(self.object_positions) == 201:
# print "------------->"
# print camera_angle, radar_position
# object_position = [0.0, 0.0, 0.0]
# if abs(camera_angle) <1.0e-6 or utils.isempty(radar_position):
# if not utils.isempty(estimated_position) and \
# utils.distance_2d(lidar_position, [0, 0, 0]) < 25.0 and \
# utils.distance_2d(lidar_position, estimated_position) < 5.0:
# object_position = lidar_position[:]
# object_position[1] += 0.4
# object_position[0] -= 0.4
# else:
# radar_position = [0.0, 0.0, 0.0]
# object_position = radar_position[:]
# else:
# if not utils.isempty(lidar_position) or not utils.isempty(radar_position):
# if not utils.isempty(lidar_position) and utils.distance_2d(radar_position, [0, 0, 0]) < 15.0:
# print "Elegir lidar: ", lidar_position
# object_position = lidar_position[:]
# else:
# object_position = radar_position[:]
# object_position[2] = camera_coordinates[2]
# object_position[1] += 0.4
# print real_time
# print radar_position, camera_angle
# print object_position, lidar_position, estimated_position
# print prev_lidar_time, next_lidar_time
# print lidar_position_prev, lidar_position_next
hasLidar = not utils.isempty(lidar_position)
hasRadar = not utils.isempty(radar_coordinates)
hasRadarCam = not utils.isempty(radar_cam_coordinates)
if self.debug:
self.out.write("Lidar: " + str(hasLidar) + " " + str(lidar_position) + "\n")
self.out.write("Radar: " + str(hasRadar) + " " + str(radar_coordinates) + "\n")
self.out.write("RadarCam: " + str(hasRadarCam) + " " + str(radar_cam_coordinates) + "\n")
self.out.write("Camera: " + str(camera_coordinates) + "\n")
self.out.write("EstPos: " + str(estimated_position) + "\n")
self.out.flush()
object_position = [0.0, 0.0, 0.0]
if utils.isempty(estimated_position) or not hasLidar:
if not utils.isempty(estimated_position):
distanceRadar = utils.distance_ang(estimated_position, radar_coordinates)
distanceRadarCam = utils.distance_ang(estimated_position, radar_cam_coordinates)
if abs(distanceRadar - distanceRadarCam) > 15:
if hasRadarCam and utils.distance_ang(radar_cam_coordinates, camera_coordinates) < 5:
object_position = radar_cam_coordinates
else:
object_position = camera_coordinates
else:
if distanceRadar < distanceRadarCam:
object_position = radar_coordinates
else:
object_position = radar_cam_coordinates
else:
if hasRadarCam and utils.distance_ang(radar_cam_coordinates, camera_coordinates) < 5:
object_position = radar_cam_coordinates
else:
object_position = camera_coordinates
else:
distanceLidar = utils.distance_ang(camera_coordinates, lidar_position)
distanceRadar = utils.distance_ang(estimated_position, radar_coordinates)
distanceRadarCam = utils.distance_ang(estimated_position, radar_cam_coordinates)
if (abs(distanceLidar) < 5 or not hasRadarCam) \
and (utils.distance_ang([0.0, 0.0, 0.0], lidar_position) < 20.0 or utils.distance_ang(radar_cam_coordinates, lidar_position) < 5.0):
#print distanceLidar
if abs(distanceLidar) > 5:
if hasRadarCam and utils.distance_ang(radar_cam_coordinates, camera_coordinates) < 5:
object_position = radar_cam_coordinates
else:
object_position = camera_coordinates
else:
object_position = lidar_position
if self.type == "Car":
object_position = lidar_position
else:
if distanceRadar < distanceRadarCam:
object_position = radar_coordinates
else:
object_position = radar_cam_coordinates
if hasRadarCam and utils.distance_ang(radar_cam_coordinates, camera_coordinates) < 5:
object_position = radar_cam_coordinates
else:
object_position = camera_coordinates
#if utils.isempty(object_position) and not utils.isempty(camera_coordinates):
# object_position = camera_coordinates
if self.debug:
self.out.write("Result: " + str(object_position) + "\n")
self.out.flush()
#print "Result: ", object_position
#object_position[1] = abs(object_position[1]) - 2
# if not utils.isempty(object_position):
# self.x, self.P = kalman_xy(self.x, self.P, object_position[:2], self.R)
# tmp = (self.x[:2]).tolist()
# tmp = [tmp[0][0], tmp[1][0], 0.0]
# self.object_positions.append(tmp)
# else:
# self.object_positions.append(object_position)
d = utils.distance_2d(object_position, [0,.0, 0.0, 0.0])
if type == "Car":
object_position[2] = -0.00 - 0.0095*d
else:
object_position[2] = -0.70 - 0.0095*d
#object_position[1] = abs(object_position[1]) - 2
self.object_positions.append(object_position)
#print real_time, self.last_realtime
#print estimated_position, object_position
#print "-->", self.get_previous_lidar(real_time)
velocity = [0.0, 0.0, 0.0]
if not utils.isempty(object_position) and not utils.isempty(self.last_position):
velocity = [np.divide(np.sum([object_position[i], -self.last_position[i]], dtype=np.float64), np.sum( [real_time/MS_IN_SEC, -self.last_realtime/MS_IN_SEC], dtype=np.float64)) \
for i in range(len(object_position))]
acceleration = [0.0, 0.0, 0.0]
if not utils.isempty(velocity) and not utils.isempty(self.last_velocity):
acceleration = [(velocity[i] - self.last_velocity[i]) / (real_time/MS_IN_SEC - self.last_realtime/MS_IN_SEC) \
for i in range(len(velocity))]
self.object_velocity.append(velocity)
self.object_acceleration.append(acceleration)
self.last_position = object_position
self.last_velocity = velocity
self.last_acceleration = acceleration
self.last_realtime = real_time