Uploading fence to Ardupilot by dronekit

[rasheeddo]

I am using Cube purple with ArduRover 4.0, and am using dronekit for many of my applications.
But I tried to upload fence points by creating a similar mission file as QGC WPL 110 format (same like when we download from Mission Planner). On that text file, I changed the mission command value to 5002 and 5004 and also corresponding param1 and MAV_FRAME value, and I used the same method as describe here to upload the fence points, but it didn't work, and I found that it stuck at the while loop here.

After sometime of looking the code of dronekit and pymavlink and some trial and error, I found out some way to upload fence mission. So in case some one is looking for a similar thing, you can try this out.

• On Dronekit
  on __init__.py at line 46

dronekit-python/dronekit/__init__.py

Line 46 in 27b292b

from pymavlink.dialects.v10 import ardupilotmega

we need to change from pymavlink.dialects.v10 import ardupilotmega to pymavlink.dialects.v20 import ardupilotmega

from Command class,

dronekit-python/dronekit/__init__.py

Line 2871 in 27b292b

class Command(mavutil.mavlink.MAVLink_mission_item_message):

I added one extra function name as upload_fence(), which I copied from original upload()

On the new upload_fence(), instead of self._vehicle._master.waypoint_count_send(self._vehicle._wploader.count()), I changed to self._vehicle._master.fence_count_send(self._vehicle._wploader.count()).
And I comment out self._vehicle._master.waypoint_clear_all_send() because I don't want it to clear all my previous waypoint mission. Then we need to create a function fence_count_send() as well on mavutil.py

• On pymavlink
  on mavutil.py at def waypoint_count_send(self,seq) function here
  I copied and made a new function from that which called def fence_count_send(self, seq), and on that new function I changed mission_count_send() to mission_fence_count_send()

on dialects/v20/ardupilotmega.py
at line 18439, I copied mission_count_send(self, target_system, target_component, count, mission_type=0, force_mavlink1=False) function and made a new function called mission_fence_count_send(self, target_system, target_component, count, mission_type=1, force_mavlink1=False). Please make sure to have mission_type = 1 on the new function.

• On your script
  make sure you set this one os.environ["MAVLINK20"] = "1" on top

I have followed the way to download/upload waypoints from here, so on that page you will see an example of upload_mission() function, so I made a copy of that function and called upload_fence(). So the code is almost same but just the for loop
the original mission is like this

for command in missionlist:
	cmds.add(command)

But for fence, you will need to add command.mission_type=1 like this

for command in missionlist:
	command.mission_type = 1
	cmds.add(command)

because the mission message will set mission_type default as 0 which is for MISSION, so we need to change it to 1 for FENCE. More detail of mission_type is here

So your fence points file should be in QGC WPL 110 format, you can simply make some dummy fence and save file from Mission Planner and take a look in there how the format looks like.
Your MISSION_ITEM command should be 5000, 5002, 5003, or 5004 depends on what type of fence you need, please check detail from here
and the param1 and frame values are different for each fence type, so from my observation are here

Example1.
This is in case of MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION (5002)

QGC WPL 110
0	0	0	5002	4.00000000	0.00000000	0.00000000	0.00000000	35.84147800	139.52423140	0.000000	1
1	0	0	5002	4.00000000	0.00000000	0.00000000	0.00000000	35.84151770	139.52433130	0.000000	1
2	0	0	5002	4.00000000	0.00000000	0.00000000	0.00000000	35.84149100	139.52434140	0.000000	1
3	0	0	5002	4.00000000	0.00000000	0.00000000	0.00000000	35.84144430	139.52426430	0.000000	1

0 on third column is MAV_FRAME_GLOBAL or Absoulute in Mission Planner, more detail of MAV_FRAME https://mavlink.io/en/messages/common.html#MAV_FRAME
4 on fifth column is param1, and in this case it represents 4 points of polygon fence

Example2.
This is in case of MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION (5004)

QGC WPL 110
0	0	3	5004	2.00000000	0.00000000	0.00000000	0.00000000	35.84141770	139.52420800	0.000000	1
1	0	3	5004	6.00000000	0.00000000	0.00000000	0.00000000	35.84131330	139.52420260	0.000000	1

3 on third column is MAV_FRAME_GLOBAL_RELATIVE_ALT
2 and 6 on fifth column are param1 of each point, and it represents a radius of circle fence

Here is my full example code

import time
import os
import json
os.environ["MAVLINK20"] = "1"
from pymavlink.dialects.v20 import common as mavlink2
from dronekit import connect, VehicleMode, Command #
from pymavlink import mavutil


vehicle = None
is_vehicle_connected = False

################################### Functions #############################################
def vehicle_connect():
	global vehicle, is_vehicle_connected

	if vehicle == None:
		try:
			# if sim.startswith('sitl'):
			# print("Connecting to Ardupilot on SITL...")
			# vehicle = connect('udp:127.0.0.1:14551', wait_ready=True)
			# else:
			print("Connecting to Ardupilot....")
			# vehicle = connect('/dev/usb_uart', wait_ready=True, baud=921600)
			vehicle = connect('/dev/ttyUSB0', wait_ready=True, baud=921600)
			# vehicle = connect('udp:127.0.0.1:14551', wait_ready=True)
			
		except Exception as e:
			print(e)
			print("Device might not be found...check the udevrules")
			# quit()
			time.sleep(1)

	if vehicle == None:
		is_vehicle_connected = False
		return False
	else:
		is_vehicle_connected = True
		return True

def readmission(aFileName):
	global cmds
	"""
	Load a mission from a file into a list. The mission definition is in the Waypoint file
	format (http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format).

	This function is used by upload_mission().
	"""
	print("\nReading mission from file: %s" % aFileName)
	# cmds = vehicle.commands
	missionlist=[]
	with open(aFileName) as f:
		for i, line in enumerate(f):
			if i==0:
				if not line.startswith('QGC WPL 110'):
					raise Exception('File is not supported WP version')
			else:
				linearray=line.split('\t')
				ln_index=int(linearray[0])
				ln_currentwp=int(linearray[1])
				ln_frame=int(linearray[2])
				ln_command=int(linearray[3])
				ln_param1=float(linearray[4])
				ln_param2=float(linearray[5])
				ln_param3=float(linearray[6])
				ln_param4=float(linearray[7])
				ln_param5=float(linearray[8])
				ln_param6=float(linearray[9])
				ln_param7=float(linearray[10])
				ln_autocontinue=int(linearray[11].strip())
				cmd = Command( 0, 0, 0, ln_frame, ln_command, ln_currentwp, ln_autocontinue, ln_param1, ln_param2, ln_param3, ln_param4, ln_param5, ln_param6, ln_param7)
				# cmd = CommandSequence( 0, 0, 0, ln_frame, ln_command, ln_currentwp, ln_autocontinue, ln_param1, ln_param2, ln_param3, ln_param4, ln_param5, ln_param6, ln_param7)
				missionlist.append(cmd)

	f.close()

	return missionlist

def upload_mission_from_file():
	global cmds
	"""
	Upload a mission from a file. 
	"""
	path = os.getcwd()
	file_path = os.path.join(path, "MISSION2.waypoints")
	if os.path.exists(file_path):
		#Read mission from file
		missionlist = readmission(file_path)
		# print("missionlist", missionlist)
		print("\nUpload mission from a file: %s" % file_path)
		#Clear existing mission from vehicle
		print('Clear old mission...')
		# cmds = vehicle.commands
		cmds.clear()
		#Add new mission to vehicle
		# print("missionlist", missionlist)
		for command in missionlist:
			print("command", command)
			cmds.add(command)

		## sometime there is error of Nonetype not iterable
		## but the mission already uploaded, so just pass it
		try:
			vehicle.commands.upload()
			print('MISSION Uploaded')
		except Exception as e:
			print("Exception!!! error as -> %s, but don't care..." %e)
			print('MISSION Uploaded with some exception')
			pass

		return True
	else:
		print("ERROR missing MISSION.txt file")
		return False

def upload_fence_from_file():
	global cmds
	"""
	Upload a mission from a file. 
	"""
	path = os.getcwd()
	file_path = os.path.join(path, "FENCE3.waypoints")
	if os.path.exists(file_path):
		#Read mission from file
		missionlist = readmission(file_path)
		# print("missionlist", missionlist)
		print("\nUpload fence from a file: %s" % file_path)
		#Clear existing mission from vehicle
		# print('Clear old mission...')
		# cmds.clear()
		#Add new mission to vehicle
		print(missionlist)
		for command in missionlist:
			command.mission_type = 1
			print("command", command)
			cmds.add(command)

		## sometime there is error of Nonetype not iterable
		## but the mission already uploaded, so just pass it
		print("Start Uploading...")
		try:
			# vehicle.commands.upload()
			# cmds.upload()
			cmds.upload_fence()
			print('FENCE Uploaded')
		except Exception as e:
			print("Exception!!! error as -> %s, but don't care..." %e)
			print('FENCE Uploaded with some exception')
			pass

		return True
	else:
		print("ERROR missing FENCE.waypoints file")
		return False


############################# Initialized CUBE ##########################
print("INFO: Connecting to vehicle.")
while (not vehicle_connect()):
	pass
print("INFO: Vehicle connected.")



cmds = vehicle.commands


# upload_mission_from_file()
upload_fence_from_file()

I am not sure the proper way to do it, but this seems to work for temporary.
I will need to check that other of my feature like guide mode yaw and others still working properly after I changed to pymavlink.dialects.v20. So I am looking forward for a high-level of geofence uploading on dronekit without these modification by myself :) . If some of you guys want to do a PR according to my code here, feel free!

[alexwien7]

For mavlink1 vs mavlink´2 issues have you tried the workaround:

self.vehicle = dk.connect(address,.....
 ....
# enforce mavlink 2
# read also https://github.com/dronekit/dronekit-python/issues/935
 time.sleep(1)
 self.vehicle._master.first_byte = True
 self.log.debug("fix dronekit to reevaluate to use mavlink 2")

[ahmedzahran12]

I tried your code but I get this error :
Exception!!! error as -> 'CommandSequence' object has no attribute 'upload_fence', but don't care...
FENCE Uploaded with some exception

[Casavantii]

Hi There! Right now entering a fence erases the others. How would you go about keeping them? I am looking as well, but your help would be appreciated. Thank you by the way!!

[ssancheze]

Hi, I've tried your code -thanks!- and managed to get it working maybe twice before getting the error:
WARNING:autopilot:Fence upload timeout

Now it's stuck in the loop:
while False in self._vehicle._wp_uploaded:

In _wp_uploaded, only the first element is true.

Any ideas on how to fix it? Thanks