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