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 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 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 handle_exit(signum, frame):
cmd_vel = rospy.Publisher('/tello/cmd_vel', Twist, queue_size=1)
cmd_land = rospy.Publisher('/tello/land', Empty, queue_size=1)
rospy.sleep(0.5)
print("Landing and reseting velocity")
cmd_land.publish(Empty())
cmd_vel.publish(controls.hold())
rospy.signal_shutdown("Landing and reseting velocity")
sys.exit(0)
def handle_exit(signum, frame):
#def handle_exit():
cmd_pub = rospy.Publisher('/tello/cmd_vel', Twist, queue_size=1, latch = True)
cmd_land = rospy.Publisher('/tello/land', Empty, queue_size=1)
rospy.sleep(0.5)
print("set to neutral + landing")
cmd_land.publish(Empty())
cmd_pub.publish(controls.hold())
rospy.signal_shutdown("set to neutral + landing")
sys.exit(0)
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 handle_exit(signum, frame):
logging.info("Decision Control - Handle Exit")
cmd_pub = rospy.Publisher('/tello/cmd_vel',
Twist,
queue_size=1,
latch=True)
cmd_land = rospy.Publisher('/tello/land', Empty, queue_size=1)
rospy.sleep(0.5)
cmd_land.publish(Empty())
cmd_pub.publish(controls.hold())
print("set to neutral + landing")
rospy.sleep(1)
sys.exit(0)
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 handle_exit(signum, frame):
Main_control.Drone_lidar.close_threads = True
#MainControl.close_threads = True
cmd_pub = rospy.Publisher('/tello/cmd_vel',
Twist,
queue_size=1,
latch=True)
cmd_land = rospy.Publisher('/tello/land', Empty, queue_size=1)
do_landing_pub = rospy.Publisher("/land/execute", Int8, queue_size=1)
jetbot_move_pub = rospy.Publisher("/jetbot_move", String, queue_size=1)
rospy.sleep(1)
print("set to neutral + landing")
cmd_land.publish(Empty())
cmd_pub.publish(controls.hold())
do_landing_pub.publish(0)
jetbot_move_pub.publish("stop")
rospy.sleep(1)
sys.exit(0)
def reset_vel(self):
print("reset vel")
self.cmd_vel.publish(controls.hold())
def reset_vel(self):
self.cmd_vel.publish(controls.hold())
def handle_exit(signum, frame):
controls.hold()
print("put to neutral")
sys.exit(0)
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
