def move_to_target_cam_forward(self,target_info,attitude,location):
x,y = target_info[1]
pitch_dir,yaw_dir = balloon_finder.pixels_to_direction(x, y, attitude.roll, attitude.pitch, attitude.yaw)
yaw_dir = yaw_dir % 360
print "Target Yaw:%f" %(yaw_dir)
target_distance = target_info[2]*10
print "Target Distance:%f" %(target_distance)
#shift origin to center of image
print "Found Target at yaw:%f heading:%f Alt:%f Dist:%f" % (attitude.yaw,yaw_dir,location.z,target_distance)
sc_logger.text(sc_logger.GENERAL, "Distance to target: {0}".format(round(target_distance,2)))
print "mode detection"
#self.condition_yaw(math.fabs(math.degrees(target_heading)))
if(target_distance > 4):
vx = 0
vy = 0
vz = 0
else:
vx = 0
vy = 0
vz = 0
#send velocity commands toward target heading
self.send_nav_velocity(vx,vy,vz)
self.condition_yaw(yaw_dir)
def analyze_image(self):
# record time
now = time.time()
# capture vehicle position
self.vehicle_pos = PositionVector.get_from_location(
self.vehicle.location.global_relative_frame)
# capture vehicle attitude in buffer
self.att_hist.update()
# get delayed attitude from buffer
veh_att_delayed = self.att_hist.get_attitude(now - self.attitude_delay)
# get new image from camera
f = self.get_frame()
# look for balloon in image using blob detector
self.balloon_found, xpos, ypos, size = balloon_finder.analyse_frame(f)
# add artificial horizon
balloon_finder.add_artificial_horizon(f, veh_att_delayed.roll,
veh_att_delayed.pitch)
if self.balloon_found:
# record time balloon was found
self.last_spotted_time = now
# convert x, y position to pitch and yaw direction (in radians)
self.balloon_pitch, self.balloon_heading = balloon_finder.pixels_to_direction(
xpos, ypos, veh_att_delayed.roll, veh_att_delayed.pitch,
veh_att_delayed.yaw)
self.balloon_pitch = math.radians(self.balloon_pitch)
self.balloon_pitch_top = self.balloon_pitch + balloon_video.pixels_to_angle_y(
size) # add balloon radius so we aim for top of balloon
self.balloon_heading = math.radians(self.balloon_heading)
# get distance
self.balloon_distance = get_distance_from_pixels(
size, balloon_finder.balloon_radius_expected)
# updated estimated balloon position
self.balloon_pos = balloon_finder.project_position(
self.vehicle_pos, self.balloon_pitch, self.balloon_heading,
self.balloon_distance)
# save image for debugging later
if not self.writer is None:
self.writer.write(f)
# increment frames analysed for stats
self.num_frames_analysed += 1
if self.stats_start_time == 0:
self.stats_start_time = time.time()
def analyze_image(self):
# record time
now = time.time()
# capture vehicle position
self.vehicle_pos = PositionVector.get_from_location(self.vehicle.location.global_relative_frame)
# capture vehicle attitude in buffer
self.att_hist.update()
# get delayed attitude from buffer
veh_att_delayed = self.att_hist.get_attitude(now - self.attitude_delay)
# get new image from camera
f = self.get_frame()
# look for balloon in image using blob detector
self.balloon_found, xpos, ypos, size = balloon_finder.analyse_frame(f)
# add artificial horizon
balloon_finder.add_artificial_horizon(f, veh_att_delayed.roll, veh_att_delayed.pitch)
if self.balloon_found:
# record time balloon was found
self.last_spotted_time = now
# convert x, y position to pitch and yaw direction (in radians)
self.balloon_pitch, self.balloon_heading = balloon_finder.pixels_to_direction(xpos, ypos, veh_att_delayed.roll, veh_att_delayed.pitch, veh_att_delayed.yaw)
self.balloon_pitch = math.radians(self.balloon_pitch)
self.balloon_pitch_top = self.balloon_pitch + balloon_video.pixels_to_angle_y(size) # add balloon radius so we aim for top of balloon
self.balloon_heading = math.radians(self.balloon_heading)
# get distance
self.balloon_distance = get_distance_from_pixels(size, balloon_finder.balloon_radius_expected)
# updated estimated balloon position
self.balloon_pos = balloon_finder.project_position(self.vehicle_pos, self.balloon_pitch, self.balloon_heading, self.balloon_distance)
# save image for debugging later
if not self.writer is None:
self.writer.write(f)
# increment frames analysed for stats
self.num_frames_analysed += 1
if self.stats_start_time == 0:
self.stats_start_time = time.time()