mavros_mission_push.py

#!/usr/bin/env python3

import rospy
from std_msgs.msg import String
from sensor_msgs.msg import NavSatFix
from mavros_msgs.msg import *
from mavros_msgs.srv import *


def create_waypoint():
    
	waypoint_clear_client()
	wl = []
	wp = Waypoint()

	wp.frame = 3
	wp.command = 22  # takeoff
	wp.is_current = False
	wp.autocontinue = True
	wp.param1 = 0  
	wp.param2 = 0
	wp.param3 = 0
	wp.param4 = 0
	wp.x_lat = 47.39765262686296
	wp.y_long = 8.545722794977497
	wp.z_alt = 5
	wl.append(wp)
    
	
	wp = Waypoint() 

	wp.frame = 3
	wp.command = 16  #Navigate to waypoint.
	wp.is_current = False
	wp.autocontinue = True
	wp.param1 = 0  # delay 
	wp.param2 = 0
	wp.param3 = 0
	wp.param4 = 0
	wp.x_lat = 47.39850232253372
	wp.y_long = 8.54589445634687
	wp.z_alt = 5
	wl.append(wp)

    
	wp = Waypoint()

	wp.frame = 3
	wp.command = 19  # Loiter
	wp.is_current = False
	wp.autocontinue = True
	wp.param1 = 0  # delay
	wp.param2 = 0
	wp.param3 = 0
	wp.param4 = 0
	wp.x_lat = 47.39742022907945 
	wp.y_long = 8.54268653450672
	wp.z_alt = 5
	wl.append(wp)
	
	

	wp = Waypoint()

	wp.frame = 3
	wp.command = 19  
	wp.is_current = False
	wp.autocontinue = True
	wp.param1 = 0  
	wp.param2 = 0
	wp.param3 = 0
	wp.param4 = 0
	wp.x_lat = 47.398974369763316
	wp.y_long = 8.543190789779251
	wp.z_alt = 5
	wl.append(wp)

	wp = Waypoint()

	wp.frame = 3
	wp.command = 19  
	wp.is_current = False
	wp.autocontinue = True
	wp.param1 = 0  
	wp.param2 = 0
	wp.param3 = 0
	wp.param4 = 0
	wp.x_lat = 47.39803753343713
	wp.y_long = 8.543813062243226
	wp.z_alt = 5
	wl.append(wp)

	wp = Waypoint()

	wp.frame = 3
	wp.command = 21  #landing
	wp.is_current = False
	wp.autocontinue = True
	wp.param1 = 0 
	wp.param2 = 0
	wp.param3 = 0
	wp.param4 = 0
	wp.x_lat = 47.3968247050806 
	wp.y_long = 8.543298078135109
	wp.z_alt = 5
	wl.append(wp)

	print(wl)
	
	try:
	    service = rospy.ServiceProxy('mavros/mission/push', WaypointPush, persistent=True)
	    service(start_index=0, waypoints=wl)
	    

	  
	    if service.call(wl).success: #burasi belki list istiyordur. birde oyle dene
	        print ('write mission success')
	    else:
	        print ('write mission error')
	  
	except rospy.ServiceException as e:
	    print ("Service call failed: %s" % e)



def waypoint_clear_client():
        try:
            response = rospy.ServiceProxy('mavros/mission/clear', WaypointClear)
            return response.call().success
        except (rospy.ServiceException, e):
            print ("Service call failed: %s" % e)
            return False


if __name__ == '__main__':
	rospy.init_node('waypoint_node', anonymous=True)
	pub = rospy.Publisher('global',String,queue_size=10)
	stat = ("here at the moment") # to be used later
	pub.publish(stat)
	create_waypoint()