Modify map using drone footage

Introduction

This tutorial shows how to use an orthophoto (generated by taking aerial pictures with a drone) and modify the open mower map. In my case I recorded the boundary of my lawn area driving the mower around and later divided the area and added obstacles using the drone footage.

openmower map boundary with orthophoto layer

Generate an orthophoto

I took about 50 pictures with a drone ca. 10m above ground and generated an orthophoto using OpenDroneMap WebODM

You can read about the process here: https://docs.opendronemap.org/tutorials/

OpenDroneMap WebODM runs in a docker container and can be found here: https://www.opendronemap.org/webodm/

You can export an orthophoto .tif format there

convert map.bag into GPX

Obviously back up your map.bag

I wrote a python script to convert the map area coordinates into utm coordinates. I ran this on the robots raspberry pi. In reads the map.bag from .ros folder and exports a .gpx format file.

Following python packages are needed

pip install utm gpxpy

You need to source /opt/ros/noetic/setup.bash

Modify the base point coodinates in the script. I use the roof mouned RTK GPS antenna as base coordinates.

Modify the input and output file paths to your needs and run the python script.

Create a file wit following content: bag_to_gpx_tracks_utm.py

chmod +x bag_to_gpx_tracks_utm.py

#!/usr/bin/python

#===========================================
# Christoph Sobel 11.05.2023
# script to convert openmower ros map.bag to gpx file
#===========================================

from codecs import latin_1_decode
import rosbag
import utm
import gpxpy.gpx
import math
import os
from tf.transformations import euler_from_quaternion

bag = rosbag.Bag('/home/pi/.ros/map.bag')
# bag = rosbag.Bag('output.bag')

lat0 = 53.00
lon0 = 8.00
# lat0 = float(os.getenv('OM_DATUM_LAT'))
# lon0 = float(os.getenv('OM_DATUM_LONG'))
print("base_point", lat0, " ", lon0)

topics = bag.get_type_and_topic_info()[1].keys()
print(topics)

gpx = gpxpy.gpx.GPX()
gpx.name = 'open_mower_map'
gpx.description = 'Map data from openmower map.bag'

# base point (e.g. gps antenna)
# base_point = gpxpy.gpx.GPXWaypoint(latitude=lat0, longitude=lon0, name="base_point")
# gpx.waypoints.append(base_point)

x0, y0, zone, ut = utm.from_latlon(lat0, lon0)
print("base_point in utm", x0, " ", y0)

gpx_track = gpxpy.gpx.GPXTrack(name="base_point")
gpx.tracks.append(gpx_track)
gpx_segment = gpxpy.gpx.GPXTrackSegment()
gpx_track.segments.append(gpx_segment)
gpx_segment.points.append(gpxpy.gpx.GPXTrackPoint(lat0, lon0, elevation=0))


# Loop through areas and obstacles
area_counter = 0

for area_type in ['mowing_areas', 'navigation_areas']:
    for topic, msg, t in bag.read_messages(topics=[area_type]):
        print("{}_{}_area".format(area_type[:-1], area_counter))
        # Create track in GPX:
        gpx_track = gpxpy.gpx.GPXTrack(name="{}_{}_area".format(area_type[:-1], area_counter))
        gpx.tracks.append(gpx_track)
        # Create segment in GPX track:
        gpx_segment = gpxpy.gpx.GPXTrackSegment()
        gpx_track.segments.append(gpx_segment)

        # outline
        for point in msg.area.points:
            # Create points:
            lat, lon = utm.to_latlon(x0 + point.x, y0 + point.y, zone, ut)
            gpx_segment.points.append(gpxpy.gpx.GPXTrackPoint(lat, lon, elevation=0))

        # obstacles
        obstacle_counter = 0
        for obst in msg.obstacles:
            print("{}_{}_obstacle_{}".format(area_type[:-1], area_counter, obstacle_counter))
            # Create track in GPX:
            gpx_track = gpxpy.gpx.GPXTrack(name="{}_{}_obstacle_{}".format(area_type[:-1], area_counter, obstacle_counter))
            gpx.tracks.append(gpx_track)
            # Create segment in GPX track:
            gpx_segment = gpxpy.gpx.GPXTrackSegment()
            gpx_track.segments.append(gpx_segment)
            for point in obst.points:
                # Create points:
                lat, lon = utm.to_latlon(x0 + point.x, y0 + point.y, zone, ut)
                gpx_segment.points.append(gpxpy.gpx.GPXTrackPoint(lat, lon, elevation=0))
            obstacle_counter += 1
        area_counter += 1

# docking point
for topic, msg, t in bag.read_messages(topics=['docking_point']):
    # print(msg)
    print("parsing docking point")
    quat_list = [msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]
    (roll, pitch, yaw) = euler_from_quaternion(quat_list)
    # print((roll, pitch, yaw))
    x2 = msg.position.x + math.cos(yaw)
    y2 = msg.position.y + math.sin(yaw)

    lat1, lon1 = utm.to_latlon(x0 + msg.position.x, y0 + msg.position.y, zone, ut)
    lat2, lon2 = utm.to_latlon(x0 + x2,y0 + y2, zone, ut)

    gpx_track = gpxpy.gpx.GPXTrack(name="docking_point")
    gpx.tracks.append(gpx_track)
    gpx_segment = gpxpy.gpx.GPXTrackSegment()
    gpx_track.segments.append(gpx_segment)
    gpx_segment.points.append(gpxpy.gpx.GPXTrackPoint(lat1, lon1, elevation=0))
    gpx_segment.points.append(gpxpy.gpx.GPXTrackPoint(lat2, lon2, elevation=0))

print('Created GPX file')
with open("map_bag.gpx", "w") as f:
    f.write(gpx.to_xml())

bag.close()
print('Done')

load GPX file into JOSM

Install the Java OpenStreetMap Editor from:

https://josm.openstreetmap.de/wiki/Download

Start JOSM and install the PicLayer Plugin. For this go to Edit->Preferences->Plugins and search for PicLayer and tick the checkbox.

Open your .gpx file, you shoud see the recorded boundaries

Boundaries visible in JOSM from imported GPX file

Right click on the Layer output.gpx and select "convert to data layer". Proceed without simplifying. Now every node should be visible.

overlay and align with an orthophoto

select Imagery->More...->New picture layer from file... and open your orthophoto .tif file
you can now use the PicLayer tools from the toolbar to align you image layer. When all is aligned, right click on the picture layer and save the picture calibration for later use.

mowing area boundaries in JOSM with orthophoto

modify the boundaries

you can now add/remove/modify/merge/divide:

mowing or navigation areas
obstacles
docking point

Using all the nice tools from JOSM.

Following rules for this:

mowing and navigation areas need to be in counter clockwise direction
obstacles need to be in clockwise direction

definition of the feature is in the value of gpx:name tag of each way like this:

mowing_area_0_area
mowing_area_0_obstacle_0
mowing_area_0_obstacle_1
mowing_area_1_area
navigation_area_0_area
navigation_area_1_area
navigation_area_1_obstacle_0
docking_point

area boundary JOSM gps:name tag information

Here you can see an added obstacle.

export as GPX and convert back to openmower map.bag

Right click on the gpx layer and "save as". Select gpx format.

source /opt/ros/noetic/setup.bash

You need to source ./devel/setup.bash in your open_mower_ros folder to get the MapArea custom ros message

Modify the base point coordinates in the script. I use the roof mounted RTK GPS antenna as base coordinates.

Modify the input and output file paths to your needs in the python script.

Run following python script and put the output_map.bag where your ros.bag was.

Create a file wit following content: gpx_to_bag.py

chmod +x gpx_to_bag.py

#!/usr/bin/python

#===========================================
# Christoph Sobel 11.05.2023
# script to convert gpx files to openmower ros map.bag
#===========================================

import gpxpy.gpx
import utm
import rosbag
import math
from geometry_msgs.msg import Polygon, Point32, Pose
from mower_map.msg import MapArea  # Imports the custom message from its package
from tf.transformations import quaternion_from_euler

# Base point
lat0 = 53.00
lon0 = 8.00
# lat0 = float(os.getenv('OM_DATUM_LAT'))
# lon0 = float(os.getenv('OM_DATUM_LONG'))
print("base_point", lat0, " ", lon0)
x0, y0, zone, ut = utm.from_latlon(lat0, lon0)
print("base_point in utm", x0, " ", y0)

gpx_file = open('map_bag_osm.gpx', 'r')
gpx = gpxpy.parse(gpx_file)

mowing_area_dict = {}
navigation_area_dict = {}
base_point = []
docking_points = []

for track in gpx.tracks:
	print("reading from gpx: ", track.name)
	# get points
	point_list = []
	for segment in track.segments:
		print("Point count:{}".format(len(segment.points)))
		for point in segment.points:
			# print(f'Point at ({point.latitude},{point.longitude}) -> {point.elevation}')
			x, y, zone, ut = utm.from_latlon(point.latitude, point.longitude)
			point_list.append(Point32(x=x-x0, y=y-y0, z=0.0))
	# store in deciated dict
	if "mowing" in track.name:
		mowing_area_dict[track.name] = point_list
	elif "navigation" in track.name:
		navigation_area_dict[track.name] = point_list
	elif "base" in track.name:
		base_point = point_list
	elif "docking" in track.name:
		docking_points  = point_list

docking_pose = Pose()
if len(docking_points) == 2:
	yaw = math.atan2(docking_points[1].y - docking_points[0].y, docking_points[1].x - docking_points[0].x)
	quaternion = quaternion_from_euler(0,0,yaw)
	
	docking_pose.position.x = docking_points[0].x
	docking_pose.position.y = docking_points[0].y
	# Pose Orientation
	docking_pose.orientation.x = quaternion[0]
	docking_pose.orientation.y = quaternion[1]
	docking_pose.orientation.z = quaternion[2]
	docking_pose.orientation.w = quaternion[3]
		

def sort_dict_return_MapAreas(area_dict):
	areas = {}
	for track_name, point_list in sorted(area_dict.items()): # store sorted polygons to MapArea
		map_area = MapArea()
		print("converting:", track_name)
		area_number = track_name.split("_")[2]
		poly = Polygon(points=point_list)
		if "area" in track_name[-4:]:
			map_area.area = poly
			areas[area_number] = map_area
		elif "obstacle" in track_name:
			map_area = areas[area_number]
			map_area.obstacles.append(poly)
			areas[area_number] = map_area
	return areas

mowing_areas_list = sort_dict_return_MapAreas(mowing_area_dict).values()
navigation_areas_list = sort_dict_return_MapAreas(navigation_area_dict).values()

with rosbag.Bag('output_map.bag', 'w') as outbag:
	print('writing bag file')
	for mowing_area in mowing_areas_list:
		outbag.write("mowing_areas", mowing_area)
	for navigation_area in navigation_areas_list:	
		outbag.write("navigation_areas", navigation_area)
	outbag.write("docking_point", docking_pose)

Modify map using drone footage

Contents

Introduction

Generate an orthophoto

convert map.bag into GPX

load GPX file into JOSM

overlay and align with an orthophoto

modify the boundaries

export as GPX and convert back to openmower map.bag

Navigation menu

Modify map using drone footage

Introduction

Generate an orthophoto

convert map.bag into GPX

load GPX file into JOSM

overlay and align with an orthophoto

modify the boundaries

export as GPX and convert back to openmower map.bag

Navigation menu

Search