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