From 13c0f58c5f68d1586d9951e5f45e4a95392c6eb2 Mon Sep 17 00:00:00 2001 From: Kyle Coble Date: Mon, 13 Nov 2023 18:12:13 -0500 Subject: [PATCH 1/3] Add the track_plotter example --- py/examples/track_follower/README.md | 2 +- py/examples/track_follower/main.py | 1 + py/examples/track_plotter/README.md | 3 + py/examples/track_plotter/main.py | 97 ++++++++++++++++++++++ py/examples/track_plotter/requirements.txt | 2 + 5 files changed, 104 insertions(+), 1 deletion(-) create mode 100644 py/examples/track_plotter/README.md create mode 100644 py/examples/track_plotter/main.py create mode 100644 py/examples/track_plotter/requirements.txt diff --git a/py/examples/track_follower/README.md b/py/examples/track_follower/README.md index 53fd9e01..e31cabd4 100644 --- a/py/examples/track_follower/README.md +++ b/py/examples/track_follower/README.md @@ -1,3 +1,3 @@ -# Amiga Brain Follow Track Follower Track example +# Amiga Brain Follow Track example URL: https://amiga.farm-ng.com/docs/examples/track_follower/ diff --git a/py/examples/track_follower/main.py b/py/examples/track_follower/main.py index f5cee42e..82794d2c 100644 --- a/py/examples/track_follower/main.py +++ b/py/examples/track_follower/main.py @@ -53,6 +53,7 @@ async def main(service_config_path: Path, track_path: Path) -> None: Args: service_config_path (Path): The path to the track_follower service config. + track_path: (Path) The filepath of the track to follow. """ # Extract the track_follower service config from the JSON file diff --git a/py/examples/track_plotter/README.md b/py/examples/track_plotter/README.md new file mode 100644 index 00000000..3c8bb5cc --- /dev/null +++ b/py/examples/track_plotter/README.md @@ -0,0 +1,3 @@ +# Amiga Brain Track Plot example + +URL: https://amiga.farm-ng.com/docs/examples/track_plotter/ diff --git a/py/examples/track_plotter/main.py b/py/examples/track_plotter/main.py new file mode 100644 index 00000000..b287cfd3 --- /dev/null +++ b/py/examples/track_plotter/main.py @@ -0,0 +1,97 @@ +"""Example plotting a pre-recorded track.""" +# Copyright (c) farm-ng, inc. +# +# Licensed under the Amiga Development Kit License (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://github.com/farm-ng/amiga-dev-kit/blob/main/LICENSE +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +from __future__ import annotations + +import argparse +from pathlib import Path + +import matplotlib.pyplot as plt +import numpy as np +from farm_ng.core.events_file_reader import proto_from_json_file +from farm_ng.core.pose_pb2 import Pose +from farm_ng.track.track_pb2 import Track +from farm_ng_core_pybind import Pose3F64 + + +def plot_track(track: Track) -> None: + """Plot a track from a Track proto message. + + Args: + track: (Track) The Track proto message to plot. + """ + x_values: list[float] = [] + y_values: list[float] = [] + headings: list[float] = [] + + waypoint: Pose + for waypoint in track.waypoints: + goal: Pose3F64 = Pose3F64.from_proto(waypoint) + + x_values.append(goal.translation[0]) + y_values.append(goal.translation[1]) + headings.append(goal.rotation.log()[-1]) + + # Plotting + plt.figure(figsize=(8, 8)) + + # Normalize the color scale to the range [0, 1] + norm = plt.Normalize(0, len(x_values) - 1) + colors = np.arange(0, len(x_values)) + + # Add heading arrows with color scale + for x, y, heading, color in zip(x_values, y_values, headings, colors): + dx = np.cos(heading) * 0.05 + dy = np.sin(heading) * 0.05 + plt.arrow(x, y, dx, dy, head_width=0.035, fc=plt.cm.plasma(norm(color)), ec=plt.cm.plasma(norm(color))) + + plt.title('Track waypoints') + plt.xlabel('X') + plt.ylabel('Y') + + # Add colorbar below the plot with a smaller height + cbar = plt.colorbar(plt.cm.ScalarMappable(cmap=plt.cm.plasma, norm=norm), orientation='horizontal') + cbar.set_label('Waypoint idx along the track') + + plt.grid(True) + plt.show() + + +def main(track_path: Path) -> None: + """Plot a track from a json file containing a Track proto message. + + Args: + track_path: (Path) The filepath of the track to plot. + """ + # Read the track and package in a Track proto message + track: Track = proto_from_json_file(track_path, Track()) + + # NOTE: If you have a deprecated FilterTrack proto message instead of a Track proto message, + # you can convert it to a Track proto message using the following code instead: + # from farm_ng.filter.filter_pb2 import FilterTrack + # from farm_ng.track.utils import filter_track_to_track + # filter_track: FilterTrack = proto_from_json_file(track_path, FilterTrack()) + # track: Track = filter_track_to_track(filter_track) + + plot_track(track) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(prog="amiga-track-plotter") + parser.add_argument("--track", type=Path, required=True, help="The filepath of the track to plot.") + args = parser.parse_args() + + track_path = Path(args.track).resolve() + + main(track_path) diff --git a/py/examples/track_plotter/requirements.txt b/py/examples/track_plotter/requirements.txt new file mode 100644 index 00000000..c50ebbcd --- /dev/null +++ b/py/examples/track_plotter/requirements.txt @@ -0,0 +1,2 @@ +farm-ng-amiga +matplotlib From 503c2f2bbb3ea2ba1ade6017d69849cd48869f5e Mon Sep 17 00:00:00 2001 From: Kyle Coble Date: Mon, 13 Nov 2023 18:43:00 -0500 Subject: [PATCH 2/3] Resolve matplotlib deprecation warning --- py/examples/track_plotter/main.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/py/examples/track_plotter/main.py b/py/examples/track_plotter/main.py index b287cfd3..7eb70231 100644 --- a/py/examples/track_plotter/main.py +++ b/py/examples/track_plotter/main.py @@ -57,12 +57,18 @@ def plot_track(track: Track) -> None: plt.arrow(x, y, dx, dy, head_width=0.035, fc=plt.cm.plasma(norm(color)), ec=plt.cm.plasma(norm(color))) plt.title('Track waypoints') - plt.xlabel('X') - plt.ylabel('Y') + plt.xlabel('X [m]') + plt.ylabel('Y [m]') # Add colorbar below the plot with a smaller height - cbar = plt.colorbar(plt.cm.ScalarMappable(cmap=plt.cm.plasma, norm=norm), orientation='horizontal') - cbar.set_label('Waypoint idx along the track') + cbar = plt.colorbar( + plt.cm.ScalarMappable(cmap=plt.cm.plasma, norm=norm), + orientation='horizontal', + ax=plt.gca(), + fraction=0.046, + pad=0.1, + ) + cbar.set_label('Waypoint idx along the Track') plt.grid(True) plt.show() From ab057d9e37a4c02edf00b84a1cf89dcd79906523 Mon Sep 17 00:00:00 2001 From: Kyle Coble Date: Mon, 13 Nov 2023 18:55:48 -0500 Subject: [PATCH 3/3] Split unpack_track into separate function --- py/examples/track_plotter/main.py | 28 ++++++++++++++++++++++++---- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/py/examples/track_plotter/main.py b/py/examples/track_plotter/main.py index 7eb70231..9eaa527a 100644 --- a/py/examples/track_plotter/main.py +++ b/py/examples/track_plotter/main.py @@ -25,11 +25,15 @@ from farm_ng_core_pybind import Pose3F64 -def plot_track(track: Track) -> None: - """Plot a track from a Track proto message. +def unpack_track(track: Track) -> tuple[list[float], list[float], list[float]]: + """Unpack a track from a Track proto message into lists of x, y, and heading values. Args: - track: (Track) The Track proto message to plot. + track: (Track) The Track proto message to unpack. + Returns: + x_values: (list[float]) The x values of the track. + y_values: (list[float]) The y values of the track. + headings: (list[float]) The heading values of the track. """ x_values: list[float] = [] y_values: list[float] = [] @@ -43,6 +47,18 @@ def plot_track(track: Track) -> None: y_values.append(goal.translation[1]) headings.append(goal.rotation.log()[-1]) + return x_values, y_values, headings + + +def plot_track(x_values: list[float], y_values: list[float], headings: list[float]) -> None: + """Plot a track from a Track proto message. + + Args: + x_values: (list[float]) The x coordinates of the track. + y_values: (list[float]) The y coordinates of the track. + headings: (list[float]) The heading values of the track. + """ + # Plotting plt.figure(figsize=(8, 8)) @@ -90,7 +106,11 @@ def main(track_path: Path) -> None: # filter_track: FilterTrack = proto_from_json_file(track_path, FilterTrack()) # track: Track = filter_track_to_track(filter_track) - plot_track(track) + # Unpack the track into lists of x, y, and heading values + x_values, y_values, headings = unpack_track(track) + + # Plot the track + plot_track(x_values, y_values, headings) if __name__ == "__main__":