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")