def choose_target(self, targets):
""" Choose the neareset possible target. """
# Should never be called with empty targets.
if not targets:
log.error('choose_target was called with no targets.')
return None
closest_target = targets[0]
min_distance = 1000
if len(targets) == 1:
return closest_target
# self.current_pose = self.explorer.pose_stamped.pose
for target in targets:
try:
(trans, rot) = self.transform_listener.lookupTransform(target.victimPose.header.frame_id,
'/base_footprint',
Time(0))
except:
log.error("Transform listener failed to acquire transformation")
return closest_target
self.current_pose = Pose()
self.current_pose.position.x = trans[0]
self.current_pose.position.y = trans[1]
self.current_pose.position.z = trans[2]
target_pose = target.victimPose.pose
target_distance = distance_2d(target_pose, self.current_pose)
if target_distance < min_distance:
min_distance = target_distance
closest_target = target
return closest_target
def get_closest_qr(self, qrs):
print qrs
closet_qr = qrs[0]
min_distance = 1000
if len(qrs) == 1:
return closet_qr
# self.current_pose = self.explorer.pose_stamped.pose
for qr in qrs:
try:
(trans, rot) = self.transform_listener.lookupTransform(
qr.frame_id, '/base_footprint', Time(3))
except:
log.error(
"Transform listener failed to acquire transformation")
return closet_qr
current_pose = Pose()
current_pose.position.x = trans[0]
current_pose.position.y = trans[1]
current_pose.position.z = trans[2]
target_pose = qr.qrPose.pose
target_distance = distance_2d(target_pose, current_pose)
if target_distance < min_distance:
min_distance = target_distance
closet_qr = qr
return closet_qr
def update_move_base_goal(self, new_pose, old_pose):
"""
Updates the current MoveBase goal while the target's pose
is updated.
If the distance between the new and the old pose is big enough
the agent is informed through the dispatcher to send a new
MoveBase goal.
:param new_pose: The target's new pose.
:param old_pose: The target's current pose.
"""
if distance_2d(old_pose.pose, new_pose.pose) > conf.BASE_THRESHOLD:
self.dispatcher.emit('move_base.resend', new_pose.pose)
def move_base_feedback(self, current, goal):
"""
The event is triggered from the move_base client when the
action server sends feedback with the current pose. Given
the goal and the current pose the agent checks if it is
close enough to the point of interest. This will work if the
move_base server takes too long to succeed.
:param :current The current PoseStamped received from the action
client.
:param :goal The PoseStamped goal of the current action.
"""
self.current_pose = current.base_position.pose
if distance_2d(goal.pose, self.current_pose) < conf.BASE_THRESHOLD:
log.warning('Base converged.')
self.base_converged.set()
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