class Simple_Mission():
def __init__(self):
rospy.init_node("simple_mission", log_level=rospy.INFO)
#Publisher for Waypoint message
self.pub = rospy.Publisher("/wolfdrone/Waypoint",
waypoint,
queue_size=10)
#Subscriber for humanDetector message
rospy.Subscriber("/wolfdrone/humanDetector",
human,
self.callback,
queue_size=10)
#Flag gets updated when human_detector.py sends a message
self.update_flag = False
#Flag gets updated when human_detector.py finds H**o Sapien !
self.human_flag = False
#His/Her Coordinates are updated in this attribute
self.human_x = 0
self.human_y = 0
self.msg = waypoint()
self.drone = Vehicle()
self.drone.home_lat = rospy.get_param("~home_lat")
self.drone.home_lon = rospy.get_param("~home_lon")
self.drone.drop_lat = rospy.get_param("~drop_lat")
self.drone.drop_lon = rospy.get_param("~drop_lon")
self.drone.land_lat = rospy.get_param("~land_lat")
self.drone.land_lon = rospy.get_param("~land_lon")
def callback(self, data):
print("Inside Waypoint Callback")
self.update_flag = data.lookforCameraResponseFlag
self.human_flag = data.humanFlag
self.human_x = data.x
self.human_y = data.y
def startup(self):
#Load the mission waypoints
self.wp_list = Utils.single_wp()
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 cleanup(self):
pass
def run(self):
self.startup()
self.mission()
self.cleanup()
class Simple_Mission():
def __init__(self):
rospy.init_node("simple_mission", log_level=rospy.INFO)
cwd = os.getcwd()
print(cwd)
self.drone = Vehicle()
self.drone.home_lat = rospy.get_param("~home_lat")
self.drone.home_lon = rospy.get_param("~home_lon")
self.drone.drop_lat = rospy.get_param("~drop_lat")
self.drone.drop_lon = rospy.get_param("~drop_lon")
self.drone.land_lat = rospy.get_param("~land_lat")
self.drone.land_lon = rospy.get_param("~land_lon")
def startup(self):
l = rospy.get_param("~major_axis")
w = rospy.get_param("~minor_axis")
x0 = rospy.get_param("~x_center")
y0 = rospy.get_param("~y_center")
alt = rospy.get_param("~alt")
grid_step = rospy.get_param("~scan_step")
self.wp_list = Utils.elliptical_wp(l, w, x0, y0, grid_step, alt)
print(self.wp_list)
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")
def cleanup(self):
pass
def run(self):
# type: () -> object
self.startup()
self.mission()
self.cleanup()
