def run(self):
if abs(Utils.compute_distance(self.land_pose,self.drone.pose)) > 0.5:
self.drone.setpoint_pose(self.land_pose)
else:
self.drone.land(self.land_lat,self.land_lon)
rospy.loginfo("Mission Complete")
exit()
def run(self):
if not self.tracking:
if abs(Utils.compute_distance(self.curr_sp,
self.drone.pose)) > 0.5:
self.drone.setpoint_pose(self.curr_sp)
else:
if not len(self.wp_list) == 0:
curr_sp = self.wp_list.pop()
self.curr_sp.pose.position.x = curr_sp[0]
self.curr_sp.pose.position.y = curr_sp[1]
self.curr_sp.pose.position.z = curr_sp[2]
# else:
else:
self.drone.setpoint_vel(TwistStamped())
def mission(self):
### PX4 rejects the offboard control mode without few setpoint commands already.
### So adding arbitrary number of waypoints before making offboard
rate = rospy.Rate(10)
t = self.wp_list[-1]
pose = PoseStamped()
pose.pose.position.x = t[0]
pose.pose.position.y = t[1]
pose.pose.position.z = t[2]
for i in range(10):
self.drone.setpoint_pose(pose)
rate.sleep()
print("Sent Arbitrary Number of waypoints")
self.drone.command_mode('OFFBOARD')
self.drone.arm()
print("List of waypoints")
print(self.wp_list)
### Waypoint Navigation code
counter = 0
mission_completed = False
rate = rospy.Rate(100)
while mission_completed == False:
rate.sleep()
# set drone pose to way point if drone hasn't reached there yet
if abs(Utils.compute_distance(pose, self.drone.pose)) > 0.5:
self.drone.setpoint_pose(pose)
else:
print("Waypoint is reached")
#Publish the Waypoint Reached Flag for camera to take picture
self.msg.waypointReachedFlag = True
self.pub.publish(self.msg)
# just a flag to disable human detection and change back to traversing set way points
if takePic:
# While human detection node has not published any new info set pose as current pose
start_time = time.time()
while (self.update_flag == False):
self.drone.setpoint_pose(pose)
# If human detection node has not published any info for 3 seconds continue scanning
if (time.time() - start_time > 3.0):
print(
"Human detector did not publish any new messages"
)
self.human_flag = False
break
self.msg.waypointReachedFlag = False
self.pub.publish(self.msg)
# Human Detection node has published message, Now check if human detected
if self.human_flag == True:
print(
"Human detected in the area! Drone to the Rescue!!!"
)
if pose.pose.position.z < 5:
print("Reached Human. Enter HOLD mode")
mission_completed = True
break
else:
pose.pose.position.z -= 2
# Human Coordinates are relative to the drone's position
pose.pose.position.x = self.human_x + pose.pose.position.x
pose.pose.position.y = self.human_y + pose.pose.position.y
else:
# If all the waypoints have been traversed update mission completed flag
print(
"Human not detected in the area. Moving on to scanning..."
)
if len(self.wp_list) == 0:
mission_completed = True
break
t = self.wp_list.pop()
pose.pose.position.x = t[0]
pose.pose.position.y = t[1]
pose.pose.position.z = t[2]
else: #/* Else part of if take pic */
if len(self.wp_list) == 0:
mission_completed = True
break
t = self.wp_list.pop()
# Normal wayopint traversal
pose.pose.position.x = t[0]
pose.pose.position.y = t[1]
pose.pose.position.z = t[2]
# Disable way point reached flag
self.msg.waypointReachedFlag = False
self.pub.publish(self.msg)
print(t)
# Once Reached near Human, HOLD at the current location
start_time = time.time()
while time.time() - start_time < 3:
continue
print("Position held for 3 seconds.Land Initiated")
land_pose = PoseStamped()
#land_local = Utils.convert_global2local(self.drone.home_lat,self.drone.home_lon,self.drone.land_lat,self.drone.land_lon)
land_pose.pose.position.x = 0 #land_local[0]
land_pose.pose.position.y = 0 #land_local[1]
land_pose.pose.position.z = 2
for i in range(10):
self.drone.setpoint_pose(land_pose)
rate.sleep()
print("Sent Arbitrary Number of waypoints")
self.drone.command_mode('OFFBOARD')
# Move towards land position
while abs(Utils.compute_distance(land_pose, self.drone.pose)) > 0.5:
self.drone.setpoint_pose(land_pose)
rate.sleep()
self.drone.land(self.drone.land_lat, self.drone.land_lon)
print("Mission Complete")
def run(self):
if abs(Utils.compute_distance(self.drop_pose, self.drone.pose)) > 0.5:
self.drone.setpoint_pose(self.drop_pose)
else:
self.mission.switch_state(Drop_Controller(self.mission,
self.drone))
def mission(self):
### PX4 rejects the offboard control mode without few setpoint commands already.
### So adding arbitrary number of waypoints before making offboard
rate = rospy.Rate(10)
print(self.wp_list)
t = self.wp_list.pop()
pose = PoseStamped()
pose.pose.position.x = t[0]
pose.pose.position.y = t[1]
pose.pose.position.z = t[2]
print(pose)
for i in range(10):
self.drone.setpoint_pose(pose)
rate.sleep()
print("Sent Arbitrary Number of waypoints")
self.drone.command_mode('OFFBOARD')
self.drone.arm()
### Waypoint Navigation code
while not len(self.wp_list) == 0:
print(Utils.compute_distance(pose, self.drone.pose))
if abs(Utils.compute_distance(pose, self.drone.pose)) > 0.5:
self.drone.setpoint_pose(pose)
rate.sleep()
else:
t = self.wp_list.pop()
pose.pose.position.x = t[0]
pose.pose.position.y = t[1]
pose.pose.position.z = t[2]
### On Scanning the area, proceeding to the drop zone
drop_pose = PoseStamped()
drop_local = Utils.convert_global2local(self.drone.home_lat,
self.drone.home_lon,
self.drone.drop_lat,
self.drone.drop_lon)
drop_pose.pose.position.x = drop_local[0]
drop_pose.pose.position.y = drop_local[1]
drop_pose.pose.position.z = 5
while abs(Utils.compute_distance(drop_pose, self.drone.pose)) > 0.5:
self.drone.setpoint_pose(drop_pose)
rate.sleep()
self.drone.land(self.drone.drop_lat, self.drone.drop_lon)
#### Once Dropped, Continue to the land location.
land_pose = PoseStamped()
land_local = Utils.convert_global2local(self.drone.home_lat,
self.drone.home_lon,
self.drone.land_lat,
self.drone.land_lon)
land_pose.pose.position.x = land_local[0]
land_pose.pose.position.y = land_local[1]
land_pose.pose.position.z = 5
for i in range(10):
self.drone.setpoint_pose(land_pose)
rate.sleep()
print("Sent Arbitrary Number of waypoints")
self.drone.command_mode('OFFBOARD')
while abs(Utils.compute_distance(land_pose, self.drone.pose)) > 0.5:
self.drone.setpoint_pose(land_pose)
rate.sleep()
self.drone.land(self.drone.land_lat, self.drone.land_lon)
print("Mission Complete")
