def start(self): # First with Offboard mode, put the drone in a certain alttitude. When the drone is in that position or lower, lands it.
for i in range(10): # Waits 5 seconds for initialization
if self.current_heading is not None:
break
else:
print("Waiting for initialization.")
time.sleep(0.5)
for i in range(4)
self.landing_activity_pub.publish("LANDING")
time.sleep(0.1)
Basic_movement().moving_to_z(self.land_height) # Probably not necesary here
while (self.land_height - self.threshold) > self.local_pose.pose.position.z > (self.land_height + self.threshold) and (self.land_height - self.threshold) > self.down_sensor_distance > (self.land_height + self.threshold):
Basic_movement().moving_to_z(self.land_height)
time.sleep(0.2)
while self.local_pose.pose.position.z > self.final_land_threshold and self.down_sensor_distance > self.final_land_threshold:
Basic_movement().moving_down(self.landing_step_distance)
time.sleep(1)
for i in range (4):
Basic_movement().moving_down(self.landing_step_distance)
time.sleep(1)
def start(self):
for i in range(10): # Waits 5 seconds for initialization
if self.current_heading is not None:
break
else:
print("Waiting for initialization.")
time.sleep(0.5)
Basic_movement().moving_to_z(self.takeoff_height)
while (self.takeoff_height - self.threshold) <= self.local_pose.pose.position.z <= (self.takeoff_height + self.threshold) and (self.takeoff_height - self.threshold) <= self.down_sensor_distance <= (self.takeoff_height + self.threshold):
Basic_movement().moving_to_z(self.takeoff_height)
time.sleep(0.2)
self.flying_status = "Drone_lifted
self.hover()
def cb_forward_sensor(self, sense):
if (self.state == "HOVER"):
if sense.range < self.min_distance_to_evit:
print("no obstacle close to drone")
else:
#move opposite direction with the difference distance
diference = self.min_distance_to_evit - sense.range
Basic_movement().moving_back(diference)
rospy.logwarn("Avoiding Front Obstacle")
def hover(self):
if (self.flying_status == "Drone_lifted"):
while (rospy.is_shutdown() is False) and (self.flying_status != "LANDING"):
self.custom_activity_pub.publish("HOVER")
if (self.hover_hight - 0.03 <= self.local_pose.pose.position.z <= self.hover_hight + 0.03):
print("AdvanDiptera is hovering")
else:
print("correcting hover altitude")
Basic_movement().move_in_z(self.hover_hight - self.local_pose.pose.position.z)
time.sleep(0.1)
def start(self):
for i in range(10): # Waits 5 seconds for initialization
if self.current_heading is not None:
break
else:
print("Waiting for initialization.")
time.sleep(0.5)
self.takeoff_state = self.modechnge_takeoff()
while (self.takeoff_height - self.threshold) < self.local_pose.pose.position.z < (self.takeoff_height + self.threshold) and (self.takeoff_height - self.threshold) < self.down_sensor_distance < (self.takeoff_height + self.threshold):
self.autotakeoff()
time.sleep(0.2)
self.offboard_state = self.modechnge()
time.sleep(0.1)
Basic_movement().move_to_z(self.takeoff_height)
time.sleep(0.2)
self.hover()
def create_random_move(self):
print("RANDOM MOVE!")
print('IS DIRECTION BLOCKED?')
print('Blocking Right: ' + str(self.blockingMovementRight))
print('Blocking Left: ' + str(self.blockingMovementLeft))
print('Blocking Back: ' + str(self.blockingMovementBack))
print('Blocking Forward: ' + str(self.blockingMovementForward))
print('DIRECTION CHOSEN:')
if self.previousMovement == -1:
exitloop = False
elif self.previousMovement == 0:
if self.blockingMovementRight == False:
Basic_movement().move_right(self.moving_random_distance)
print("Random Movement ---> Still Right")
rospy.loginfo("Random Movement: Still Right!")
exitloop = True
elif self.previousMovement == 1:
if self.blockingMovementLeft == False:
Basic_movement().move_left(self.moving_random_distance)
print("Random Movement ---> Still Left")
rospy.loginfo("Random Movement: Still Left!")
exitloop = True
elif self.previousMovement == 2:
if self.blockingMovementBack == False:
Basic_movement().move_back(self.moving_random_distance)
print("Random Movement ---> Still Back")
rospy.loginfo("Random Movement: Still Back!")
exitloop = True
elif self.previousMovement == 3:
if self.blockingMovementForward == False:
Basic_movement().move_forward(self.moving_random_distance)
print("Random Movement ---> Still Forward")
rospy.loginfo("Random Movement: Still Forward!")
exitloop = True
# If it is blocked or first random movement, generate random movement
while exitloop == False:
# generate random integer values
value = randint(0, 3) # Random int value between 0 and 3
if value == 0:
if self.blockingMovementRight == False:
exitloop = True
Basic_movement().move_right(self.moving_random_distance)
print("Random Movement ---> Right")
rospy.loginfo("Random Movement: Right!")
self.previousMovement = 0
elif value == 1:
if self.blockingMovementLeft == False:
exitloop = True
Basic_movement().move_left(self.moving_random_distance)
print("Random Movement ---> Left")
rospy.loginfo("Random Movement: Left!")
self.previousMovement = 1
elif value == 2:
if self.blockingMovementBack == False:
exitloop = True
Basic_movement().move_back(self.moving_random_distance)
print("Random Movement ---> Back")
rospy.loginfo("Random Movement: Back!")
self.previousMovement = 2
elif value == 3:
if self.blockingMovementForward == False:
exitloop = True
Basic_movement().move_forward(self.moving_random_distance)
print("Random Movement ---> Forward")
rospy.loginfo("Random Movement: Forward!")
self.previousMovement = 3