def enter(self): self.drone.set_xy_speed(12) rospy.loginfo("Entering Drop Mode") self.drone.command_offboard() self.drone.arm() self.home_lat = rospy.get_param("~home_lat") self.home_lon = rospy.get_param("~home_lon") self.land_lat = rospy.get_param("~land_lat") self.land_lon = rospy.get_param("~land_lon") self.land_pose = PoseStamped() land_local = Utils.convert_global2local(self.home_lat,self.home_lon,self.land_lat,self.land_lon) self.land_pose.pose.position.x = land_local[0] self.land_pose.pose.position.y = land_local[1] self.land_pose.pose.position.z = 5
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")