def drone_creeping_line(self):
#if maybe wall
if self.Drone_lidar.detect_wall() == 2:
self.jetbot_move_pub.publish("stop")
self.cmd_pub.publish(controls.hold())
#if definitely wall:
# turn around and search and align to jetbot
if self.Drone_lidar.detect_wall() == 1:
print("turn around")
self.jetbot_move_pub.publish("stop")
if self.wall_count % 2 == 0:
self.cmd_pub.publish(controls.control(az=1))
else:
self.cmd_pub.publish(controls.control(az=-1))
time.sleep(3.5)
self.wall_count += 1
while not self.drone_search_jetbot(True):
pass
if not self.wall_count == self.max_wallcount:
self.jetbot_move_pub.publish("patrol")
self.cmd_pub.publish((controls.control(z=0.8)))
time.sleep(1)
#if no wall
if self.Drone_lidar.detect_wall() == 0:
self.cmd_pub.publish(controls.control(y=0.8))
self.jetbot_move_pub.publish("patrol")
print("no wall")
"""
def camera_callback(self, msg):
img = bridge.imgmsg_to_cv2(msg, "bgr8")
img = imutils.resize(img, width=300)
debug_img = img.copy()
img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
if not self.lander.init_background(img):
return
# Filter background
dfilter = self.lander.deriv_filter(img)
img_filtered = cv2.bitwise_and(img_hsv, img_hsv, mask=dfilter)
vel = [0,0,0,0]
if self.last_range is not None:
range_time, range = self.last_range
if time.time()-range_time < 0.5:
#self.update_state(vel, range)
if range < 1500:
self.cmd_vel.publish(controls.control(az=0.7))
rospy.sleep(4)
else:
vel[1] = 0.5
self.cmd_vel.publish(controls.control(x=vel[0], y=vel[1], z=vel[2], az=vel[3]))
# Setup velocity timeout if set
#if self.vel_timeout is not None:
# self.vel_timeout.shutdown()
#self.vel_timeout = rospy.Timer(rospy.Duration(1), self.reset_vel(), oneshot=True)
self.bg_img.publish(bridge.cv2_to_imgmsg(dfilter))
self.debug_img.publish(bridge.cv2_to_imgmsg(debug_img))
def go_trough_gate(self):
logging.info("Decision Control - Go Through Gate")
sx = 1
sz = 0.6
self.cmd_pub.publish(controls.control(y=sx, z=-sz))
time.sleep(1.5)
self.cmd_pub.publish(controls.control(y=sx, z=sz))
time.sleep(1.5)
self.cmd_pub.publish(controls.hold())
time.sleep(1.5)
return 1
def camera_callback(self, msg):
img = bridge.imgmsg_to_cv2(msg, "bgr8")
debugimg = img.copy()
#pltimg = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
#plt.imshow(pltimg)
#plt.show()
#image is already w = 300
#img = imutils.resize(img, width=300)
img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
if not self.lander.init_background(img):
return
# Filter background
dfilter = self.lander.deriv_filter(img)
img_filtered = cv2.bitwise_and(img_hsv, img_hsv, mask=dfilter)
#img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
#img_filtered = cv2.bitwise_and(img, img, mask=dfilter)
# Find craft position
#craft_pos = self.lander.find_craft_leds(img_hsv, self.led_blue, self.led_red_lower, self.led_red_higher, debug_img=img)
craft_pos = self.lander.find_craft_frame(dfilter, debug_img=debugimg)
# Update craft velocity
vx, vy, vz = 0, 0, 0
if craft_pos is not None:
x, y, a, h = craft_pos
land_pos = (0.5, 0.5)
vx, vy = self.lander.land_update(img_filtered,
x,
y,
a,
h,
land_pos,
debug_img=debugimg)
ih, iw, _ = img.shape
target = (iw * land_pos[0], ih * land_pos[1])
if abs(x - target[0]) < 0.05 * iw and abs(
y - target[1] < 0.05 * ih):
vz = -0.4 * self.landingtimer()
if h < 0.62:
self.lander.land()
else:
self.landingtimer(reset=True)
# Publish velocity command
self.cmd_vel.publish(controls.control(x=vx, y=vy, z=vz))
# Setup velocity timeout if set
#if self.vel_timeout is not None:
# self.vel_timeout.shutdown()
#self.vel_timeout = rospy.Timer(rospy.Duration(1), self.reset_vel(), oneshot=True)
#"""
self.bg_img.publish(bridge.cv2_to_imgmsg(dfilter))
self.debug_img.publish(bridge.cv2_to_imgmsg(debugimg))
def search(self):
self.cmd_pub.publish(controls.control(az = np.sign(self.metronomedir)*0.5 ))
time.sleep(0.3)
self.metronome += self.metronomedir
print("metronome, dir", self.metronome, self.metronomedir)
if abs(self.metronome) > self.metronome_sweepsize:
self.metronomedir = -self.metronomedir
self.metronome_sweepsize +=1
if self.metronome == 0 and self.metronomedir == 1:
self.cmd_pub.publish(controls.control(y=0.8))
time.sleep(1)
print("go forward")
"""
def move_around_gate(self, alpha, gate_dist, gate_x, gate_z):
# y+
# ^
# x- <- -> x+
# v
# y-
# . = z+
# + = z-
# clockwise = az+ ?
# cntrclockwise = az- ?
#
# looking from gate: if drone is left: alpha < 0
# right: alpha > 0
#looking from drone: if gate is left: x < 0
# right: x > 0
# high: z < 0
# low: z > 0
#print("move")
#print(alpha,gate_x,gate_z,gate_dist)
#see what input is like, the convert alpha to deg, with 0 = center, +90 = left, -90 = right
#convert x and z to % of image
#set speeds to 0
sx,sy,sz,saz = 0,0,0,0
#TODO check signs for all
#angle ranges from -0.1 to + 0.1
alpha = np.sign(alpha)*min(abs(alpha*10), 0.3)
sy += alpha
saz += -alpha
print("alpha,dist,x,y" ,(alpha, gate_dist, gate_x, gate_z))
#if abs(gate_x < 0.1):
# gate_x = np.sign(gate_x)*0.1
#if abs(gate_z < 0.1):
# gate_z = np.sign(gate_z)*0.1
control_multiple = 1.5
gate_x = gate_x*control_multiple
gate_z = gate_z*control_multiple*2
gate_x = np.sign(gate_x)*min(abs(gate_x), 0.3)
gate_z = np.sign(gate_z)*min(abs(gate_z), 0.3)
sy += gate_x
sz += -gate_z
sx = min(gate_dist - self.target_dist, 0.2) #max speed =1
sx = sx*control_multiple
if use_sqrt_for_motion:
sqrt_cali = 10.0
#sx = np.sign(sx)*np.sqrt(abs(sx)/sqrt_cali)
sy = np.sign(sy)*np.sqrt(abs(sy)/sqrt_cali)
sz = np.sign(sz)*np.sqrt(abs(sz)/sqrt_cali)
#saz = np.sign(saz)*np.sqrt(abs(saz)/sqrt_cali)
self.cmd_pub.publish(controls.control(y = sx, x = sy, z = sz, az = saz))
def align_to_platform(self, platform_pos):
self.reset_metronome()
self.searctime = None
platform_x, platform_y, platform_size = platform_pos
platform_size = (platform_size - 70.0) / 70.0
self.align_pid_angle.setpoint = 0
self.align_pid_dist.setpoint = 0
self.align_pid_height.setpoint = 0.7
print("platform_pos: ", platform_pos)
saz = platform_x / 2.0
sz = 0.75 * (platform_size - 70) / 70.0
sy = 1.0 * (-1) * (platform_y - 0.7)
saz = self.align_pid_angle(-platform_x)
sz = self.align_pid_height(platform_y)
sy = self.align_pid_dist(platform_size)
self.cmd_pub.publish(controls.control(az=saz, y=sy, z=sz))
if abs(platform_x) < 0.15 and abs(platform_y) < 0.8 and abs(
platform_y) > 0.6 and abs(platform_size) < 0.15:
return 1
return 0
def search(self):
if self.searctime is None:
self.searctime = time.time()
if time.time() - self.searctime < 1:
pass
#self.cmd_pub.publish(controls.control(az = 0.5))
else:
self.cmd_pub.publish(controls.control(y=-0.4))
def both_scramble(self):
self.cmd_takeoff.publish(Empty())
time.sleep(4.0)
self.jetbot_move_pub.publish("circle")
self.cmd_pub.publish(controls.control(y=0.7, az=0.7))
time.sleep(4.0)
self.cmd_pub.publish(controls.hold())
time.sleep(1.0)
def search(self):
logging.info("Decision Control - Search for Gate")
self.cmd_pub.publish(controls.control(az=np.sign(self.metronome) *
0.6))
time.sleep(0.5)
self.metronome += 1
if self.metronome > 10:
self.metronome = -15
def fly_to_platform(self, platform_x):
if abs(platform_x) > 0.05:
self._align_to_platform(platform_x)
else:
sx = 1
saz = platform_x / 3.0
self.cmd_pub.publish(controls.control(y=sx, az=saz))
print("fly" + str(saz))
def rotation_search(self):
self.cmd_pub.publish(
controls.control(az=np.sign(self.metronomedir) * 0.6, y=-0.2))
time.sleep(0.3)
self.metronome += self.metronomedir
print("metronome, dir", self.metronome, self.metronomedir)
if abs(self.metronome) > self.metronome_sweepsize:
self.metronomedir = -self.metronomedir
self.metronome_sweepsize += 1
def go_trough_gate(self):
sx = 1.7
sz = 0.6
stop = 1
for i in range(5):
print("GO")
self.cmd_pub.publish(controls.hold())
time.sleep(0.2)
self.cmd_pub.publish(controls.control(y=sx/4, z = -sz/4))
time.sleep(0.5)
self.cmd_pub.publish(controls.control(y=sx/2, z = -sz/2))
time.sleep(0.5)
self.cmd_pub.publish(controls.control(y=3*sx/4, z = -3*sz/4))
time.sleep(0.5)
self.cmd_pub.publish(controls.control(y=sx, z = -sz))
time.sleep(1.5)
self.cmd_pub.publish(controls.control(y=-stop))
time.sleep(1)
self.cmd_pub.publish(controls.control(z = sz))
time.sleep(1.5)
self.cmd_pub.publish(controls.hold())
time.sleep(1)
self.reset_metronome()
return 1
def move_around_gate(self, alpha, gate_x, gate_z, gate_dist):
logging.info("Decision Control - Angle Correction")
# y+
# ^
# x- <- -> x+
# v
# y-
# . = z+
# + = z-
# clockwise = az+ ?
# cntrclockwise = az- ?
#
# looking from gate: if drone is left: alpha < 0
# right: alpha > 0
#looking from drone: if gate is left: x < 0
# right: x > 0
# high: z < 0
# low: z > 0
#print("move")
#print(alpha,gate_x,gate_z,gate_dist)
#see what input is like, the convert alpha to deg, with 0 = center, +90 = left, -90 = right
#convert x and z to % of image
#set speeds to 0
sx, sy, sz, saz = 0, 0, 0, 0
#TODO check signs for all
#angle ranges from -0.1 to + 0.1
sy += alpha * 10.0
saz += -alpha * 10.0
print("alpha,dist,x,y", (alpha, gate_dist, gate_x, gate_z))
#if abs(gate_x < 0.1):
# gate_x = np.sign(gate_x)*0.1
#if abs(gate_z < 0.1):
# gate_z = np.sign(gate_z)*0.1
control_multiple = 1
gate_x = gate_x * control_multiple
gate_z = gate_z * control_multiple
sy += gate_x
sz += -gate_z
sx = min(gate_dist - self.target_dist, 1) #max speed =1
sx = sx * control_multiple
self.cmd_pub.publish(controls.control(y=sx, x=sy, z=sz, az=saz))
def fly_over_platform(self, platform_pos):
if platform_pos is None:
return 0
platform_x, platform_y, platform_size = platform_pos
self.searctime = None
if platform_pos is None:
return 0
elif abs(platform_x) > 0.1 and abs(platform_y) > 0.8 and abs(
platform_y
) < 0.6 and platform_size > 60 and platform_size < 80:
self.align_to_platform(platform_pos)
return 0
self.cmd_pub.publish(controls.hold())
time.sleep(0.2)
sx = 1
self.cmd_pub.publish(controls.control(y=sx))
return 1
def move_around_gate(self, alpha, gate_x, gate_z, gate_dist):
#see what input is like, the convert alpha to deg, with 0 = center, +90 = left, -90 = right
#convert x and z to % of image
target_dist = 1 #m
#set speeds to 0
sx, sy, sz, saz = 0, 0, 0, 0
#check signs for all
sy += alpha / 90.0 #at 90 deg, speed=max=1
saz += alpha / 90.0
sy += gate_x
sz += -gate_z
sx = min(gate_dist - target_dist, 1) #max speed =1
self.cmd_pub.publish(controls.control(y=sx, x=sy, z=sz, az=saz))
def go_trough_gate(self):
sx = 1
self.cmd_pub.publish(controls.control(y=sx))
time.sleep(2)
self.cmd_pub.publish(controls.hold())
return 1
def _align_to_platform(self, platform_x):
saz = platform_x
self.cmd_pub.publish(controls.control(az=saz))
print("align" + str(saz))
def search_pattern():
self.cmd_pub.publish(controls.control(az=0.5))
def image_processing(self, img):
#img = bridge.imgmsg_to_cv2(msg, "bgr8")
debugimg = img
img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
####calling init_background, this will return 1 when process complete
if not self.lander.init_background(img):
return
# Filter background
dfilter = self.lander.deriv_filter(img)
img_filtered = cv2.bitwise_and(img_hsv, img_hsv, mask=dfilter)
# img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# img_filtered = cv2.bitwise_and(img, img, mask=dfilter)
# Find craft position
# craft_pos = self.lander.find_craft_leds(img_hsv, self.led_blue, self.led_red_lower, self.led_red_higher, debug_img=img)
####craft pos = x,y,a,h (==posx, posy, angle, height)
#### = None if spotted elements too small
craft_pos = self.lander.find_craft_frame(dfilter, debug_img=debugimg)
# Update craft velocity
vx, vy, vz = 0, 0, 0
if craft_pos is not None:
x, y, a, h = craft_pos
land_pos = (0.5, 0.5)
vx, vy = self.lander.land_update(img_filtered,
x,
y,
a,
h,
land_pos,
debug_img=debugimg)
ih, iw, _ = img.shape
target = (iw * land_pos[0], ih * land_pos[1])
if abs(x - target[0]) < 0.03 * iw and abs(
y - target[1] < 0.03 * ih):
vz = -0.4 * self.landingtimer()
print("go down")
if h < 0.62 and self.landingtimer() and not self.landed:
self.lander.land()
self.landed = 1
self.time_of_landing = time.time()
print("land")
else:
self.landingtimer(reset=True)
# Publish velocity command
self.cmd_vel.publish(controls.control(x=vx, y=vy, z=vz))
#after 15 sec of landing, check if landing was succesful
if self.landed == 1 and time.time() - self.time_of_landing > 15:
if craft_pos is None:
self.landed = 0
self.landing_done_pub.publish(self.landed)
self.landed = 0
# Setup velocity timeout if set
# if self.vel_timeout is not None:
# self.vel_timeout.shutdown()
# self.vel_timeout = rospy.Timer(rospy.Duration(1), self.reset_vel(), oneshot=True)
# """
#publish debugs at limited fps
if time.time() - self.debug_pubtimer > 0.2:
self.debug_pubtimer = time.time()
self.bg_img.publish(bridge.cv2_to_imgmsg(dfilter))
self.debug_img.publish(bridge.cv2_to_imgmsg(debugimg))