From d380f51925609378a2f47b2c3e2b7ddb661433a2 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 2 Jul 2025 12:33:20 +0000 Subject: [PATCH 01/23] Updated README --- README.md | 4 + src/nav2_tutorial/config/nav2_plan_viz.rviz | 459 ++++++++++++++++++++ 2 files changed, 463 insertions(+) create mode 100644 src/nav2_tutorial/config/nav2_plan_viz.rviz diff --git a/README.md b/README.md index 7ad4352..362bb55 100644 --- a/README.md +++ b/README.md @@ -258,3 +258,7 @@ For launching the graphical interface, you can run the following command (note t ros2 launch nav2_tutorial mapviz.launch.py ``` Or alternatively use the `--mapviz` flag on the `gps_waypoint_follower.launch.py` script. + + +# Debugging trajectories +To analyze the trajectories performed by the robot and determine if any issues have occured, the user can perform a recording of all topics by running `ros2 bag record --all`. Then, launch RViz2 with the provided configuration file to easily visualize the debug topics containing the waypoint coordinates, the global and local plans, the costmaps, among other things: `ros2 run rviz2 rviz2 -d /home/dev/ros_ws/src/nav2_tutorial/config/nav2_plan_viz.rviz`. diff --git a/src/nav2_tutorial/config/nav2_plan_viz.rviz b/src/nav2_tutorial/config/nav2_plan_viz.rviz new file mode 100644 index 0000000..26a40ac --- /dev/null +++ b/src/nav2_tutorial/config/nav2_plan_viz.rviz @@ -0,0 +1,459 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Path1 + - /Path2 + - /Path3 + - /Odometry1 + - /Odometry1/Shape1 + - /MarkerArray1 + - /MarkerArray2 + - /Path5 + Splitter Ratio: 0.5 + Tree Height: 555 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /gps_waypoints_path + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 10 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.10000000149011612 + Head Radius: 0.05000000074505806 + Shaft Length: 0.10000000149011612 + Shaft Radius: 0.029999999329447746 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fixposition/odometry_enu + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + seg_line: true + wps: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /gps_waypoints_markers + Value: true + - Alpha: 0.699999988079071 + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + Value: true + - Alpha: 0.699999988079071 + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 204; 41; 204 + Enabled: false + History Length: 1 + Name: PointStamped + Radius: 0.20000000298023224 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lookahead_point + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 170; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /received_global_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + full_traj: true + full_traj_pts: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /gps_full_traj_markers + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 170; 85; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /gps_full_traj + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.572829246520996 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.4763948619365692 + Y: -0.11822476238012314 + Z: 0.7200839519500732 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.3047964572906494 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.8253976702690125 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1308 + Hide Left Dock: false + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001e500000482fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000002f5000001c8000001c700ffffff000000010000010f00000482fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000482000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006b80000003efc0100000002fb0000000800540069006d00650100000000000006b80000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004cd0000048200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1720 + X: 0 + Y: 27 From 7ec60b56d8ec90a2223d520ac1847cf1bb6c55f7 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 2 Jul 2025 14:58:49 +0200 Subject: [PATCH 02/23] First draft dashboard --- src/nav2_tutorial/src/dashboard/dashboard.py | 183 ++++++++++++++++++ .../src/loggers/gps_periodic_logger.py | 14 +- 2 files changed, 195 insertions(+), 2 deletions(-) create mode 100644 src/nav2_tutorial/src/dashboard/dashboard.py diff --git a/src/nav2_tutorial/src/dashboard/dashboard.py b/src/nav2_tutorial/src/dashboard/dashboard.py new file mode 100644 index 0000000..820c774 --- /dev/null +++ b/src/nav2_tutorial/src/dashboard/dashboard.py @@ -0,0 +1,183 @@ +import dash +import dash_bootstrap_components as dbc +from dash import html, dcc, Output, Input, State +import os +import subprocess +import glob +import threading +import queue +import time + +# Globals +log_queue = queue.Queue() +log_buffer = [] +LOG_BUFFER_MAX_LINES = 100 +TRAJECTORY_DIR = '/home/dev/ros_ws/src/nav2_tutorial/trajectories' +RECORD_SCRIPT = '/home/dev/ros_ws/src/nav2_tutorial/src/loggers/gps_periodic_logger.py' +PRECISE_SCRIPT = '/home/dev/ros_ws/src/nav2_tutorial/src/precise_wp_follower.py' +SMOOTH_SCRIPT = '/home/dev/ros_ws/src/nav2_tutorial/src/smooth_wp_follower.py' + +def get_trajectories(): + return sorted(glob.glob(os.path.join(TRAJECTORY_DIR, '*.yaml'))) + +# --- Logger process management --- +logger_process = None +log_queue = queue.Queue() +logger_thread = None +logger_running = threading.Event() + +def run_logger_process(): + global logger_process + logger_running.set() + logger_process = subprocess.Popen( + ['python3', RECORD_SCRIPT], + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + bufsize=1 + ) + try: + for line in logger_process.stdout: + log_queue.put(line) + except Exception as e: + log_queue.put(f"[Logger error]: {e}\n") + logger_running.clear() + +def start_logger(): + global logger_thread, logger_running + if logger_running.is_set(): + return # Already running + logger_thread = threading.Thread(target=run_logger_process, daemon=True) + logger_thread.start() + +def stop_logger(): + global logger_process, logger_running + if logger_process and logger_running.is_set(): + logger_process.terminate() + logger_running.clear() + log_queue.put("[Logger stopped]\n") + +def get_live_log(): + while not log_queue.empty(): + line = log_queue.get() + log_buffer.append(line) + if len(log_buffer) > LOG_BUFFER_MAX_LINES: + log_buffer.pop(0) + return ''.join(log_buffer) + +# --- Dash app setup --- +app = dash.Dash(__name__, external_stylesheets=[dbc.themes.BOOTSTRAP]) + +app.layout = dbc.Container([ + html.H2("Trajectory Dashboard"), + dbc.Row([ + dbc.Col([ + html.H4("Recorded Trajectories"), + dcc.Dropdown( + id="trajectory-dropdown", + options=[{"label": os.path.basename(f), "value": f} for f in get_trajectories()], + placeholder="Select a trajectory", + style={"margin-bottom": "10px"} + ), + dbc.Button("Use Last", id="use-last-btn", color="secondary", style={"width": "100%", "margin-bottom": "20px"}), + dbc.Button("Record Trajectory", id="record-btn", color="success", style={"width": "100%", "margin-bottom": "10px"}), + dbc.Button("Stop Recording", id="stop-record-btn", color="danger", style={"width": "100%", "margin-bottom": "10px"}), + ], width=3), + dbc.Col([ + html.H4("Controls"), + html.Div([ + html.Label("Mode:"), + dcc.Slider( + id="mode-slider", + min=0, max=1, step=1, + marks={0: "Precise", 1: "Smooth"}, + value=0, + ) + ], style={"margin-bottom": "20px"}), + dbc.Button("Play Trajectory", id="play-btn", color="primary", style={"width": "100%"}), + html.Div(id="status", style={"margin-top": "20px"}), + html.Hr(), + html.H5("Live Logger Output"), + html.Pre(id='live-log', style={'height': '300px', 'overflow': 'auto', 'background': '#1a1a1a', 'color': '#22ee22'}), + dcc.Interval(id='log-interval', interval=1000, n_intervals=0), # Refresh logs every second + ], width=9), + ]) +], fluid=True) + +# Refresh dropdown when new trajectories are created +@app.callback( + Output("trajectory-dropdown", "options"), + Input("record-btn", "n_clicks"), + Input("play-btn", "n_clicks"), + Input("use-last-btn", "n_clicks"), + prevent_initial_call=True +) +def refresh_trajectories(*_): + return [{"label": os.path.basename(f), "value": f} for f in get_trajectories()] + +# Start recording +@app.callback( + Output("status", "children", allow_duplicate=True), + Input("record-btn", "n_clicks"), + prevent_initial_call=True +) +def start_recording(n): + if logger_running.is_set(): + return dbc.Alert("Logger already running!", color="warning") + start_logger() + return dbc.Alert("Recording started!", color="success") + +# Stop recording +@app.callback( + Output("status", "children", allow_duplicate=True), + Input("stop-record-btn", "n_clicks"), + prevent_initial_call=True +) +def stop_recording(n): + if not logger_running.is_set(): + return dbc.Alert("Logger is not running!", color="warning") + stop_logger() + return dbc.Alert("Recording stopped.", color="info") + +# Play trajectory +@app.callback( + Output("status", "children", allow_duplicate=True), + Input("play-btn", "n_clicks"), + State("trajectory-dropdown", "value"), + State("mode-slider", "value"), + prevent_initial_call=True +) +def play_trajectory(n, yaml_file, mode): + if not yaml_file: + return dbc.Alert("Please select a trajectory!", color="warning") + script = PRECISE_SCRIPT if mode == 0 else SMOOTH_SCRIPT + try: + subprocess.Popen(['python3', script, '--file', yaml_file]) + return dbc.Alert(f"Playing {os.path.basename(yaml_file)} in {'Precise' if mode == 0 else 'Smooth'} mode", color="info") + except Exception as e: + return dbc.Alert(f"Error: {e}", color="danger") + +# Use last trajectory +@app.callback( + Output("trajectory-dropdown", "value"), + Output("status", "children", allow_duplicate=True), + Input("use-last-btn", "n_clicks"), + prevent_initial_call=True +) +def use_last(n): + files = get_trajectories() + if not files: + return dash.no_update, dbc.Alert("No trajectories found!", color="warning") + last_file = files[-1] + return last_file, dbc.Alert(f"Selected last trajectory: {os.path.basename(last_file)}", color="info") + +# Live logger output +@app.callback( + Output('live-log', 'children'), + Input('log-interval', 'n_intervals') +) +def update_log(_): + return get_live_log() + +if __name__ == "__main__": + app.run(debug=True, host="0.0.0.0", port=8051) diff --git a/src/nav2_tutorial/src/loggers/gps_periodic_logger.py b/src/nav2_tutorial/src/loggers/gps_periodic_logger.py index 479b5ea..0bda143 100644 --- a/src/nav2_tutorial/src/loggers/gps_periodic_logger.py +++ b/src/nav2_tutorial/src/loggers/gps_periodic_logger.py @@ -7,6 +7,7 @@ import time import yaml import select +import signal import termios import argparse from datetime import datetime @@ -20,6 +21,8 @@ from src.utils.gps_utils import euler_from_quaternion +# Global variables +stop_requested = False class GpsPeriodicLogger(Node): def __init__(self, logging_file_path: str, interval: float): @@ -140,6 +143,10 @@ def _parse_arguments(argv: list[str] | None = None) -> tuple[str, float]: def _stdin_ready() -> bool: return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) +def handle_sigterm(signum, frame): + global stop_requested + stop_requested = True + def main(argv: list[str] | None = None) -> None: yaml_path, interval = _parse_arguments(argv) @@ -150,8 +157,12 @@ def main(argv: list[str] | None = None) -> None: old_settings = termios.tcgetattr(fd) tty.setcbreak(fd) + global stop_requested + stop_requested = False + signal.signal(signal.SIGTERM, handle_sigterm) + try: - while rclpy.ok(): + while rclpy.ok() and not stop_requested: rclpy.spin_once(node, timeout_sec=0.1) if _stdin_ready(): ch = sys.stdin.read(1) @@ -167,6 +178,5 @@ def main(argv: list[str] | None = None) -> None: termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) rclpy.shutdown() - if __name__ == '__main__': main() From ea5e0d07cc7499d35b9d504c72c3cd5982b15dd0 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 2 Jul 2025 15:04:06 +0200 Subject: [PATCH 03/23] Minor improvements --- src/nav2_tutorial/src/dashboard/dashboard.py | 174 ++++++++++++------- 1 file changed, 115 insertions(+), 59 deletions(-) diff --git a/src/nav2_tutorial/src/dashboard/dashboard.py b/src/nav2_tutorial/src/dashboard/dashboard.py index 820c774..298df25 100644 --- a/src/nav2_tutorial/src/dashboard/dashboard.py +++ b/src/nav2_tutorial/src/dashboard/dashboard.py @@ -1,21 +1,18 @@ import dash import dash_bootstrap_components as dbc -from dash import html, dcc, Output, Input, State +from dash import html, dcc, Output, Input, State, ctx import os import subprocess import glob import threading import queue -import time -# Globals -log_queue = queue.Queue() -log_buffer = [] -LOG_BUFFER_MAX_LINES = 100 +# ----- Constants ----- TRAJECTORY_DIR = '/home/dev/ros_ws/src/nav2_tutorial/trajectories' RECORD_SCRIPT = '/home/dev/ros_ws/src/nav2_tutorial/src/loggers/gps_periodic_logger.py' PRECISE_SCRIPT = '/home/dev/ros_ws/src/nav2_tutorial/src/precise_wp_follower.py' SMOOTH_SCRIPT = '/home/dev/ros_ws/src/nav2_tutorial/src/smooth_wp_follower.py' +INTERACTIVE_SCRIPT = '/home/jetson/dev/nav2_tutorial/src/nav2_tutorial/src/interactive_wp_follower.py' def get_trajectories(): return sorted(glob.glob(os.path.join(TRAJECTORY_DIR, '*.yaml'))) @@ -23,6 +20,8 @@ def get_trajectories(): # --- Logger process management --- logger_process = None log_queue = queue.Queue() +log_buffer = [] +LOG_BUFFER_MAX_LINES = 100 logger_thread = None logger_running = threading.Event() @@ -65,55 +64,144 @@ def get_live_log(): log_buffer.pop(0) return ''.join(log_buffer) -# --- Dash app setup --- +# ----- App Layout ----- app = dash.Dash(__name__, external_stylesheets=[dbc.themes.BOOTSTRAP]) app.layout = dbc.Container([ html.H2("Trajectory Dashboard"), dbc.Row([ dbc.Col([ - html.H4("Recorded Trajectories"), + html.H4("Trajectories"), dcc.Dropdown( id="trajectory-dropdown", options=[{"label": os.path.basename(f), "value": f} for f in get_trajectories()], placeholder="Select a trajectory", style={"margin-bottom": "10px"} ), - dbc.Button("Use Last", id="use-last-btn", color="secondary", style={"width": "100%", "margin-bottom": "20px"}), + dbc.Button("Use Last", id="use-last-btn", color="secondary", style={"width": "100%", "margin-bottom": "10px"}), + dbc.Badge("Using last", id="using-last-indicator", color="info", pill=True, style={"display": "none", "margin-bottom": "10px"}), + dbc.Button("Reverse", id="reverse-btn", color="warning", style={"width": "100%", "margin-bottom": "10px"}), + dbc.Badge("Reverse", id="reverse-indicator", color="warning", pill=True, style={"display": "none", "margin-bottom": "10px"}), dbc.Button("Record Trajectory", id="record-btn", color="success", style={"width": "100%", "margin-bottom": "10px"}), dbc.Button("Stop Recording", id="stop-record-btn", color="danger", style={"width": "100%", "margin-bottom": "10px"}), ], width=3), dbc.Col([ html.H4("Controls"), - html.Div([ - html.Label("Mode:"), - dcc.Slider( - id="mode-slider", - min=0, max=1, step=1, - marks={0: "Precise", 1: "Smooth"}, - value=0, - ) - ], style={"margin-bottom": "20px"}), - dbc.Button("Play Trajectory", id="play-btn", color="primary", style={"width": "100%"}), + dbc.RadioItems( + id="mode-radio", + options=[ + {"label": "Precise", "value": "precise"}, + {"label": "Smooth", "value": "smooth"}, + {"label": "Interactive", "value": "interactive"}, + ], + value="precise", + inline=True, + inputStyle={"margin-right": "8px", "margin-left": "16px"} + ), + dbc.Button("Play Trajectory", id="play-btn", color="primary", style={"width": "100%", "margin-top": "10px"}), html.Div(id="status", style={"margin-top": "20px"}), html.Hr(), html.H5("Live Logger Output"), html.Pre(id='live-log', style={'height': '300px', 'overflow': 'auto', 'background': '#1a1a1a', 'color': '#22ee22'}), - dcc.Interval(id='log-interval', interval=1000, n_intervals=0), # Refresh logs every second + dcc.Interval(id='log-interval', interval=1000, n_intervals=0), ], width=9), - ]) -], fluid=True) + ]), + # Hidden stores for state + dcc.Store(id="using-last-store", data=False), + dcc.Store(id="reverse-store", data=False), +]) -# Refresh dropdown when new trajectories are created +# ----- CALLBACKS ----- + +# "Use Last" and dropdown management (fixes all Output collision issues) @app.callback( + Output("using-last-store", "data"), + Output("using-last-indicator", "style"), + Output("trajectory-dropdown", "value"), Output("trajectory-dropdown", "options"), - Input("record-btn", "n_clicks"), - Input("play-btn", "n_clicks"), + Output("trajectory-dropdown", "placeholder"), Input("use-last-btn", "n_clicks"), + Input("trajectory-dropdown", "value"), + State("trajectory-dropdown", "options"), + State("using-last-store", "data"), + prevent_initial_call=True, +) +def manage_use_last_and_dropdown(use_last_clicks, dropdown_value, options, is_using_last): + triggered = ctx.triggered_id + + if triggered == "use-last-btn": + using_last = True + style = {"display": "inline-block", "margin-bottom": "10px"} + options = [{"label": os.path.basename(f), "value": f} for f in get_trajectories()] + return using_last, style, "USING_LAST", options, "Using last recording" + + if triggered == "trajectory-dropdown": + if dropdown_value != "USING_LAST": + return False, {"display": "none"}, dropdown_value, options, "Select a trajectory" + else: + return True, {"display": "inline-block", "margin-bottom": "10px"}, "USING_LAST", options, "Using last recording" + + # Fallback (should not happen) + return is_using_last, {"display": "inline-block" if is_using_last else "none", "margin-bottom": "10px"}, dropdown_value, options, "Using last recording" if is_using_last else "Select a trajectory" + +# Reverse button toggle & indicator +@app.callback( + Output("reverse-store", "data"), + Output("reverse-indicator", "style"), + Input("reverse-btn", "n_clicks"), + State("reverse-store", "data"), + prevent_initial_call=True +) +def toggle_reverse(n, is_reverse): + is_reverse = not is_reverse + style = {"display": "inline-block", "margin-bottom": "10px"} if is_reverse else {"display": "none"} + return is_reverse, style + +# Play trajectory logic +@app.callback( + Output("status", "children", allow_duplicate=True), + Input("play-btn", "n_clicks"), + State("mode-radio", "value"), + State("trajectory-dropdown", "value"), + State("using-last-store", "data"), + State("reverse-store", "data"), prevent_initial_call=True ) -def refresh_trajectories(*_): - return [{"label": os.path.basename(f), "value": f} for f in get_trajectories()] +def play_trajectory(n, mode, dropdown_value, using_last, reverse): + if using_last or dropdown_value == "USING_LAST": + arg = "--last" + traj_display = "last recording" + elif dropdown_value: + arg = dropdown_value + traj_display = os.path.basename(dropdown_value) + else: + return dbc.Alert("Please select a trajectory!", color="warning") + + # Pick script path + if mode == "precise": + script = PRECISE_SCRIPT + elif mode == "smooth": + script = SMOOTH_SCRIPT + else: + script = INTERACTIVE_SCRIPT + + cmd = ["python3", script] + if arg == "--last": + cmd.append("--last") + else: + cmd.append(arg) + if reverse: + cmd.append("--reverse") + + try: + subprocess.Popen(cmd) + mode_label = {"precise": "Precise", "smooth": "Smooth", "interactive": "Interactive"}[mode] + details = f"Playing {traj_display} in {mode_label} mode" + if reverse: + details += " (Reverse)" + return dbc.Alert(details, color="info") + except Exception as e: + return dbc.Alert(f"Error: {e}", color="danger") # Start recording @app.callback( @@ -139,38 +227,6 @@ def stop_recording(n): stop_logger() return dbc.Alert("Recording stopped.", color="info") -# Play trajectory -@app.callback( - Output("status", "children", allow_duplicate=True), - Input("play-btn", "n_clicks"), - State("trajectory-dropdown", "value"), - State("mode-slider", "value"), - prevent_initial_call=True -) -def play_trajectory(n, yaml_file, mode): - if not yaml_file: - return dbc.Alert("Please select a trajectory!", color="warning") - script = PRECISE_SCRIPT if mode == 0 else SMOOTH_SCRIPT - try: - subprocess.Popen(['python3', script, '--file', yaml_file]) - return dbc.Alert(f"Playing {os.path.basename(yaml_file)} in {'Precise' if mode == 0 else 'Smooth'} mode", color="info") - except Exception as e: - return dbc.Alert(f"Error: {e}", color="danger") - -# Use last trajectory -@app.callback( - Output("trajectory-dropdown", "value"), - Output("status", "children", allow_duplicate=True), - Input("use-last-btn", "n_clicks"), - prevent_initial_call=True -) -def use_last(n): - files = get_trajectories() - if not files: - return dash.no_update, dbc.Alert("No trajectories found!", color="warning") - last_file = files[-1] - return last_file, dbc.Alert(f"Selected last trajectory: {os.path.basename(last_file)}", color="info") - # Live logger output @app.callback( Output('live-log', 'children'), From c497c02142c7e868757ba5480ff13d0d143eec19 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 2 Jul 2025 15:13:12 +0200 Subject: [PATCH 04/23] Improved logger --- src/nav2_tutorial/src/dashboard/dashboard.py | 68 +++++++++++++++++++- 1 file changed, 65 insertions(+), 3 deletions(-) diff --git a/src/nav2_tutorial/src/dashboard/dashboard.py b/src/nav2_tutorial/src/dashboard/dashboard.py index 298df25..fbac6b5 100644 --- a/src/nav2_tutorial/src/dashboard/dashboard.py +++ b/src/nav2_tutorial/src/dashboard/dashboard.py @@ -64,6 +64,54 @@ def get_live_log(): log_buffer.pop(0) return ''.join(log_buffer) +# --- Follower process management --- +follower_process = None +follower_thread = None +follower_log_queue = queue.Queue() +follower_log_buffer = [] +FOLLOWER_LOG_BUFFER_MAX_LINES = 100 +follower_running = threading.Event() + +def run_follower_process(cmd): + global follower_process + follower_running.set() + follower_process = subprocess.Popen( + cmd, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + bufsize=1 + ) + try: + for line in follower_process.stdout: + follower_log_queue.put(line) + except Exception as e: + follower_log_queue.put(f"[Follower error]: {e}\n") + follower_running.clear() + +def start_follower(cmd): + global follower_thread, follower_running + if follower_running.is_set(): + follower_log_queue.put("[Follower already running]\n") + return + follower_thread = threading.Thread(target=run_follower_process, args=(cmd,), daemon=True) + follower_thread.start() + +def stop_follower(): + global follower_process, follower_running + if follower_process and follower_running.is_set(): + follower_process.terminate() + follower_running.clear() + follower_log_queue.put("[Follower stopped]\n") + +def get_follower_live_log(): + while not follower_log_queue.empty(): + line = follower_log_queue.get() + follower_log_buffer.append(line) + if len(follower_log_buffer) > FOLLOWER_LOG_BUFFER_MAX_LINES: + follower_log_buffer.pop(0) + return ''.join(follower_log_buffer) + # ----- App Layout ----- app = dash.Dash(__name__, external_stylesheets=[dbc.themes.BOOTSTRAP]) @@ -99,11 +147,17 @@ def get_live_log(): inputStyle={"margin-right": "8px", "margin-left": "16px"} ), dbc.Button("Play Trajectory", id="play-btn", color="primary", style={"width": "100%", "margin-top": "10px"}), + # Optional: add a Stop Follower button if you want to manually interrupt followers + # dbc.Button("Stop Follower", id="stop-follower-btn", color="danger", style={"width": "100%", "margin-top": "10px"}), html.Div(id="status", style={"margin-top": "20px"}), html.Hr(), html.H5("Live Logger Output"), - html.Pre(id='live-log', style={'height': '300px', 'overflow': 'auto', 'background': '#1a1a1a', 'color': '#22ee22'}), + html.Pre(id='live-log', style={'height': '200px', 'overflow': 'auto', 'background': '#1a1a1a', 'color': '#22ee22'}), dcc.Interval(id='log-interval', interval=1000, n_intervals=0), + html.Hr(), + html.H5("Live Follower Output"), + html.Pre(id='live-follower-log', style={'height': '200px', 'overflow': 'auto', 'background': '#222222', 'color': '#ffd700'}), + dcc.Interval(id='follower-log-interval', interval=1000, n_intervals=0), ], width=9), ]), # Hidden stores for state @@ -157,7 +211,7 @@ def toggle_reverse(n, is_reverse): style = {"display": "inline-block", "margin-bottom": "10px"} if is_reverse else {"display": "none"} return is_reverse, style -# Play trajectory logic +# Play trajectory logic (launches the follower and streams output) @app.callback( Output("status", "children", allow_duplicate=True), Input("play-btn", "n_clicks"), @@ -194,7 +248,7 @@ def play_trajectory(n, mode, dropdown_value, using_last, reverse): cmd.append("--reverse") try: - subprocess.Popen(cmd) + start_follower(cmd) mode_label = {"precise": "Precise", "smooth": "Smooth", "interactive": "Interactive"}[mode] details = f"Playing {traj_display} in {mode_label} mode" if reverse: @@ -235,5 +289,13 @@ def stop_recording(n): def update_log(_): return get_live_log() +# Live follower output +@app.callback( + Output('live-follower-log', 'children'), + Input('follower-log-interval', 'n_intervals') +) +def update_follower_log(_): + return get_follower_live_log() + if __name__ == "__main__": app.run(debug=True, host="0.0.0.0", port=8051) From 20700d11be8b33e83dbeb93c995b5fdd251ab128 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 3 Jul 2025 08:06:50 +0000 Subject: [PATCH 05/23] Added map visualization --- src/nav2_tutorial/src/dashboard/dashboard.py | 298 +++++++++++-------- 1 file changed, 167 insertions(+), 131 deletions(-) diff --git a/src/nav2_tutorial/src/dashboard/dashboard.py b/src/nav2_tutorial/src/dashboard/dashboard.py index fbac6b5..eb45384 100644 --- a/src/nav2_tutorial/src/dashboard/dashboard.py +++ b/src/nav2_tutorial/src/dashboard/dashboard.py @@ -6,6 +6,8 @@ import glob import threading import queue +import yaml +import math # ----- Constants ----- TRAJECTORY_DIR = '/home/dev/ros_ws/src/nav2_tutorial/trajectories' @@ -17,65 +19,32 @@ def get_trajectories(): return sorted(glob.glob(os.path.join(TRAJECTORY_DIR, '*.yaml'))) -# --- Logger process management --- -logger_process = None -log_queue = queue.Queue() -log_buffer = [] -LOG_BUFFER_MAX_LINES = 100 -logger_thread = None -logger_running = threading.Event() - -def run_logger_process(): - global logger_process - logger_running.set() - logger_process = subprocess.Popen( - ['python3', RECORD_SCRIPT], - stdout=subprocess.PIPE, - stderr=subprocess.STDOUT, - text=True, - bufsize=1 - ) +def load_trajectory_from_yaml(yaml_path): try: - for line in logger_process.stdout: - log_queue.put(line) - except Exception as e: - log_queue.put(f"[Logger error]: {e}\n") - logger_running.clear() - -def start_logger(): - global logger_thread, logger_running - if logger_running.is_set(): - return # Already running - logger_thread = threading.Thread(target=run_logger_process, daemon=True) - logger_thread.start() + with open(yaml_path, "r") as f: + data = yaml.safe_load(f) + waypoints = data.get("waypoints", []) + lats = [wp["latitude"] for wp in waypoints] + lons = [wp["longitude"] for wp in waypoints] + yaws = [wp["yaw"] if "yaw" in wp else 0 for wp in waypoints] + return lats, lons, yaws + except Exception: + return [], [], [] -def stop_logger(): - global logger_process, logger_running - if logger_process and logger_running.is_set(): - logger_process.terminate() - logger_running.clear() - log_queue.put("[Logger stopped]\n") +# --- Process management (combined) --- +process = None +process_thread = None +process_log_queue = queue.Queue() +process_log_buffer = [] +PROCESS_LOG_BUFFER_MAX_LINES = 100 +process_running = threading.Event() +last_process_type = "logger" # or "follower" -def get_live_log(): - while not log_queue.empty(): - line = log_queue.get() - log_buffer.append(line) - if len(log_buffer) > LOG_BUFFER_MAX_LINES: - log_buffer.pop(0) - return ''.join(log_buffer) - -# --- Follower process management --- -follower_process = None -follower_thread = None -follower_log_queue = queue.Queue() -follower_log_buffer = [] -FOLLOWER_LOG_BUFFER_MAX_LINES = 100 -follower_running = threading.Event() - -def run_follower_process(cmd): - global follower_process - follower_running.set() - follower_process = subprocess.Popen( +def run_process(cmd, process_type): + global process, last_process_type + process_running.set() + last_process_type = process_type + process = subprocess.Popen( cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, @@ -83,34 +52,43 @@ def run_follower_process(cmd): bufsize=1 ) try: - for line in follower_process.stdout: - follower_log_queue.put(line) + for line in process.stdout: + process_log_queue.put(line) except Exception as e: - follower_log_queue.put(f"[Follower error]: {e}\n") - follower_running.clear() + process_log_queue.put(f"[Process error]: {e}\n") + process_running.clear() -def start_follower(cmd): - global follower_thread, follower_running - if follower_running.is_set(): - follower_log_queue.put("[Follower already running]\n") +def start_process(cmd, process_type): + global process_thread, process_running + if process_running.is_set(): + process_log_queue.put("[A process is already running]\n") return - follower_thread = threading.Thread(target=run_follower_process, args=(cmd,), daemon=True) - follower_thread.start() + process_thread = threading.Thread(target=run_process, args=(cmd, process_type), daemon=True) + process_thread.start() -def stop_follower(): - global follower_process, follower_running - if follower_process and follower_running.is_set(): - follower_process.terminate() - follower_running.clear() - follower_log_queue.put("[Follower stopped]\n") +def stop_process(): + global process, process_running + if process and process_running.is_set(): + process.terminate() + process_running.clear() + process_log_queue.put("[Process stopped]\n") -def get_follower_live_log(): - while not follower_log_queue.empty(): - line = follower_log_queue.get() - follower_log_buffer.append(line) - if len(follower_log_buffer) > FOLLOWER_LOG_BUFFER_MAX_LINES: - follower_log_buffer.pop(0) - return ''.join(follower_log_buffer) +def get_combined_live_log(): + # Compose context message at the top + if process_running.is_set(): + if last_process_type == "follower": + header = "Displaying output from trajectory follower...\n\n" + else: + header = "Displaying output from logger...\n\n" + else: + header = "No active process.\n\n" + # Gather lines from queue + while not process_log_queue.empty(): + line = process_log_queue.get() + process_log_buffer.append(line) + if len(process_log_buffer) > PROCESS_LOG_BUFFER_MAX_LINES: + process_log_buffer.pop(0) + return header + ''.join(process_log_buffer) # ----- App Layout ----- app = dash.Dash(__name__, external_stylesheets=[dbc.themes.BOOTSTRAP]) @@ -119,7 +97,14 @@ def get_follower_live_log(): html.H2("Trajectory Dashboard"), dbc.Row([ dbc.Col([ - html.H4("Trajectories"), + dcc.Graph(id="trajectory-map", style={"height": "520px", "margin-bottom": "8px"}), + dbc.Checkbox(id="show-orientation", value=False, className="mb-2", style={"margin-left": "2px"}), + html.Label("Show orientation", htmlFor="show-orientation", style={"margin-left": "25px", "font-size": "90%"}), + ], width=12), + ], style={"margin-bottom": "10px"}), + dbc.Row([ + dbc.Col([ + html.H4("Trajectory Controls"), dcc.Dropdown( id="trajectory-dropdown", options=[{"label": os.path.basename(f), "value": f} for f in get_trajectories()], @@ -130,11 +115,9 @@ def get_follower_live_log(): dbc.Badge("Using last", id="using-last-indicator", color="info", pill=True, style={"display": "none", "margin-bottom": "10px"}), dbc.Button("Reverse", id="reverse-btn", color="warning", style={"width": "100%", "margin-bottom": "10px"}), dbc.Badge("Reverse", id="reverse-indicator", color="warning", pill=True, style={"display": "none", "margin-bottom": "10px"}), - dbc.Button("Record Trajectory", id="record-btn", color="success", style={"width": "100%", "margin-bottom": "10px"}), - dbc.Button("Stop Recording", id="stop-record-btn", color="danger", style={"width": "100%", "margin-bottom": "10px"}), ], width=3), dbc.Col([ - html.H4("Controls"), + html.H4("Action"), dbc.RadioItems( id="mode-radio", options=[ @@ -147,27 +130,94 @@ def get_follower_live_log(): inputStyle={"margin-right": "8px", "margin-left": "16px"} ), dbc.Button("Play Trajectory", id="play-btn", color="primary", style={"width": "100%", "margin-top": "10px"}), - # Optional: add a Stop Follower button if you want to manually interrupt followers - # dbc.Button("Stop Follower", id="stop-follower-btn", color="danger", style={"width": "100%", "margin-top": "10px"}), + dbc.Button("Record Trajectory", id="record-btn", color="success", style={"width": "100%", "margin-top": "10px"}), + dbc.Button("Stop Process", id="stop-process-btn", color="danger", style={"width": "100%", "margin-top": "10px"}), html.Div(id="status", style={"margin-top": "20px"}), - html.Hr(), - html.H5("Live Logger Output"), - html.Pre(id='live-log', style={'height': '200px', 'overflow': 'auto', 'background': '#1a1a1a', 'color': '#22ee22'}), - dcc.Interval(id='log-interval', interval=1000, n_intervals=0), - html.Hr(), - html.H5("Live Follower Output"), - html.Pre(id='live-follower-log', style={'height': '200px', 'overflow': 'auto', 'background': '#222222', 'color': '#ffd700'}), - dcc.Interval(id='follower-log-interval', interval=1000, n_intervals=0), - ], width=9), + ], width=3), + dbc.Col([ + html.H4("Live Output"), + html.Pre(id='live-output', style={'height': '340px', 'overflow': 'auto', 'background': '#141414', 'color': '#e2e2e2', "font-size": "14px", "padding": "12px"}), + dcc.Interval(id='live-output-interval', interval=1000, n_intervals=0), + ], width=6), ]), - # Hidden stores for state dcc.Store(id="using-last-store", data=False), dcc.Store(id="reverse-store", data=False), ]) # ----- CALLBACKS ----- -# "Use Last" and dropdown management (fixes all Output collision issues) +# Trajectory map +@app.callback( + Output("trajectory-map", "figure"), + Input("trajectory-dropdown", "value"), + Input("using-last-store", "data"), + Input("show-orientation", "value"), +) +def update_trajectory_map(selected_file, using_last, show_orientation): + if using_last or selected_file == "USING_LAST": + files = get_trajectories() + if not files: + return { + "data": [], + "layout": {"title": "No trajectories found", "height": 500} + } + yaml_path = files[-1] + elif selected_file: + yaml_path = selected_file + else: + return { + "data": [], + "layout": {"title": "No trajectory selected", "height": 500} + } + lats, lons, yaws = load_trajectory_from_yaml(yaml_path) + if not lats or not lons: + return { + "data": [], + "layout": {"title": "No waypoints in file", "height": 500} + } + # Trajectory line + data = [{ + "type": "scattermapbox", + "lat": lats, + "lon": lons, + "mode": "lines+markers", + "marker": {"size": 8, "color": "red"}, + "line": {"width": 2, "color": "blue"}, + "name": "Trajectory" + }] + # Orientation arrows + if show_orientation: + arrow_scale = 0.000009 + arrow_lats, arrow_lons = [], [] + for lat, lon, yaw in zip(lats, lons, yaws): + dlat = math.cos(yaw) * arrow_scale + dlon = math.sin(yaw) * arrow_scale / max(math.cos(math.radians(lat)), 1e-6) + arrow_lats += [lat, lat + dlat, None] + arrow_lons += [lon, lon + dlon, None] + data.append({ + "type": "scattermapbox", + "lat": arrow_lats, + "lon": arrow_lons, + "mode": "lines", + "line": {"width": 2, "color": "orange"}, + "name": "Yaw", + "showlegend": False + }) + return { + "data": data, + "layout": { + "mapbox": { + "style": "open-street-map", + "center": {"lat": lats[0], "lon": lons[0]}, + "zoom": 17, + }, + "margin": {"l": 0, "r": 0, "t": 30, "b": 0}, + "title": os.path.basename(yaml_path), + "height": 500, + } + } + +# "Use Last" and dropdown management @app.callback( Output("using-last-store", "data"), Output("using-last-indicator", "style"), @@ -182,20 +232,16 @@ def get_follower_live_log(): ) def manage_use_last_and_dropdown(use_last_clicks, dropdown_value, options, is_using_last): triggered = ctx.triggered_id - if triggered == "use-last-btn": using_last = True style = {"display": "inline-block", "margin-bottom": "10px"} options = [{"label": os.path.basename(f), "value": f} for f in get_trajectories()] return using_last, style, "USING_LAST", options, "Using last recording" - if triggered == "trajectory-dropdown": if dropdown_value != "USING_LAST": return False, {"display": "none"}, dropdown_value, options, "Select a trajectory" else: return True, {"display": "inline-block", "margin-bottom": "10px"}, "USING_LAST", options, "Using last recording" - - # Fallback (should not happen) return is_using_last, {"display": "inline-block" if is_using_last else "none", "margin-bottom": "10px"}, dropdown_value, options, "Using last recording" if is_using_last else "Select a trajectory" # Reverse button toggle & indicator @@ -211,7 +257,7 @@ def toggle_reverse(n, is_reverse): style = {"display": "inline-block", "margin-bottom": "10px"} if is_reverse else {"display": "none"} return is_reverse, style -# Play trajectory logic (launches the follower and streams output) +# Play trajectory: launches follower @app.callback( Output("status", "children", allow_duplicate=True), Input("play-btn", "n_clicks"), @@ -222,6 +268,8 @@ def toggle_reverse(n, is_reverse): prevent_initial_call=True ) def play_trajectory(n, mode, dropdown_value, using_last, reverse): + if process_running.is_set(): + return dbc.Alert("Another process is already running!", color="warning") if using_last or dropdown_value == "USING_LAST": arg = "--last" traj_display = "last recording" @@ -230,15 +278,12 @@ def play_trajectory(n, mode, dropdown_value, using_last, reverse): traj_display = os.path.basename(dropdown_value) else: return dbc.Alert("Please select a trajectory!", color="warning") - - # Pick script path if mode == "precise": script = PRECISE_SCRIPT elif mode == "smooth": script = SMOOTH_SCRIPT else: script = INTERACTIVE_SCRIPT - cmd = ["python3", script] if arg == "--last": cmd.append("--last") @@ -246,9 +291,8 @@ def play_trajectory(n, mode, dropdown_value, using_last, reverse): cmd.append(arg) if reverse: cmd.append("--reverse") - try: - start_follower(cmd) + start_process(cmd, "follower") mode_label = {"precise": "Precise", "smooth": "Smooth", "interactive": "Interactive"}[mode] details = f"Playing {traj_display} in {mode_label} mode" if reverse: @@ -257,45 +301,37 @@ def play_trajectory(n, mode, dropdown_value, using_last, reverse): except Exception as e: return dbc.Alert(f"Error: {e}", color="danger") -# Start recording +# Start logger (record trajectory) @app.callback( Output("status", "children", allow_duplicate=True), Input("record-btn", "n_clicks"), prevent_initial_call=True ) def start_recording(n): - if logger_running.is_set(): - return dbc.Alert("Logger already running!", color="warning") - start_logger() + if process_running.is_set(): + return dbc.Alert("Another process is already running!", color="warning") + start_process(['python3', RECORD_SCRIPT], "logger") return dbc.Alert("Recording started!", color="success") -# Stop recording +# Stop logger or follower @app.callback( Output("status", "children", allow_duplicate=True), - Input("stop-record-btn", "n_clicks"), + Input("stop-process-btn", "n_clicks"), prevent_initial_call=True ) -def stop_recording(n): - if not logger_running.is_set(): - return dbc.Alert("Logger is not running!", color="warning") - stop_logger() - return dbc.Alert("Recording stopped.", color="info") - -# Live logger output -@app.callback( - Output('live-log', 'children'), - Input('log-interval', 'n_intervals') -) -def update_log(_): - return get_live_log() +def stop_process_callback(n): + if not process_running.is_set(): + return dbc.Alert("No process is running.", color="warning") + stop_process() + return dbc.Alert("Process stopped.", color="info") -# Live follower output +# Combined live output @app.callback( - Output('live-follower-log', 'children'), - Input('follower-log-interval', 'n_intervals') + Output('live-output', 'children'), + Input('live-output-interval', 'n_intervals') ) -def update_follower_log(_): - return get_follower_live_log() +def update_live_output(_): + return get_combined_live_log() if __name__ == "__main__": - app.run(debug=True, host="0.0.0.0", port=8051) + app.run(debug=True, host="0.0.0.0", port=8055) From db96b5e372a6fff5231bb9e38f68e6bb88dcd919 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 3 Jul 2025 08:15:24 +0000 Subject: [PATCH 06/23] Fixed object placement --- src/nav2_tutorial/src/dashboard/dashboard.py | 39 +++++++++++--------- 1 file changed, 22 insertions(+), 17 deletions(-) diff --git a/src/nav2_tutorial/src/dashboard/dashboard.py b/src/nav2_tutorial/src/dashboard/dashboard.py index eb45384..4142235 100644 --- a/src/nav2_tutorial/src/dashboard/dashboard.py +++ b/src/nav2_tutorial/src/dashboard/dashboard.py @@ -98,8 +98,11 @@ def get_combined_live_log(): dbc.Row([ dbc.Col([ dcc.Graph(id="trajectory-map", style={"height": "520px", "margin-bottom": "8px"}), - dbc.Checkbox(id="show-orientation", value=False, className="mb-2", style={"margin-left": "2px"}), - html.Label("Show orientation", htmlFor="show-orientation", style={"margin-left": "25px", "font-size": "90%"}), + html.Div([ + dbc.Checkbox(id="show-orientation", className="me-2"), + html.Label("Show orientation", htmlFor="show-orientation", style={"margin-bottom": "0", "font-size": "95%"}) + ], style={"display": "flex", "align-items": "center", "margin-bottom": "15px", "margin-left": "8px"} + ), ], width=12), ], style={"margin-bottom": "10px"}), dbc.Row([ @@ -117,21 +120,23 @@ def get_combined_live_log(): dbc.Badge("Reverse", id="reverse-indicator", color="warning", pill=True, style={"display": "none", "margin-bottom": "10px"}), ], width=3), dbc.Col([ - html.H4("Action"), - dbc.RadioItems( - id="mode-radio", - options=[ - {"label": "Precise", "value": "precise"}, - {"label": "Smooth", "value": "smooth"}, - {"label": "Interactive", "value": "interactive"}, - ], - value="precise", - inline=True, - inputStyle={"margin-right": "8px", "margin-left": "16px"} - ), - dbc.Button("Play Trajectory", id="play-btn", color="primary", style={"width": "100%", "margin-top": "10px"}), - dbc.Button("Record Trajectory", id="record-btn", color="success", style={"width": "100%", "margin-top": "10px"}), - dbc.Button("Stop Process", id="stop-process-btn", color="danger", style={"width": "100%", "margin-top": "10px"}), + html.H4("Actions"), + html.Div([ + dbc.RadioItems( + id="mode-radio", + options=[ + {"label": "Precise", "value": "precise"}, + {"label": "Smooth", "value": "smooth"}, + {"label": "Interactive", "value": "interactive"}, + ], + value="precise", + inline=False, # vertical layout, no shifting + style={"margin-bottom": "12px"}, + ), + ], style={"margin-bottom": "20px"}), + dbc.Button("Play Trajectory", id="play-btn", color="primary", style={"width": "100%", "margin-bottom": "12px"}), + dbc.Button("Record Trajectory", id="record-btn", color="success", style={"width": "100%", "margin-bottom": "12px"}), + dbc.Button("Stop Process", id="stop-process-btn", color="danger", style={"width": "100%", "margin-bottom": "12px"}), html.Div(id="status", style={"margin-top": "20px"}), ], width=3), dbc.Col([ From 45fbf1a4b5ced2d2d78e9ac545f99676b6294122 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 3 Jul 2025 12:33:27 +0000 Subject: [PATCH 07/23] Improved docker container --- .devcontainer/Dockerfile | 40 +++++++++++++++++++++++-------- .devcontainer/devcontainer.json | 2 +- .devcontainer/docker-compose.yaml | 3 +++ .devcontainer/requirements.txt | 8 +++++++ 4 files changed, 42 insertions(+), 11 deletions(-) create mode 100644 .devcontainer/requirements.txt diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 67b7ec3..ad46e9e 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -2,6 +2,11 @@ ARG BASE_IMAGE=ros:jazzy-perception FROM ${BASE_IMAGE} +# Build arguments for the new user. +ARG USER_NAME=dev +ARG USER_ID=1000 +ARG GROUP_ID=1000 + # Remove the default "ubuntu" user so that UID 1000 is free. RUN userdel -r ubuntu || true @@ -11,7 +16,15 @@ ENV QT_X11_NO_MITSHM=1 ENV TERM=xterm-256color # Install common APT packages, then conditionally install Mapviz on x86. -RUN apt update && \ +RUN set -e \ + && apt update -o Acquire::AllowInsecureRepositories=true || true \ + && apt install -y --no-install-recommends curl gnupg ca-certificates \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + | gpg --dearmor --yes --batch --no-tty \ + --output /usr/share/keyrings/ros2-latest-archive-keyring.gpg \ + && cp /usr/share/keyrings/ros2-latest-archive-keyring.gpg \ + /usr/share/keyrings/ros-archive-keyring.gpg \ + && apt update && \ apt install -y \ libasio-dev \ apt-utils \ @@ -28,15 +41,13 @@ RUN apt update && \ iproute2 \ can-utils \ net-tools \ - python3-tk \ - python3-matplotlib \ - python3-geopandas \ - python3-pyproj \ ros-jazzy-xacro \ ros-jazzy-navigation2 \ ros-jazzy-nav2-bringup \ ros-jazzy-teleop-twist-keyboard \ ros-jazzy-launch-xml \ + python3-venv \ + python3-pip \ sudo && \ \ # Detect device architecture. @@ -55,11 +66,6 @@ RUN apt update && \ # Clean up apt cache. rm -rf /var/lib/apt/lists/* -# Build arguments for the new user. -ARG USER_NAME=dev -ARG USER_ID=1000 -ARG GROUP_ID=1000 - # Create a new group and user with the specified UID/GID, and grant passwordless sudo. RUN groupadd --gid ${GROUP_ID} ${USER_NAME} || echo "Group ${USER_NAME} already exists" && \ useradd --uid ${USER_ID} --gid ${GROUP_ID} --shell /bin/bash --create-home ${USER_NAME} && \ @@ -73,6 +79,16 @@ RUN echo "alias ls='ls --color=auto'" >> /etc/bash.bashrc && \ # Switch to the new "dev" user. USER ${USER_NAME} +WORKDIR /home/${USER_NAME} + +# Create virtual environment for Python packages +RUN python3 -m venv nav2_env && \ + /bin/bash -c "source nav2_env/bin/activate && pip install --upgrade pip" + +# Install Python packages from requirements.txt +COPY requirements.txt ./ +RUN /bin/bash -c "source nav2_env/bin/activate && \ + pip install -r requirements.txt" # Make interactive shells auto-source ROS and ROS workspace RUN echo 'source /opt/ros/jazzy/setup.bash' >> ~/.bashrc \ @@ -81,5 +97,9 @@ RUN echo 'source /opt/ros/jazzy/setup.bash' >> ~/.bashrc \ # Ensure the user's local bin is in PATH. ENV PATH=/home/${USER_NAME}/.local/bin:${PATH} +# Expose venv to every subsequent shell +ENV VIRTUAL_ENV=/home/${USER_NAME}/nav2_env +ENV PATH=/home/${USER_NAME}/nav2_env/bin:$PATH + # Set DISPLAY for X11 forwarding. ENV DISPLAY=:0 diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 31e1e02..6dfe159 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -9,7 +9,7 @@ "ms-toolsai.jupyter" ], "settings": { - "python.pythonPath": "/usr/bin/python3" + "python.pythonPath": "/home/dev/nav2_env/bin/python" }, "remoteUser": "dev", "updateRemoteUserUID": false diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index 14eebe6..446e1ef 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -27,6 +27,9 @@ services: - | set -e + # source virtual environment + source /home/dev/nav2_env/bin/activate + # source ROS source /opt/ros/jazzy/setup.bash diff --git a/.devcontainer/requirements.txt b/.devcontainer/requirements.txt new file mode 100644 index 0000000..cae41b0 --- /dev/null +++ b/.devcontainer/requirements.txt @@ -0,0 +1,8 @@ +numpy +pyyaml +tk +matplotlib +geopandas +pyproj +dash +dash-bootstrap-components \ No newline at end of file From 60863f90b2e00f98b90cdf390766d5205f7e4f08 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 3 Jul 2025 13:22:21 +0000 Subject: [PATCH 08/23] Added click and go to dashboard --- src/nav2_tutorial/src/dashboard/dashboard.py | 61 ++++++++++++++++++- .../src/dashboard/send_clicked_point.py | 30 +++++++++ .../src/interactive_wp_follower.py | 2 +- 3 files changed, 90 insertions(+), 3 deletions(-) create mode 100755 src/nav2_tutorial/src/dashboard/send_clicked_point.py diff --git a/src/nav2_tutorial/src/dashboard/dashboard.py b/src/nav2_tutorial/src/dashboard/dashboard.py index 4142235..b8b5cd0 100644 --- a/src/nav2_tutorial/src/dashboard/dashboard.py +++ b/src/nav2_tutorial/src/dashboard/dashboard.py @@ -14,7 +14,8 @@ RECORD_SCRIPT = '/home/dev/ros_ws/src/nav2_tutorial/src/loggers/gps_periodic_logger.py' PRECISE_SCRIPT = '/home/dev/ros_ws/src/nav2_tutorial/src/precise_wp_follower.py' SMOOTH_SCRIPT = '/home/dev/ros_ws/src/nav2_tutorial/src/smooth_wp_follower.py' -INTERACTIVE_SCRIPT = '/home/jetson/dev/nav2_tutorial/src/nav2_tutorial/src/interactive_wp_follower.py' +INTERACTIVE_SCRIPT = '/home/dev/ros_ws/src/nav2_tutorial/src/interactive_wp_follower.py' +CLICKED_SCRIPT = "/home/dev/ros_ws/src/nav2_tutorial/src/dashboard/send_clicked_point.py" def get_trajectories(): return sorted(glob.glob(os.path.join(TRAJECTORY_DIR, '*.yaml'))) @@ -58,11 +59,18 @@ def run_process(cmd, process_type): process_log_queue.put(f"[Process error]: {e}\n") process_running.clear() +def clear_process_log(): + global process_log_buffer, process_log_queue + process_log_buffer.clear() + while not process_log_queue.empty(): + process_log_queue.get() + def start_process(cmd, process_type): global process_thread, process_running if process_running.is_set(): process_log_queue.put("[A process is already running]\n") return + clear_process_log() process_thread = threading.Thread(target=run_process, args=(cmd, process_type), daemon=True) process_thread.start() @@ -147,6 +155,8 @@ def get_combined_live_log(): ]), dcc.Store(id="using-last-store", data=False), dcc.Store(id="reverse-store", data=False), + dcc.Store(id="clicked-point", data=None), + dcc.Store(id="mode-store", data="precise"), ]) # ----- CALLBACKS ----- @@ -157,8 +167,10 @@ def get_combined_live_log(): Input("trajectory-dropdown", "value"), Input("using-last-store", "data"), Input("show-orientation", "value"), + Input("clicked-point", "data"), + State("mode-store", "data"), ) -def update_trajectory_map(selected_file, using_last, show_orientation): +def update_trajectory_map(selected_file, using_last, show_orientation, clicked_point, mode): if using_last or selected_file == "USING_LAST": files = get_trajectories() if not files: @@ -208,6 +220,21 @@ def update_trajectory_map(selected_file, using_last, show_orientation): "name": "Yaw", "showlegend": False }) + # Visualize clicked point if in interactive mode and point exists + if mode == "interactive" and clicked_point and "lat" in clicked_point and "lon" in clicked_point: + data.append({ + "type": "scattermapbox", + "lat": [clicked_point["lat"]], + "lon": [clicked_point["lon"]], + "mode": "markers", + "marker": { + "size": 16, + "color": "green", + "opacity": 1.0, + "symbol": "circle" + }, + "name": "Clicked Goal" + }) return { "data": data, "layout": { @@ -338,5 +365,35 @@ def stop_process_callback(n): def update_live_output(_): return get_combined_live_log() +@app.callback( + Output("mode-store", "data"), + Input("mode-radio", "value"), +) +def update_mode_store(selected_mode): + return selected_mode + +# Point navigation +def send_point_to_ros(lat, lon): + subprocess.Popen(["python3", CLICKED_SCRIPT, str(lat), str(lon)]) + +@app.callback( + Output("clicked-point", "data"), + Output("status", "children", allow_duplicate=True), + Input("trajectory-map", "clickData"), + State("mode-store", "data"), + prevent_initial_call=True +) +def handle_map_click(clickData, current_mode): + if current_mode != "interactive": + # Ignore clicks unless in Interactive mode + return dash.no_update, dash.no_update + if clickData and "points" in clickData: + lat = clickData["points"][0]["lat"] + lon = clickData["points"][0]["lon"] + send_point_to_ros(lat, lon) + return {"lat": lat, "lon": lon}, dbc.Alert(f"Sent goal: lat={lat:.6f}, lon={lon:.6f}", color="info") + return dash.no_update, dash.no_update + + if __name__ == "__main__": app.run(debug=True, host="0.0.0.0", port=8055) diff --git a/src/nav2_tutorial/src/dashboard/send_clicked_point.py b/src/nav2_tutorial/src/dashboard/send_clicked_point.py new file mode 100755 index 0000000..ea38118 --- /dev/null +++ b/src/nav2_tutorial/src/dashboard/send_clicked_point.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python3 +import sys +import rclpy +from geometry_msgs.msg import PointStamped + +def main(): + if len(sys.argv) < 3: + print("Usage: send_clicked_point.py LAT LON") + sys.exit(1) + lat = float(sys.argv[1]) + lon = float(sys.argv[2]) + + rclpy.init() + node = rclpy.create_node('clicked_point_sender') + pub = node.create_publisher(PointStamped, '/clicked_point', 1) + msg = PointStamped() + msg.header.frame_id = 'wgs84' + msg.header.stamp = node.get_clock().now().to_msg() + msg.point.x = lon + msg.point.y = lat + msg.point.z = 0.0 + for _ in range(3): # Publish a few times + pub.publish(msg) + rclpy.spin_once(node, timeout_sec=0.1) + print(f"Published clicked point: lat={lat} lon={lon}") + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/src/nav2_tutorial/src/interactive_wp_follower.py b/src/nav2_tutorial/src/interactive_wp_follower.py index 88071f9..a1bb7ed 100644 --- a/src/nav2_tutorial/src/interactive_wp_follower.py +++ b/src/nav2_tutorial/src/interactive_wp_follower.py @@ -2,7 +2,7 @@ from rclpy.node import Node from nav2_simple_commander.robot_navigator import BasicNavigator from geometry_msgs.msg import PointStamped -from .utils.gps_utils import latLonYaw2Geopose +from src.utils.gps_utils import latLonYaw2Geopose class InteractiveGpsWpCommander(Node): From dc64ee98bf8d9319b3da055ec2e0a3d13753ba6a Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 3 Jul 2025 15:22:04 +0000 Subject: [PATCH 09/23] Added click grid --- src/nav2_tutorial/src/dashboard/dashboard.py | 49 ++++++++++++++++++-- 1 file changed, 45 insertions(+), 4 deletions(-) diff --git a/src/nav2_tutorial/src/dashboard/dashboard.py b/src/nav2_tutorial/src/dashboard/dashboard.py index b8b5cd0..31c1a37 100644 --- a/src/nav2_tutorial/src/dashboard/dashboard.py +++ b/src/nav2_tutorial/src/dashboard/dashboard.py @@ -8,6 +8,7 @@ import queue import yaml import math +import numpy as np # ----- Constants ----- TRAJECTORY_DIR = '/home/dev/ros_ws/src/nav2_tutorial/trajectories' @@ -192,7 +193,8 @@ def update_trajectory_map(selected_file, using_last, show_orientation, clicked_p "data": [], "layout": {"title": "No waypoints in file", "height": 500} } - # Trajectory line + + # Trajectory data data = [{ "type": "scattermapbox", "lat": lats, @@ -202,7 +204,8 @@ def update_trajectory_map(selected_file, using_last, show_orientation, clicked_p "line": {"width": 2, "color": "blue"}, "name": "Trajectory" }] - # Orientation arrows + + # Add orientation arrows if requested if show_orientation: arrow_scale = 0.000009 arrow_lats, arrow_lons = [], [] @@ -220,6 +223,36 @@ def update_trajectory_map(selected_file, using_last, show_orientation, clicked_p "name": "Yaw", "showlegend": False }) + + # Dense click grid (interactive only) + if mode == "interactive": + # Use the mean of the loaded trajectory as the center, or first point if only one exists + if len(lats) > 1 and len(lons) > 1: + lat_center, lon_center = sum(lats)/len(lats), sum(lons)/len(lons) + else: + lat_center, lon_center = lats[0], lons[0] + grid_lat, grid_lon = generate_click_grid( + lat_center=lat_center, + lon_center=lon_center, + lat_range=0.0003, # Adjust as needed (≈32m at mid latitudes) + lon_range=0.0005, # ≈40m + n=60 # 40x40 = 1600 clickable points + ) + data.append({ + "type": "scattermapbox", + "lat": grid_lat, + "lon": grid_lon, + "mode": "markers", + "marker": { + "size": 11, + "color": "blue", + "opacity": 0.3, + "symbol": "circle", + }, + "name": "ClickGrid", + "hoverinfo": "none", + }) + # Visualize clicked point if in interactive mode and point exists if mode == "interactive" and clicked_point and "lat" in clicked_point and "lon" in clicked_point: data.append({ @@ -228,20 +261,21 @@ def update_trajectory_map(selected_file, using_last, show_orientation, clicked_p "lon": [clicked_point["lon"]], "mode": "markers", "marker": { - "size": 16, + "size": 18, "color": "green", "opacity": 1.0, "symbol": "circle" }, "name": "Clicked Goal" }) + return { "data": data, "layout": { "mapbox": { "style": "open-street-map", "center": {"lat": lats[0], "lon": lons[0]}, - "zoom": 17, + "zoom": 18, }, "margin": {"l": 0, "r": 0, "t": 30, "b": 0}, "title": os.path.basename(yaml_path), @@ -249,6 +283,7 @@ def update_trajectory_map(selected_file, using_last, show_orientation, clicked_p } } + # "Use Last" and dropdown management @app.callback( Output("using-last-store", "data"), @@ -373,6 +408,12 @@ def update_mode_store(selected_mode): return selected_mode # Point navigation +def generate_click_grid(lat_center, lon_center, lat_range=0.002, lon_range=0.003, n=40): + lats = np.linspace(lat_center - lat_range, lat_center + lat_range, n) + lons = np.linspace(lon_center - lon_range, lon_center + lon_range, n) + grid_lats, grid_lons = np.meshgrid(lats, lons) + return grid_lats.flatten(), grid_lons.flatten() + def send_point_to_ros(lat, lon): subprocess.Popen(["python3", CLICKED_SCRIPT, str(lat), str(lon)]) From f7b5648c4e816a72d569e1710d5b36d1e2ca1c4b Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 9 Jul 2025 16:17:50 +0200 Subject: [PATCH 10/23] Overall improvements to Docker container and scripts --- .devcontainer/Dockerfile | 3 +- .devcontainer/docker-compose.yaml | 2 +- .devcontainer/requirements.txt | 3 + README.md | 6 +- scripts/clean-container.sh | 3 + scripts/logs-container.sh | 2 + scripts/restart-container.sh | 2 + scripts/start-container.sh | 2 + scripts/test-can.sh | 2 + src/nav2_tutorial/src/precise_wp_follower.py | 258 ++++---- src/nav2_tutorial/src/smooth_wp_follower.py | 592 +++++++------------ src/nav2_tutorial/src/utils/parser_utils.py | 141 +++++ 12 files changed, 487 insertions(+), 529 deletions(-) create mode 100755 scripts/clean-container.sh create mode 100755 scripts/logs-container.sh create mode 100755 scripts/restart-container.sh create mode 100755 scripts/start-container.sh create mode 100755 scripts/test-can.sh create mode 100644 src/nav2_tutorial/src/utils/parser_utils.py diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index ad46e9e..9cf0edd 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -83,7 +83,7 @@ WORKDIR /home/${USER_NAME} # Create virtual environment for Python packages RUN python3 -m venv nav2_env && \ - /bin/bash -c "source nav2_env/bin/activate && pip install --upgrade pip" + /bin/bash -c "source nav2_env/bin/activate && pip install --upgrade pip && pip install 'setuptools<69' 'pip<24'" # Install Python packages from requirements.txt COPY requirements.txt ./ @@ -100,6 +100,7 @@ ENV PATH=/home/${USER_NAME}/.local/bin:${PATH} # Expose venv to every subsequent shell ENV VIRTUAL_ENV=/home/${USER_NAME}/nav2_env ENV PATH=/home/${USER_NAME}/nav2_env/bin:$PATH +ENV PYTHONPATH=/home/dev/nav2_env/lib/python3.12/site-packages:$PYTHONPATH # Set DISPLAY for X11 forwarding. ENV DISPLAY=:0 diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index 446e1ef..c4f22bd 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -34,7 +34,7 @@ services: source /opt/ros/jazzy/setup.bash # ensure the workspace is built or latest build is sourced - colcon build --cmake-args -DBUILD_TESTING=OFF + colcon build --symlink-install --cmake-args -DBUILD_TESTING=OFF # source the workspace source install/setup.bash diff --git a/.devcontainer/requirements.txt b/.devcontainer/requirements.txt index cae41b0..17fff89 100644 --- a/.devcontainer/requirements.txt +++ b/.devcontainer/requirements.txt @@ -1,3 +1,6 @@ +catkin_pkg +empy==3.3.4 +lark numpy pyyaml tk diff --git a/README.md b/README.md index 362bb55..4a8ed5d 100644 --- a/README.md +++ b/README.md @@ -72,7 +72,7 @@ converter: Build the ROS2 workspace. ```bash source /opt/ros/jazzy/setup.bash -colcon build --cmake-args -DBUILD_TESTING=OFF +colcon build --symlink-install --cmake-args -DBUILD_TESTING=OFF ``` @@ -177,7 +177,7 @@ waypoints: ### Manual Logger (`gps_keylogger.py`) ```bash -ros2 run nav2_tutorial gps_keylogger.py [optional_output.yaml] +ros2 run nav2_tutorial gps_keylogger [optional_output.yaml] ``` * Press `'f'` to log a waypoint @@ -190,7 +190,7 @@ Waypoints are saved immediately and safely to disk. ### Periodic Logger (`gps_periodic_logger.py`) ```bash -ros2 run nav2_tutorial gps_periodic_logger.py [optional_output.yaml] -i 0.2 +ros2 run nav2_tutorial gps_periodic_logger [optional_output.yaml] -i 0.2 ``` * Logs waypoints automatically every 0.2s (default) diff --git a/scripts/clean-container.sh b/scripts/clean-container.sh new file mode 100755 index 0000000..a654e31 --- /dev/null +++ b/scripts/clean-container.sh @@ -0,0 +1,3 @@ +#!/bin/bash +docker compose -f ~/dev/nav2_tutorial/.devcontainer/docker-compose.yaml down +docker compose -f ~/dev/nav2_tutorial/.devcontainer/docker-compose.yaml up --build -d diff --git a/scripts/logs-container.sh b/scripts/logs-container.sh new file mode 100755 index 0000000..c6e667d --- /dev/null +++ b/scripts/logs-container.sh @@ -0,0 +1,2 @@ +#!/bin/bash +docker compose -f ~/dev/nav2_tutorial/.devcontainer/docker-compose.yaml logs vrtk diff --git a/scripts/restart-container.sh b/scripts/restart-container.sh new file mode 100755 index 0000000..fdbd396 --- /dev/null +++ b/scripts/restart-container.sh @@ -0,0 +1,2 @@ +#!/bin/bash +docker compose -f ~/dev/nav2_tutorial/.devcontainer/docker-compose.yaml restart vrtk diff --git a/scripts/start-container.sh b/scripts/start-container.sh new file mode 100755 index 0000000..a6b471b --- /dev/null +++ b/scripts/start-container.sh @@ -0,0 +1,2 @@ +#!/bin/bash +docker compose -f ~/dev/nav2_tutorial/.devcontainer/docker-compose.yaml exec vrtk bash diff --git a/scripts/test-can.sh b/scripts/test-can.sh new file mode 100755 index 0000000..11860c6 --- /dev/null +++ b/scripts/test-can.sh @@ -0,0 +1,2 @@ +#!/bin/bash +candump can0 diff --git a/src/nav2_tutorial/src/precise_wp_follower.py b/src/nav2_tutorial/src/precise_wp_follower.py index 655092d..cbf665f 100644 --- a/src/nav2_tutorial/src/precise_wp_follower.py +++ b/src/nav2_tutorial/src/precise_wp_follower.py @@ -1,179 +1,131 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +import glob +import math import os import sys -import time -import math import yaml -import glob -import argparse -from datetime import datetime +from typing import List, Tuple import rclpy -from nav2_simple_commander.robot_navigator import BasicNavigator from ament_index_python.packages import get_package_share_directory +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from pyproj import Transformer +from src.utils.gps_utils import quaternion_from_euler +from src.utils.parser_utils import YamlWaypointParser, _latest_file, _resolve_ws_paths, _select_yaml -from src.utils.gps_utils import latLonYaw2Geopose - - -def _latest_file(directory: str, pattern: str = "gps_waypoints_2*.yaml") -> str | None: - """Return the lexically newest file matching *pattern* inside *directory*. - - The files produced by *terminal_logger.py* embed a timestamp in their name - (e.g. ``gps_waypoints_20250317_142311.yaml``) so lexical order === - chronological order. If *directory* does not exist or contains no matching - files, ``None`` is returned. - """ - if not os.path.isdir(directory): - return None - files = glob.glob(os.path.join(directory, pattern)) - if not files: - return None - files.sort() - return files[-1] - - -def _resolve_ws_paths() -> tuple[str, str]: - """Return ``(src_traj_dir, share_traj_dir)`` for *nav2_tutorial*. - - *share_traj_dir* is the directory installed by ``ament`` (typically - ``install/share/nav2_tutorial/trajectories``) while *src_traj_dir* points to - the editable source tree (``/src/nav2_tutorial/trajectories``). - """ - share_dir = get_package_share_directory("nav2_tutorial") - share_traj_dir = os.path.join(share_dir, "trajectories") - - # share/nav2_tutorial -> share -> install -> - ws_root = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(share_dir)))) - src_traj_dir = os.path.join(ws_root, "src", "nav2_tutorial", "trajectories") - return src_traj_dir, share_traj_dir - - -def _select_yaml(args: argparse.Namespace) -> str: - """Figure out which YAML file we should load. - - Priority: - 1. If the user passed an explicit path, use it. - 2. If ``--last`` was requested, compare the newest file from the *src* and - *share* directories and choose the newer one (with a preference for the - *src* file if both have identical timestamps). - 3. Otherwise, attempt to load ``gps_waypoints.yaml`` from *src* first then - *share*. - - A *FileNotFoundError* is raised if no suitable file is found so that the - caller can decide how to handle the error. - """ - src_traj_dir, share_traj_dir = _resolve_ws_paths() - - # 1) Explicit path - if args.yaml_file is not None: - explicit = os.path.expanduser(args.yaml_file) - if not os.path.isfile(explicit): - raise FileNotFoundError(explicit) - return explicit - - # 2) --last flag - if args.last: - src_latest = _latest_file(src_traj_dir) - share_latest = _latest_file(share_traj_dir) - - if src_latest and share_latest: - # Compare by timestamp embedded in filename (lexical order) or mtime - chosen = src_latest if os.path.basename(src_latest) > os.path.basename(share_latest) else share_latest - else: - chosen = src_latest or share_latest # whichever is not None (could still be None) - - if chosen is None: - raise FileNotFoundError(f"No waypoint files matching 'gps_waypoints_2*.yaml' found in '{src_traj_dir}' or '{share_traj_dir}'.") - return chosen - - # 3) Default lookup "gps_waypoints.yaml" - default_name = "gps_waypoints.yaml" - candidate_src = os.path.join(src_traj_dir, default_name) - if os.path.isfile(candidate_src): - return candidate_src - - candidate_share = os.path.join(share_traj_dir, default_name) - if os.path.isfile(candidate_share): - return candidate_share - - raise FileNotFoundError(f"Default waypoint file '{default_name}' not found in '{src_traj_dir}' or '{share_traj_dir}'.") - - -class YamlWaypointParser: - """Parse a set of GPS waypoints from a YAML file.""" - - def __init__(self, wps_file_path: str): - if not os.path.isfile(wps_file_path): - raise FileNotFoundError(wps_file_path) - - try: - with open(wps_file_path, "r") as wps_file: - self.wps_dict = yaml.safe_load(wps_file) or {} - except yaml.YAMLError as err: - raise RuntimeError(f"Failed to parse YAML file '{wps_file_path}': {err}") - - if "waypoints" not in self.wps_dict: - raise KeyError(f"Key 'waypoints' missing from YAML file '{wps_file_path}'.") - - @staticmethod - def _reverse_yaw(yaw: float) -> float: - """Return yaw rotated by π, wrapped to [-π, π].""" - new_yaw = yaw + math.pi - # Wrap to [-π, π] - return (new_yaw + math.pi) % (2 * math.pi) - math.pi - - def get_wps(self, reverse: bool = False): - """Return a list of ``GeoPose`` objects, possibly in reverse order.""" - waypoints = self.wps_dict["waypoints"][::-1] if reverse else self.wps_dict["waypoints"] - gepose_wps = [] - for wp in waypoints: - yaw = self._reverse_yaw(wp["yaw"]) if reverse else wp["yaw"] - gepose_wps.append(latLonYaw2Geopose(wp["latitude"], wp["longitude"], yaw)) - return gepose_wps +# ROS msgs +from sensor_msgs.msg import NavSatFix +from geometry_msgs.msg import PoseStamped +from rclpy.qos import qos_profile_sensor_data class GpsWpCommander: - """Use the Nav2 *GPS Waypoint Follower* to follow a set of waypoints.""" - - def __init__(self, wps_file_path: str, reverse: bool = False): + def __init__(self, yaml_path: str, *, reverse: bool = False): self.navigator = BasicNavigator("basic_navigator") - self.wp_parser = YamlWaypointParser(wps_file_path) - self.reverse = reverse - - def start_wpf(self): - """Block until all waypoints have been reached.""" + self.wps = YamlWaypointParser(yaml_path).get_wps(reverse=reverse) + if not self.wps: + raise RuntimeError("Waypoint list is empty.") + + # Build ENU transformer --------------------------------------- + base_lat, base_lon, base_alt = self._get_datum() + enu_pipeline = ( + "+proj=pipeline " + "+step +proj=unitconvert +xy_in=deg +xy_out=rad " + "+step +proj=axisswap +order=2,1,3 " + "+step +proj=cart +ellps=WGS84 " + f"+step +proj=topocentric +ellps=WGS84 +lat_0={base_lat} +lon_0={base_lon} +h_0={base_alt} " + ) + self._tf_llh2enu = Transformer.from_pipeline(enu_pipeline) + + self.max_retries = 1 + + # --------------------- datum helper ------------------------------ + def _get_datum(self, topic: str = "/fixposition/datum", timeout_s: float = 3.0): + datum = None + def _cb(msg: NavSatFix): + nonlocal datum + datum = (msg.latitude, msg.longitude, msg.altitude) + sub = self.navigator.create_subscription(NavSatFix, topic, _cb, qos_profile_sensor_data) + deadline = self.navigator.get_clock().now() + rclpy.duration.Duration(seconds=timeout_s) + while rclpy.ok() and datum is None: + if self.navigator.get_clock().now() > deadline: + raise RuntimeError(f"No NavSatFix on {topic} within {timeout_s}s") + rclpy.spin_once(self.navigator, timeout_sec=0.05) + self.navigator.destroy_subscription(sub) + return datum + + # --------------------- conversion helper ------------------------- + def _latlon_to_pose(self, lat: float, lon: float, yaw: float) -> PoseStamped: + x, y, _ = self._tf_llh2enu.transform(lat, lon, 0.0) + ps = PoseStamped() + ps.header.frame_id = "map" + ps.header.stamp = rclpy.time.Time().to_msg() + ps.pose.position.x, ps.pose.position.y = x, y + ps.pose.position.z = 0.0 + ps.pose.orientation = quaternion_from_euler(0.0, 0.0, yaw) + return ps + + # --------------------- main loop --------------------------------- + def start(self): self.navigator.waitUntilNav2Active(localizer="robot_localization") - wps = self.wp_parser.get_wps(reverse=self.reverse) - self.navigator.followGpsWaypoints(wps) - while not self.navigator.isTaskComplete(): - time.sleep(0.1) - print("Waypoints completed successfully") - - -def main(argv: list[str] | None = None): + poses = [self._latlon_to_pose(wp["latitude"], wp["longitude"], wp["yaw"]) for wp in self.wps] + self.navigator.get_logger().info(f"Starting precise run with {len(poses)} waypoint(s)…") + + for idx, pose in enumerate(poses, start=1): + retries_left = self.max_retries + while True: + self.navigator.goToPose(pose) + # spin until done or shutdown + while rclpy.ok() and not self.navigator.isTaskComplete(): + rclpy.spin_once(self.navigator, timeout_sec=0.05) + result = self.navigator.getResult() + # -------------------------------------------------- SUCCEEDED + if result == TaskResult.SUCCEEDED: + self.navigator.get_logger().info(f"Waypoint {idx}/{len(poses)} reached") + break + # -------------------------------------------------- FAILED / CANCELED + if result in (TaskResult.FAILED, TaskResult.CANCELED): + if retries_left > 0: + retries_left -= 1 + self.navigator.get_logger().warn( + f"Waypoint {idx} {result.name.lower()} - retrying ({self.max_retries - retries_left}/{self.max_retries})") + continue + self.navigator.get_logger().error(f"Waypoint {idx} failed twice - aborting trajectory") + return + # -------------------------------------------------- other / unknown + self.navigator.get_logger().error(f"Unexpected TaskResult {result} - aborting") + return + + self.navigator.get_logger().info("All waypoints completed successfully!") + + +def main(argv: List[str] | None = None): rclpy.init(args=argv) - parser = argparse.ArgumentParser(description="Follow GPS waypoints from a YAML file") - parser.add_argument("yaml_file", nargs="?", default=None, help="Path to the YAML waypoints file") - parser.add_argument("--last", action="store_true", help="Load the most recent waypoints file in the config directory") - parser.add_argument("--reverse", action="store_true", help="Follow the trajectory in reverse order (adds 180° to yaw)") + parser.add_argument("yaml_file", nargs="?", default=None, help="Path to YAML waypoint file") + parser.add_argument("--last", action="store_true", help="Use the most recent waypoint file") + parser.add_argument("--reverse", action="store_true", help="Follow the path in reverse order (adds 180° yaw)") args = parser.parse_args() try: - yaml_file_path = _select_yaml(args) - except FileNotFoundError as e: - print(f"[WARN] {e}", file=sys.stderr) + yaml_path = _select_yaml(args) + except FileNotFoundError as exc: + print(f"[ERROR] {exc}", file=sys.stderr) sys.exit(1) - # Feedback to the user - print(f"Loading waypoints file: {yaml_file_path}") - try: - gps_wpf = GpsWpCommander(yaml_file_path, reverse=args.reverse) - gps_wpf.start_wpf() + commander = GpsWpCommander(yaml_path, reverse=args.reverse) + commander.start() + except KeyboardInterrupt: + print("Interrupted by user - shutting down…", file=sys.stderr) except Exception as exc: print(f"[ERROR] {exc}", file=sys.stderr) sys.exit(1) - + finally: + rclpy.shutdown() if __name__ == "__main__": main() diff --git a/src/nav2_tutorial/src/smooth_wp_follower.py b/src/nav2_tutorial/src/smooth_wp_follower.py index 139f853..5f0790f 100644 --- a/src/nav2_tutorial/src/smooth_wp_follower.py +++ b/src/nav2_tutorial/src/smooth_wp_follower.py @@ -2,18 +2,17 @@ import sys import math import yaml -import glob import argparse import rclpy from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult -from ament_index_python.packages import get_package_share_directory -from src.utils.gps_utils import latLonYaw2Geopose from rclpy.action import ActionClient from rclpy.qos import qos_profile_sensor_data from uuid import UUID from pyproj import Transformer +from src.utils.gps_utils import quaternion_from_euler +from src.utils.parser_utils import YamlWaypointParser, _latest_file, _resolve_ws_paths, _select_yaml # ROS messages from sensor_msgs.msg import NavSatFix @@ -30,7 +29,7 @@ def _hsv_to_rgb(h, s=1.0, v=1.0): - """h ∈ [0,1] → (r,g,b) ∈ [0,1]³ (simple HSV→RGB)""" + """HSV to RGB converter.""" i = int(h*6.0) % 6 f = h*6.0 - i p, q, t = v*(1-s), v*(1-f*s), v*(1-(1-f)*s) @@ -43,162 +42,17 @@ def _hsv_to_rgb(h, s=1.0, v=1.0): return r, g, b -def quaternion_from_euler(roll: float, pitch: float, yaw: float): - """ - Returns (x, y, z, w). - """ - cy = math.cos(yaw * 0.5) - sy = math.sin(yaw * 0.5) - cp = math.cos(pitch * 0.5) - sp = math.sin(pitch * 0.5) - cr = math.cos(roll * 0.5) - sr = math.sin(roll * 0.5) - - return ( - sr * cp * cy - cr * sp * sy, - cr * sp * cy + sr * cp * sy, - cr * cp * sy - sr * sp * cy, - cr * cp * cy + sr * sp * sy, - ) - - -def _latest_file(directory: str, pattern: str = "gps_waypoints_2*.yaml") -> str | None: - """Return the lexically newest file matching *pattern* inside *directory*. - - The files produced by *terminal_logger.py* embed a timestamp in their name - (e.g. ``gps_waypoints_20250317_142311.yaml``) so lexical order === - chronological order. If *directory* does not exist or contains no matching - files, ``None`` is returned. - """ - if not os.path.isdir(directory): - return None - files = glob.glob(os.path.join(directory, pattern)) - if not files: - return None - files.sort() - return files[-1] - - -def _resolve_ws_paths() -> tuple[str, str]: - """Return ``(src_traj_dir, share_traj_dir)`` for *nav2_tutorial*. - - *share_traj_dir* is the directory installed by ``ament`` (typically - ``install/share/nav2_tutorial/trajectories``) while *src_traj_dir* points to - the editable source tree (``/src/nav2_tutorial/trajectories``). - """ - share_dir = get_package_share_directory("nav2_tutorial") - share_traj_dir = os.path.join(share_dir, "trajectories") - - # share/nav2_tutorial -> share -> install -> - ws_root = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(share_dir)))) - src_traj_dir = os.path.join(ws_root, "src", "nav2_tutorial", "trajectories") - return src_traj_dir, share_traj_dir - - -def _select_yaml(args: argparse.Namespace) -> str: - """Figure out which YAML file we should load. - - Priority: - 1. If the user passed an explicit path, use it. - 2. If ``--last`` was requested, compare the newest file from the *src* and - *share* directories and choose the newer one (with a preference for the - *src* file if both have identical timestamps). - 3. Otherwise, attempt to load ``gps_waypoints.yaml`` from *src* first then - *share*. - - A *FileNotFoundError* is raised if no suitable file is found so that the - caller can decide how to handle the error. - """ - src_traj_dir, share_traj_dir = _resolve_ws_paths() - - # 1) Explicit path - if args.yaml_file is not None: - explicit = os.path.expanduser(args.yaml_file) - if not os.path.isfile(explicit): - raise FileNotFoundError(explicit) - return explicit - - # 2) --last flag - if args.last: - src_latest = _latest_file(src_traj_dir) - share_latest = _latest_file(share_traj_dir) - - if src_latest and share_latest: - # Compare by timestamp embedded in filename (lexical order) or mtime - chosen = src_latest if os.path.basename(src_latest) > os.path.basename(share_latest) else share_latest - else: - chosen = src_latest or share_latest # whichever is not None (could still be None) - - if chosen is None: - raise FileNotFoundError(f"No waypoint files matching 'gps_waypoints_2*.yaml' found in '{src_traj_dir}' or '{share_traj_dir}'.") - return chosen - - # 3) Default lookup "gps_waypoints.yaml" - default_name = "gps_waypoints.yaml" - candidate_src = os.path.join(src_traj_dir, default_name) - if os.path.isfile(candidate_src): - return candidate_src - - candidate_share = os.path.join(share_traj_dir, default_name) - if os.path.isfile(candidate_share): - return candidate_share - - raise FileNotFoundError(f"Default waypoint file '{default_name}' not found in '{src_traj_dir}' or '{share_traj_dir}'.") - - -class YamlWaypointParser: - """Parse a set of GPS waypoints from a YAML file.""" - - def __init__(self, wps_file_path: str): - if not os.path.isfile(wps_file_path): - raise FileNotFoundError(wps_file_path) - - try: - with open(wps_file_path, "r") as wps_file: - self.wps_dict = yaml.safe_load(wps_file) or {} - except yaml.YAMLError as err: - raise RuntimeError(f"Failed to parse YAML file '{wps_file_path}': {err}") - - if "waypoints" not in self.wps_dict: - raise KeyError(f"Key 'waypoints' missing from YAML file '{wps_file_path}'.") - - @staticmethod - def _reverse_yaw(yaw: float) -> float: - """Return yaw rotated by pi, wrapped to [-pi, pi].""" - new_yaw = yaw + math.pi - return (new_yaw + math.pi) % (2 * math.pi) - math.pi - - def get_wps(self, reverse: bool = False): - """Return a list of ``GeoPose`` objects, possibly in reverse order.""" - waypoints = self.wps_dict["waypoints"][::-1] if reverse else self.wps_dict["waypoints"] - - # Filter out immediate duplicates (distance < 1 cm) - cleaned = [waypoints[0]] - for wp in waypoints[1:]: - dx = (wp["latitude"] - cleaned[-1]["latitude"] ) - dy = (wp["longitude"] - cleaned[-1]["longitude"]) - if abs(dx) > 1e-8 or abs(dy) > 1e-8: # ≈ 1 cm threshold - cleaned.append(wp) - waypoints = cleaned - - geopose_wps = [] - for wp in waypoints: - yaw = self._reverse_yaw(wp["yaw"]) if reverse else wp["yaw"] - geopose_wps.append(latLonYaw2Geopose(wp["latitude"], wp["longitude"], yaw)) - return geopose_wps - - def _get_goal_handle(nav): - """Return the ActionGoal handle of the *latest* NavigateThroughPoses goal.""" + """Return the ActionGoal handle of the latest NavigateThroughPoses goal.""" for attr in ("_ntp_goal_handle", # Iron, Jazzy "_nav_through_poses_goal_handle" # Humble, Galactic ): if hasattr(nav, attr): return getattr(nav, attr) - return None # very old / rolling-edge: no public handle + return None def _get_current_uuid(nav): - """Return the UUID (bytes) of the goal that is *currently* active.""" + """Return the UUID (bytes) of the goal that is currently active.""" for attr in ("_current_nav_through_poses_uuid", # Humble "_current_ntp_uuid" # Iron/Jazzy ): @@ -210,7 +64,7 @@ def _get_current_uuid(nav): class GpsWpCommander: """Follow a smooth sequence of GPS waypoints.""" - def __init__(self, wps_file_path: str, reverse: bool = False): + def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1): self.navigator = BasicNavigator("basic_navigator") self.wp_parser = YamlWaypointParser(wps_file_path) self.reverse = reverse @@ -226,45 +80,45 @@ def __init__(self, wps_file_path: str, reverse: bool = False): enu_pipeline = ( "+proj=pipeline " "+step +proj=unitconvert +xy_in=deg +xy_out=rad " - "+step +proj=axisswap +order=2,1,3 " # lon,lat order for cart + "+step +proj=axisswap +order=2,1,3 " # lon,lat order "+step +proj=cart +ellps=WGS84 " f"+step +proj=topocentric +ellps=WGS84 " f" +lat_0={_base_lat} +lon_0={_base_lon} +h_0={_base_alt} " ) - self._tf_llh2enu = Transformer.from_pipeline(enu_pipeline) # Parameters for the NavigateThroughPoses task - self.odom_timeout_s = 2.0 # wait this long for first /fixposition msg - self.max_retries = 1 # re-attempt each failed segment once + self.odom_timeout_s = 2.0 # wait this long for first /fixposition msg + self.max_retries = 1 # re-attempt each failed segment once self._retries_left = self.max_retries self._last_goal_uuid: UUID | None = None + self.num_loop = num_loop # Sliding-window parameters # For long mission: seg_len_max = 9.0, advance_tol = 3.60, overlap_tol = 1.30 - self.seg_len_max = 6.0 # m – length of window Nav2 sees - self.advance_tol = 2.6 # m – when to roll the window forward - self.overlap_tol = 1.0 # m – keep poses this close as overlap - self.start_seg_tol = 3.0 # m – max distance we “snap” to the path + # For several loops: seg_len_max = 20.0, advance_tol = 4.60, overlap_tol = 3.00 + self.seg_len_max = 6.0 # m - length of window Nav2 sees + self.advance_tol = 2.6 # m - when to roll the window forward + self.overlap_tol = 1.0 # m - keep poses this close as overlap + self.start_seg_tol = 3.0 # m - max distance we “snap” to the path self._fp_client = None # Statistics - self._total_wps = 0 # filled in start_ntp() - self._visited_wps = 0 # will be advanced per segment + self._total_wps = 0 + self._visited_wps = 0 + # Marker visualization if MARKER_VIZ: latched_qos = QoSProfile( depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) - # Path for visualizing waypoints + # Path and MarkerArray for visualizing the current segment self.waypoint_path_pub = self.navigator.create_publisher( Path, "/gps_waypoints_path", latched_qos) - - # MarkerArray for visualizing waypoints self.waypoint_marker_pub = self.navigator.create_publisher( MarkerArray, "/gps_waypoints_markers", latched_qos) - # Path for visualizing the full trajectory + # Path and MarkerArray for visualizing the full trajectory self.full_traj_pub = self.navigator.create_publisher( Path, "/gps_full_traj", latched_qos) self.full_traj_mrk_pub = self.navigator.create_publisher( @@ -273,8 +127,8 @@ def __init__(self, wps_file_path: str, reverse: bool = False): def _get_datum(self, topic: str = "/fixposition/datum", timeout_s: float = 3.0) -> tuple[float, float, float]: """ - Block until a NavSatFix is received on *topic* (max *timeout_s*). - Returns (lat [deg], lon [deg], alt [m]). Raises RuntimeError on timeout. + Block until a NavSatFix is received on topic (max timeout_s). + Returns (lat [deg], lon [deg], alt [m]). Raises RuntimeError on timeout. """ datum: list[float] | None = None @@ -293,11 +147,11 @@ def _cb(msg: NavSatFix): if self.navigator.get_clock().now() > deadline: raise RuntimeError(f"No NavSatFix on {topic} within {timeout_s}s") - self.navigator.destroy_subscription(sub) # one-shot: free resources + self.navigator.destroy_subscription(sub) return tuple(datum) - # Convert (lat, lon, yaw) to PoseStamped in the map frame def _latlon_to_pose(self, lat: float, lon: float, yaw: float) -> PoseStamped: + """Convert latitude, longitude, and yaw to a PoseStamped in the map frame.""" x, y, _ = self._tf_llh2enu.transform(lat, lon, 0.0) pose = PoseStamped() pose.header.frame_id = "map" @@ -305,19 +159,15 @@ def _latlon_to_pose(self, lat: float, lon: float, yaw: float) -> PoseStamped: pose.pose.position.x = x pose.pose.position.y = y pose.pose.position.z = 0.0 - qx, qy, qz, qw = quaternion_from_euler(0.0, 0.0, yaw) - pose.pose.orientation.x = qx - pose.pose.orientation.y = qy - pose.pose.orientation.z = qz - pose.pose.orientation.w = qw + pose.pose.orientation = quaternion_from_euler(0.0, 0.0, yaw) return pose def start_ntp(self): + """Start the NavigateThroughPoses task with the waypoints.""" + # Wait for the robot_localization node to be active self.navigator.waitUntilNav2Active(localizer="robot_localization") - # ------------------------------------------------------------ - # 1) Convert the whole YAML path once - # ------------------------------------------------------------ + # Convert the whole YAML path once all_poses = [ self._latlon_to_pose(wp["latitude"], wp["longitude"], self.wp_parser._reverse_yaw(wp["yaw"]) if self.reverse else wp["yaw"]) @@ -328,208 +178,202 @@ def start_ntp(self): # Publish the full trajectory for visualization self._publish_full_traj(all_poses) - self._total_wps = len(all_poses) - total_len = self._path_length(all_poses) - self.navigator.get_logger().info( - f"Preparing segments for trajectory of {total_len:0.2f} m " - f"consisting of {self._total_wps} waypoints…") - remaining = all_poses # “to-do” list - seg_idx = 0 - - # ------------------------------------------------------------ - # 2) Prime the first window - # ------------------------------------------------------------ - robot = self._get_robot_pose() - - # Fast-forward to the first pose within start_seg_tol - closest_idx = self._closest_wp_index(robot, remaining) - if self._dist(robot, remaining[closest_idx]) > self.start_seg_tol: - closest_idx = 0 # fall back to the first wp - - if closest_idx: - self._visited_wps += closest_idx # statistics - skipped = closest_idx - remaining = remaining[closest_idx:] + for loop_idx in range(self.num_loop): + if self.num_loop > 1: + self.navigator.get_logger().info( + f"Performing loop {loop_idx + 1} out of {self.num_loop}…") + + self._total_wps = len(all_poses) + total_len = self._path_length(all_poses) self.navigator.get_logger().info( - f"Skipped {skipped} waypoint(s); " - f"starting with wp #{skipped+1}/{self._total_wps}") - - # if everything was within the tolerance, we’re already done! - if not remaining: - self.navigator.get_logger().info("Trajectory already completed.") - return - - window = self._make_window(robot, remaining) - if len(window) == 1: # no waypoint fit inside seg_len_max - window.append(remaining[0]) - new_wps = 1 - else: - new_wps = len(window) - 1 - - self._visited_wps += new_wps - self._log_segment(window, - overlap_cnt=0, - seg_idx =seg_idx) - self._publish_waypoints(window, seg_idx) - self.navigator.goThroughPoses(window) - - remaining = remaining[new_wps:] - seg_idx += 1 + f"Preparing segments for trajectory of {total_len:0.2f} m " + f"consisting of {self._total_wps} waypoints…") + remaining = all_poses + seg_idx = 0 - # ------------------------------------------------------------ - # 3) Continuous streaming loop - # ------------------------------------------------------------ - while rclpy.ok(): - rclpy.spin_once(self.navigator, timeout_sec=0.05) + # Prime the first window + robot = self._get_robot_pose() + + if self.num_loop == 1: + # Fast-forward to the first pose within start_seg_tol + closest_idx = self._closest_wp_index(robot, remaining) + if self._dist(robot, remaining[closest_idx]) > self.start_seg_tol: + closest_idx = 0 # fall back to the first wp + + if closest_idx: + self._visited_wps += closest_idx # statistics + skipped = closest_idx + remaining = remaining[closest_idx:] + self.navigator.get_logger().info( + f"Skipped {skipped} waypoint(s); " + f"starting with wp #{skipped+1}/{self._total_wps}") + + # If everything was within the tolerance, finish trajectory follower + if not remaining: + self.navigator.get_logger().info("Trajectory already completed.") + return + + window = self._make_window(robot, remaining) + if len(window) == 1: # no waypoint fit inside seg_len_max + window.append(remaining[0]) + new_wps = 1 + else: + new_wps = len(window) - 1 + + self._visited_wps += new_wps + self._log_segment(window, + overlap_cnt=0, + seg_idx =seg_idx) + self._publish_waypoints(window, seg_idx) + self.navigator.goThroughPoses(window) + + remaining = remaining[new_wps:] + seg_idx += 1 - if self.navigator.isTaskComplete(): - status = self.navigator.getResult() # TaskResult enum - - goal_handle = _get_goal_handle(self.navigator) # may be None - current_id = _get_current_uuid(self.navigator) - - # If we can’t identify the result reliably, treat it as current - if goal_handle is None or current_id is None: - result_is_current = True - else: - result_id = UUID(bytes=goal_handle.goal_id.uuid) - result_is_current = (result_id == UUID(bytes=current_id)) - - # *Stale* result from the goal we just pre-empted - if not result_is_current: - continue # ignore and keep streaming - - # Status handling - robot = self._get_robot_pose() # live pose for all cases - - #----------------------------------------------------------- - # 1) SUCCEEDED → accept only if really at tail *and* - # nothing remains in the global list - #----------------------------------------------------------- - if status == TaskResult.SUCCEEDED: - at_tail = (self._dist(robot, window[-1]) <= self.advance_tol) - if at_tail and not remaining: - self.navigator.get_logger().info("Trajectory finished") - break # all done + # Continuous streaming loop + while rclpy.ok(): + rclpy.spin_once(self.navigator, timeout_sec=0.05) + + if self.navigator.isTaskComplete(): + status = self.navigator.getResult() + + goal_handle = _get_goal_handle(self.navigator) + current_id = _get_current_uuid(self.navigator) + + # If we can’t identify the result reliably, treat it as current + if goal_handle is None or current_id is None: + result_is_current = True else: - # Success was premature → resend the unreached tail - unreached = [p for p in window - if self._dist(robot, p) > self.advance_tol] - if not unreached: - # corner-case: we *are* at tail but still have - # poses in ‘remaining’ → just continue, the normal - # sliding-window logic will pick them up + result_id = UUID(bytes=goal_handle.goal_id.uuid) + result_is_current = (result_id == UUID(bytes=current_id)) + + # Stale result from the goal we just pre-empted + if not result_is_current: + continue + + # Status handling + robot = self._get_robot_pose() + + # 1) SUCCEEDED: accept only if really at tail and nothing remains in the global list + if status == TaskResult.SUCCEEDED: + at_tail = (self._dist(robot, window[-1]) <= self.advance_tol) + if at_tail and not remaining: + self.navigator.get_logger().info("Trajectory finished") + break + else: + # Success was premature: resend the unreached tail + unreached = [p for p in window + if self._dist(robot, p) > self.advance_tol] + if not unreached: + # corner-case: we are at tail but still have poses in ‘remaining’. + # Continue; the normal sliding-window logic will pick them up. + continue + retry_window = [robot] + unreached + self.navigator.get_logger().warn( + f"Premature SUCCEEDED - retrying {len(unreached)} " + f"poses of current segment") + self._publish_waypoints(retry_window, seg_idx) + self.navigator.goThroughPoses(retry_window) + window = retry_window continue - retry_window = [robot] + unreached - self.navigator.get_logger().warn( - f"Premature SUCCEEDED - retrying {len(unreached)} " - f"poses of current segment") - self._publish_waypoints(retry_window, seg_idx) # same colour - self.navigator.goThroughPoses(retry_window) - window = retry_window # keep tracking it - continue # do **not** touch `remaining` - - #----------------------------------------------------------- - # 2) FAILED or CANCELLED → one retry - #----------------------------------------------------------- - if status in (TaskResult.FAILED, TaskResult.CANCELED): - if self._retries_left > 0: - self._retries_left -= 1 - unreached = [p for p in window - if self._dist(robot, p) > self.advance_tol] - if not unreached: + + # 2) FAILED or CANCELLED: Retry the segment + if status in (TaskResult.FAILED, TaskResult.CANCELED): + if self._retries_left > 0: + self._retries_left -= 1 + unreached = [p for p in window + if self._dist(robot, p) > self.advance_tol] + if not unreached: + self.navigator.get_logger().error( + "Retry requested but no poses remain in window") + break + retry_window = [robot] + unreached + self.navigator.get_logger().warn( + f"Segment {seg_idx+1} {status.name.lower()} - " + f"retrying {len(unreached)} poses") + self._publish_waypoints(retry_window, seg_idx) + self.navigator.goThroughPoses(retry_window) + window = retry_window + continue + else: self.navigator.get_logger().error( - "Retry requested but no poses remain in window") + "Segment failed twice - aborting") break - retry_window = [robot] + unreached - self.navigator.get_logger().warn( - f"Segment {seg_idx+1} {status.name.lower()} - " - f"retrying {len(unreached)} poses") - self._publish_waypoints(retry_window, seg_idx) - self.navigator.goThroughPoses(retry_window) - window = retry_window - continue - else: - self.navigator.get_logger().error( - "Segment failed twice - aborting") - break - # Live pose - robot = self._get_robot_pose() + # Live pose + robot = self._get_robot_pose() - # Are we close to the window’s tail? - if self._dist(robot, window[-1]) > self.advance_tol: - continue # still driving inside current window - - # Slide the window forward - tail = window[-1] # end-pose of current window + # Assess whether we are close to the window’s tail + if self._dist(robot, window[-1]) > self.advance_tol: + continue + + # Slide the window forward + tail = window[-1] - # -------- overlap: walk *backwards* inside the old window -------- - dist_rt = self._dist(robot, tail) # robot → tail + # Overlap: walk backwards inside the old window + dist_rt = self._dist(robot, tail) - overlap = [] - for p in reversed(window): - if (self._dist(p, tail) <= self.overlap_tol and # close to tail - self._dist(robot, p) <= dist_rt + 1e-6): # not behind robot - overlap.append(p) - else: - break - overlap.reverse() + overlap = [] + for p in reversed(window): + if (self._dist(p, tail) <= self.overlap_tol and # close to tail + self._dist(robot, p) <= dist_rt + 1e-6): # not behind robot + overlap.append(p) + else: + break + overlap.reverse() - if not remaining: - continue # path finished; let Nav2 coast to the end + if not remaining: + continue # path finished; let Nav2 coast to the end - # -------- build the forward part of the new window --------------- - forward = self._make_window(tail, remaining)[1:] # skip dup ‘tail’ - new_window = overlap + forward + # Build the forward part of the new window + forward = self._make_window(tail, remaining)[1:] # skip duplicate ‘tail’ + new_window = overlap + forward - # drop from ‘remaining’ exactly the poses we just attached - remaining = remaining[len(forward):] - - # log stats **before** we cancel / send - self._visited_wps += len(forward) # advance tally - self._log_segment(new_window, - overlap_cnt=len(overlap), - seg_idx =seg_idx) - - # # Pre-empt the running action - # self.navigator.cancelTask() - # # ─── Wait until the cancellation is fully processed ─── - # cancel_deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( - # seconds=3.0) # plenty – normally < 0.3 s - # while not self.navigator.isTaskComplete(): - # if self.navigator.get_clock().now() > cancel_deadline: - # self.navigator.get_logger().error("Cancel timeout – aborting") - # return - # rclpy.spin_once(self.navigator, timeout_sec=0.05) + # Drop from ‘remaining’ exactly the poses we just attached + remaining = remaining[len(forward):] - # # ───────────────────────────────────────────────────────────── - # # Extra guard: wait until internal follow_path is cancelled - # # ───────────────────────────────────────────────────────────── - # if self._fp_client is None: - # self._fp_client = ActionClient(self.navigator, FollowPath, "follow_path") - # if not self._fp_client.wait_for_server(timeout_sec=2.0): - # self.navigator.get_logger().warn("follow_path server not available") - - # fp_deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( - # seconds=3.0) - # while True: - # goal_handles = self._fp_client._goal_handles # internal list - # if not goal_handles: - # break # nothing active - # status = goal_handles[0].status - # if status in (GoalStatus.STATUS_SUCCEEDED, GoalStatus.STATUS_CANCELED): - # break - # if self.navigator.get_clock().now() > fp_deadline: - # self.navigator.get_logger().error("follow_path cancel timeout") - # return - # rclpy.spin_once(self.navigator, timeout_sec=0.05) - - self._publish_waypoints(new_window, seg_idx) - self.navigator.goThroughPoses(new_window) - self._retries_left = self.max_retries - seg_idx += 1 - window = new_window + # Log stats before cancel / send goal + self._visited_wps += len(forward) + self._log_segment(new_window, + overlap_cnt=len(overlap), + seg_idx =seg_idx) + + # # Pre-empt the running action + # self.navigator.cancelTask() + # # Wait until the cancellation is fully processed + # cancel_deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( + # seconds=3.0) # plenty - normally < 0.3 s + # while not self.navigator.isTaskComplete(): + # if self.navigator.get_clock().now() > cancel_deadline: + # self.navigator.get_logger().error("Cancel timeout - aborting") + # return + # rclpy.spin_once(self.navigator, timeout_sec=0.05) + + # # Extra guard: wait until internal follow_path is cancelled + # if self._fp_client is None: + # self._fp_client = ActionClient(self.navigator, FollowPath, "follow_path") + # if not self._fp_client.wait_for_server(timeout_sec=2.0): + # self.navigator.get_logger().warn("follow_path server not available") + + # fp_deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( + # seconds=3.0) + # while True: + # goal_handles = self._fp_client._goal_handles # internal list + # if not goal_handles: + # break # nothing active + # status = goal_handles[0].status + # if status in (GoalStatus.STATUS_SUCCEEDED, GoalStatus.STATUS_CANCELED): + # break + # if self.navigator.get_clock().now() > fp_deadline: + # self.navigator.get_logger().error("follow_path cancel timeout") + # return + # rclpy.spin_once(self.navigator, timeout_sec=0.05) + + self._publish_waypoints(new_window, seg_idx) + self.navigator.goThroughPoses(new_window) + self._retries_left = self.max_retries + seg_idx += 1 + window = new_window def _odom_cb(self, msg: Odometry) -> None: """Cache the most recent odometry pose as a `PoseStamped`.""" @@ -556,6 +400,7 @@ def _get_robot_pose(self) -> PoseStamped: return self._last_odom_pose def _publish_waypoints(self, poses: list[PoseStamped], seg_idx: int): + """Publish the current segment as a Path and MarkerArray.""" # Path message path = Path() path.header.frame_id = "map" @@ -575,11 +420,10 @@ def _publish_waypoints(self, poses: list[PoseStamped], seg_idx: int): hue = (seg_idx * 0.18) % 1.0 # golden-ratio hop around the circle r, g, b = _hsv_to_rgb(hue, 1.0, 1.0) line.color.r, line.color.g, line.color.b, line.color.a = r, g, b, 1.0 - # copy points + for p in poses: line.points.append(p.pose.position) - - self.waypoint_path_pub.publish(path) # still publish for tooling + self.waypoint_path_pub.publish(path) self.waypoint_marker_pub.publish(MarkerArray(markers=[line])) # Optional coloured spheres @@ -598,10 +442,12 @@ def _publish_waypoints(self, poses: list[PoseStamped], seg_idx: int): self.waypoint_marker_pub.publish(markers) def _dist(self, a: PoseStamped, b: PoseStamped) -> float: + """Return the Euclidean distance between two PoseStamped messages.""" dx, dy = a.pose.position.x - b.pose.position.x, a.pose.position.y - b.pose.position.y return math.hypot(dx, dy) def _path_length(self, poses: list[PoseStamped]) -> float: + """Return the total length of the path defined by the poses.""" return sum(self._dist(a, b) for a, b in zip(poses[:-1], poses[1:])) def _make_window(self, start: PoseStamped, todo: list[PoseStamped]) -> list[PoseStamped]: @@ -620,11 +466,13 @@ def _make_window(self, start: PoseStamped, todo: list[PoseStamped]) -> list[Pose return window def _segment_length(self, poses: list[PoseStamped]) -> float: + """Return the length of the segment defined by the poses.""" return sum(self._dist(a, b) for a, b in zip(poses[:-1], poses[1:])) def _log_segment(self, window, *, overlap_cnt: int, seg_idx: int) -> None: + """Log the segment statistics.""" seg_len = self._segment_length(window) num_wps = len(window) progress = self._visited_wps @@ -671,6 +519,7 @@ def _publish_full_traj(self, poses: list[PoseStamped]): markers=[line] + dots.markers)) def _closest_wp_index(self, robot: PoseStamped, poses: list[PoseStamped]) -> int: + """Compute the index of the closest waypoint to the robot.""" d_min, idx_min = float("inf"), 0 for i, p in enumerate(poses): d = self._dist(robot, p) @@ -680,12 +529,14 @@ def _closest_wp_index(self, robot: PoseStamped, poses: list[PoseStamped]) -> int def main(argv: list[str] | None = None): + """Main entry point for the GPS waypoint follower.""" rclpy.init(args=argv) parser = argparse.ArgumentParser(description="Follow GPS waypoints from a YAML file") parser.add_argument("yaml_file", nargs="?", default=None, help="Path to the YAML waypoints file") parser.add_argument("--last", action="store_true", help="Load the most recent waypoints file in the config directory") parser.add_argument("--reverse", action="store_true", help="Follow the trajectory in reverse order (adds 180° to yaw)") + parser.add_argument("--loop", type=int, default=1, help="Number of times to loop through the waypoints (default: 1)") args = parser.parse_args() try: @@ -696,9 +547,8 @@ def main(argv: list[str] | None = None): # Feedback to the user print(f"Loading waypoints file: {yaml_file_path}") - try: - gps_wpf = GpsWpCommander(yaml_file_path, reverse=args.reverse) + gps_wpf = GpsWpCommander(yaml_file_path, reverse=args.reverse, num_loop=args.loop) gps_wpf.start_ntp() except Exception as exc: print(f"[ERROR] {exc}", file=sys.stderr) diff --git a/src/nav2_tutorial/src/utils/parser_utils.py b/src/nav2_tutorial/src/utils/parser_utils.py new file mode 100644 index 0000000..2c12aa7 --- /dev/null +++ b/src/nav2_tutorial/src/utils/parser_utils.py @@ -0,0 +1,141 @@ +import os +import glob +import yaml +import argparse +from typing import List, Tuple +from src.utils.gps_utils import latLonYaw2Geopose +from ament_index_python.packages import get_package_share_directory + + +class YamlWaypointParser: + """Parse a set of GPS waypoints from a YAML file.""" + + def __init__(self, wps_file_path: str): + if not os.path.isfile(wps_file_path): + raise FileNotFoundError(wps_file_path) + + try: + with open(wps_file_path, "r") as wps_file: + self.wps_dict = yaml.safe_load(wps_file) or {} + except yaml.YAMLError as err: + raise RuntimeError(f"Failed to parse YAML file '{wps_file_path}': {err}") + + if "waypoints" not in self.wps_dict: + raise KeyError(f"Key 'waypoints' missing from YAML file '{wps_file_path}'.") + + @staticmethod + def _reverse_yaw(yaw: float) -> float: + """Return yaw rotated by pi, wrapped to [-pi, pi].""" + yaw += math.pi + return (yaw + math.pi) % (2.0 * math.pi) - math.pi + + @staticmethod + def _dedup(wps: List[dict]) -> List[dict]: + """Filter duplicate waypoints that are closer than ~1 cm.""" + if not wps: + return [] + cleaned = [wps[0]] + for wp in wps[1:]: + dx = wp["latitude"] - cleaned[-1]["latitude"] + dy = wp["longitude"] - cleaned[-1]["longitude"] + if abs(dx) > 1e-8 or abs(dy) > 1e-8: # ~1 cm in lat/lon + cleaned.append(wp) + return cleaned + + def get_wps(self, reverse: bool = False) -> List[dict]: + """Return a list of ``GeoPose`` objects, possibly in reverse order.""" + waypoints = self.wps_dict["waypoints"][::-1] if reverse else self.wps_dict["waypoints"] + waypoints = self._dedup(waypoints) + + + + + geopose_wps = [] + for wp in waypoints: + yaw = self._reverse_yaw(wp["yaw"]) if reverse else wp["yaw"] + geopose_wps.append(latLonYaw2Geopose(wp["latitude"], wp["longitude"], yaw)) + return geopose_wps + + +def _latest_file(directory: str, pattern: str = "gps_waypoints_2*.yaml") -> str | None: + """Return the lexically newest file matching *pattern* inside *directory*. + + The files produced by *terminal_logger.py* embed a timestamp in their name + (e.g. ``gps_waypoints_20250317_142311.yaml``) so lexical order === + chronological order. If *directory* does not exist or contains no matching + files, ``None`` is returned. + """ + if not os.path.isdir(directory): + return None + files = glob.glob(os.path.join(directory, pattern)) + if not files: + return None + files.sort() + return files[-1] + + +def _resolve_ws_paths() -> tuple[str, str]: + """Return ``(src_traj_dir, share_traj_dir)`` for *nav2_tutorial*. + + *share_traj_dir* is the directory installed by ``ament`` (typically + ``install/share/nav2_tutorial/trajectories``) while *src_traj_dir* points to + the editable source tree (``/src/nav2_tutorial/trajectories``). + """ + share_dir = get_package_share_directory("nav2_tutorial") + share_traj_dir = os.path.join(share_dir, "trajectories") + + # share/nav2_tutorial -> share -> install -> + ws_root = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(share_dir)))) + src_traj_dir = os.path.join(ws_root, "src", "nav2_tutorial", "trajectories") + return src_traj_dir, share_traj_dir + + +def _select_yaml(args: argparse.Namespace) -> str: + """Figure out which YAML file we should load. + + Priority: + 1. If the user passed an explicit path, use it. + 2. If ``--last`` was requested, compare the newest file from the *src* and + *share* directories and choose the newer one (with a preference for the + *src* file if both have identical timestamps). + 3. Otherwise, attempt to load ``gps_waypoints.yaml`` from *src* first then + *share*. + + A *FileNotFoundError* is raised if no suitable file is found so that the + caller can decide how to handle the error. + """ + src_traj_dir, share_traj_dir = _resolve_ws_paths() + + # 1) Explicit path + if args.yaml_file is not None: + explicit = os.path.expanduser(args.yaml_file) + if not os.path.isfile(explicit): + raise FileNotFoundError(explicit) + return explicit + + # 2) --last flag + if args.last: + src_latest = _latest_file(src_traj_dir) + share_latest = _latest_file(share_traj_dir) + + if src_latest and share_latest: + # Compare by timestamp embedded in filename (lexical order) or mtime + chosen = src_latest if os.path.basename(src_latest) > os.path.basename(share_latest) else share_latest + else: + chosen = src_latest or share_latest # whichever is not None (could still be None) + + if chosen is None: + raise FileNotFoundError(f"No waypoint files matching 'gps_waypoints_2*.yaml' found in '{src_traj_dir}' or '{share_traj_dir}'.") + return chosen + + # 3) Default lookup "gps_waypoints.yaml" + default_name = "gps_waypoints.yaml" + candidate_src = os.path.join(src_traj_dir, default_name) + if os.path.isfile(candidate_src): + return candidate_src + + candidate_share = os.path.join(share_traj_dir, default_name) + if os.path.isfile(candidate_share): + return candidate_share + + raise FileNotFoundError(f"Default waypoint file '{default_name}' not found in '{src_traj_dir}' or '{share_traj_dir}'.") From ca5f87245443016a3f9285824787ac65ba1941cc Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 9 Jul 2025 17:38:50 +0200 Subject: [PATCH 11/23] Further cleanup of followers --- src/nav2_tutorial/src/smooth_wp_follower.py | 190 +++++++------------- src/nav2_tutorial/src/utils/nav_utils.py | 31 ++++ src/nav2_tutorial/src/utils/parser_utils.py | 78 ++++++-- 3 files changed, 161 insertions(+), 138 deletions(-) create mode 100644 src/nav2_tutorial/src/utils/nav_utils.py diff --git a/src/nav2_tutorial/src/smooth_wp_follower.py b/src/nav2_tutorial/src/smooth_wp_follower.py index 5f0790f..cb69fc1 100644 --- a/src/nav2_tutorial/src/smooth_wp_follower.py +++ b/src/nav2_tutorial/src/smooth_wp_follower.py @@ -10,8 +10,7 @@ from rclpy.action import ActionClient from rclpy.qos import qos_profile_sensor_data from uuid import UUID -from pyproj import Transformer -from src.utils.gps_utils import quaternion_from_euler +from src.utils.nav_utils import _hsv_to_rgb, _get_goal_handle, _get_current_uuid from src.utils.parser_utils import YamlWaypointParser, _latest_file, _resolve_ws_paths, _select_yaml # ROS messages @@ -28,71 +27,27 @@ MARKER_VIZ = True -def _hsv_to_rgb(h, s=1.0, v=1.0): - """HSV to RGB converter.""" - i = int(h*6.0) % 6 - f = h*6.0 - i - p, q, t = v*(1-s), v*(1-f*s), v*(1-(1-f)*s) - if i == 0: r,g,b = v,t,p - elif i == 1: r,g,b = q,v,p - elif i == 2: r,g,b = p,v,t - elif i == 3: r,g,b = p,q,v - elif i == 4: r,g,b = t,p,v - else: r,g,b = v,p,q - return r, g, b - - -def _get_goal_handle(nav): - """Return the ActionGoal handle of the latest NavigateThroughPoses goal.""" - for attr in ("_ntp_goal_handle", # Iron, Jazzy - "_nav_through_poses_goal_handle" # Humble, Galactic - ): - if hasattr(nav, attr): - return getattr(nav, attr) - return None - -def _get_current_uuid(nav): - """Return the UUID (bytes) of the goal that is currently active.""" - for attr in ("_current_nav_through_poses_uuid", # Humble - "_current_ntp_uuid" # Iron/Jazzy - ): - if hasattr(nav, attr): - return getattr(nav, attr) - return None - - class GpsWpCommander: """Follow a smooth sequence of GPS waypoints.""" def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1): self.navigator = BasicNavigator("basic_navigator") - self.wp_parser = YamlWaypointParser(wps_file_path) self.reverse = reverse + self.num_loop = num_loop # Odometry topic with ENU pose in map frame self._last_odom_pose: PoseStamped | None = None self.navigator.create_subscription(Odometry, "/fixposition/odometry_enu", self._odom_cb, 10) - # Get datum - _base_lat, _base_lon, _base_alt = self._get_datum() - - # Transform from WGS-84 (lat, lon, h) to ENU (e, n, u) in the map frame - enu_pipeline = ( - "+proj=pipeline " - "+step +proj=unitconvert +xy_in=deg +xy_out=rad " - "+step +proj=axisswap +order=2,1,3 " # lon,lat order - "+step +proj=cart +ellps=WGS84 " - f"+step +proj=topocentric +ellps=WGS84 " - f" +lat_0={_base_lat} +lon_0={_base_lon} +h_0={_base_alt} " - ) - self._tf_llh2enu = Transformer.from_pipeline(enu_pipeline) - - # Parameters for the NavigateThroughPoses task + # Initialize waypoint parser + base_lat, base_lon, base_alt = self._get_datum() + self.wp_parser = YamlWaypointParser(wps_file_path, base_lat, base_lon, base_alt) + + # Parameters for the GoThroughPoses task self.odom_timeout_s = 2.0 # wait this long for first /fixposition msg self.max_retries = 1 # re-attempt each failed segment once self._retries_left = self.max_retries self._last_goal_uuid: UUID | None = None - self.num_loop = num_loop # Sliding-window parameters # For long mission: seg_len_max = 9.0, advance_tol = 3.60, overlap_tol = 1.30 @@ -150,30 +105,13 @@ def _cb(msg: NavSatFix): self.navigator.destroy_subscription(sub) return tuple(datum) - def _latlon_to_pose(self, lat: float, lon: float, yaw: float) -> PoseStamped: - """Convert latitude, longitude, and yaw to a PoseStamped in the map frame.""" - x, y, _ = self._tf_llh2enu.transform(lat, lon, 0.0) - pose = PoseStamped() - pose.header.frame_id = "map" - pose.header.stamp = rclpy.time.Time().to_msg() - pose.pose.position.x = x - pose.pose.position.y = y - pose.pose.position.z = 0.0 - pose.pose.orientation = quaternion_from_euler(0.0, 0.0, yaw) - return pose - def start_ntp(self): - """Start the NavigateThroughPoses task with the waypoints.""" + """Start the GoThroughPoses task with the waypoints.""" # Wait for the robot_localization node to be active self.navigator.waitUntilNav2Active(localizer="robot_localization") # Convert the whole YAML path once - all_poses = [ - self._latlon_to_pose(wp["latitude"], wp["longitude"], - self.wp_parser._reverse_yaw(wp["yaw"]) if self.reverse else wp["yaw"]) - for wp in (self.wp_parser.wps_dict["waypoints"][::-1] if self.reverse else - self.wp_parser.wps_dict["waypoints"]) - ] + all_poses = self.wp_parser.get_wps(reverse=self.reverse) # Publish the full trajectory for visualization self._publish_full_traj(all_poses) @@ -222,8 +160,8 @@ def start_ntp(self): self._visited_wps += new_wps self._log_segment(window, - overlap_cnt=0, - seg_idx =seg_idx) + overlap_cnt = 0, + seg_idx = seg_idx) self._publish_waypoints(window, seg_idx) self.navigator.goThroughPoses(window) @@ -335,8 +273,8 @@ def start_ntp(self): # Log stats before cancel / send goal self._visited_wps += len(forward) self._log_segment(new_window, - overlap_cnt=len(overlap), - seg_idx =seg_idx) + overlap_cnt = len(overlap), + seg_idx = seg_idx) # # Pre-empt the running action # self.navigator.cancelTask() @@ -399,48 +337,6 @@ def _get_robot_pose(self) -> PoseStamped: return self._last_odom_pose - def _publish_waypoints(self, poses: list[PoseStamped], seg_idx: int): - """Publish the current segment as a Path and MarkerArray.""" - # Path message - path = Path() - path.header.frame_id = "map" - path.header.stamp = rclpy.time.Time().to_msg() - path.poses = poses - - # RViz colours Paths per-topic, not per-message → we publish a Marker - # LINE_STRIP instead (same header) so each segment gets its own hue. - line = Marker() - line.header = path.header - line.ns = "seg_line" - line.id = seg_idx # unique id per segment - line.type = Marker.LINE_STRIP - line.action = Marker.ADD - line.pose.orientation.w = 1.0 # identity - line.scale.x = 0.03 # line width - hue = (seg_idx * 0.18) % 1.0 # golden-ratio hop around the circle - r, g, b = _hsv_to_rgb(hue, 1.0, 1.0) - line.color.r, line.color.g, line.color.b, line.color.a = r, g, b, 1.0 - - for p in poses: - line.points.append(p.pose.position) - self.waypoint_path_pub.publish(path) - self.waypoint_marker_pub.publish(MarkerArray(markers=[line])) - - # Optional coloured spheres - markers = MarkerArray() - for i, p in enumerate(poses): - m = Marker() - m.header = path.header - m.ns = "wps" - m.id = seg_idx*1000 + i - m.type = Marker.SPHERE - m.action = Marker.ADD - m.pose = p.pose - m.scale.x = m.scale.y = m.scale.z = 0.05 - m.color.r, m.color.g, m.color.b, m.color.a = r, g, b, 1.0 - markers.markers.append(m) - self.waypoint_marker_pub.publish(markers) - def _dist(self, a: PoseStamped, b: PoseStamped) -> float: """Return the Euclidean distance between two PoseStamped messages.""" dx, dy = a.pose.position.x - b.pose.position.x, a.pose.position.y - b.pose.position.y @@ -468,6 +364,15 @@ def _make_window(self, start: PoseStamped, todo: list[PoseStamped]) -> list[Pose def _segment_length(self, poses: list[PoseStamped]) -> float: """Return the length of the segment defined by the poses.""" return sum(self._dist(a, b) for a, b in zip(poses[:-1], poses[1:])) + + def _closest_wp_index(self, robot: PoseStamped, poses: list[PoseStamped]) -> int: + """Compute the index of the closest waypoint to the robot.""" + d_min, idx_min = float("inf"), 0 + for i, p in enumerate(poses): + d = self._dist(robot, p) + if d < d_min: + d_min, idx_min = d, i + return idx_min def _log_segment(self, window, *, overlap_cnt: int, @@ -482,7 +387,49 @@ def _log_segment(self, window, f"Num. waypoints: {num_wps}, " f"Overlap: {overlap_cnt}, " f"Progress: {progress}/{self._total_wps}") + + def _publish_waypoints(self, poses: list[PoseStamped], seg_idx: int): + """Publish the current segment as a Path and MarkerArray.""" + # Path message + path = Path() + path.header.frame_id = "map" + path.header.stamp = rclpy.time.Time().to_msg() + path.poses = poses + # RViz colours Paths per-topic, not per-message → we publish a Marker + # LINE_STRIP instead (same header) so each segment gets its own hue. + line = Marker() + line.header = path.header + line.ns = "seg_line" + line.id = seg_idx # unique id per segment + line.type = Marker.LINE_STRIP + line.action = Marker.ADD + line.pose.orientation.w = 1.0 # identity + line.scale.x = 0.03 # line width + hue = (seg_idx * 0.18) % 1.0 # golden-ratio hop around the circle + r, g, b = _hsv_to_rgb(hue, 1.0, 1.0) + line.color.r, line.color.g, line.color.b, line.color.a = r, g, b, 1.0 + + for p in poses: + line.points.append(p.pose.position) + self.waypoint_path_pub.publish(path) + self.waypoint_marker_pub.publish(MarkerArray(markers=[line])) + + # Optional coloured spheres + markers = MarkerArray() + for i, p in enumerate(poses): + m = Marker() + m.header = path.header + m.ns = "wps" + m.id = seg_idx*1000 + i + m.type = Marker.SPHERE + m.action = Marker.ADD + m.pose = p.pose + m.scale.x = m.scale.y = m.scale.z = 0.05 + m.color.r, m.color.g, m.color.b, m.color.a = r, g, b, 1.0 + markers.markers.append(m) + self.waypoint_marker_pub.publish(markers) + def _publish_full_traj(self, poses: list[PoseStamped]): """Publish the whole trajectory as a latched Path + MarkerArray.""" path = Path() @@ -517,15 +464,6 @@ def _publish_full_traj(self, poses: list[PoseStamped]): self.full_traj_mrk_pub.publish(MarkerArray( markers=[line] + dots.markers)) - - def _closest_wp_index(self, robot: PoseStamped, poses: list[PoseStamped]) -> int: - """Compute the index of the closest waypoint to the robot.""" - d_min, idx_min = float("inf"), 0 - for i, p in enumerate(poses): - d = self._dist(robot, p) - if d < d_min: - d_min, idx_min = d, i - return idx_min def main(argv: list[str] | None = None): diff --git a/src/nav2_tutorial/src/utils/nav_utils.py b/src/nav2_tutorial/src/utils/nav_utils.py new file mode 100644 index 0000000..8f73fa6 --- /dev/null +++ b/src/nav2_tutorial/src/utils/nav_utils.py @@ -0,0 +1,31 @@ +def _hsv_to_rgb(h, s=1.0, v=1.0): + """HSV to RGB converter.""" + i = int(h*6.0) % 6 + f = h*6.0 - i + p, q, t = v*(1-s), v*(1-f*s), v*(1-(1-f)*s) + if i == 0: r,g,b = v,t,p + elif i == 1: r,g,b = q,v,p + elif i == 2: r,g,b = p,v,t + elif i == 3: r,g,b = p,q,v + elif i == 4: r,g,b = t,p,v + else: r,g,b = v,p,q + return r, g, b + + +def _get_goal_handle(nav): + """Return the ActionGoal handle of the latest NavigateThroughPoses goal.""" + for attr in ("_ntp_goal_handle", # Iron, Jazzy + "_nav_through_poses_goal_handle" # Humble, Galactic + ): + if hasattr(nav, attr): + return getattr(nav, attr) + return None + +def _get_current_uuid(nav): + """Return the UUID (bytes) of the goal that is currently active.""" + for attr in ("_current_nav_through_poses_uuid", # Humble + "_current_ntp_uuid" # Iron/Jazzy + ): + if hasattr(nav, attr): + return getattr(nav, attr) + return None \ No newline at end of file diff --git a/src/nav2_tutorial/src/utils/parser_utils.py b/src/nav2_tutorial/src/utils/parser_utils.py index 2c12aa7..24c19d1 100644 --- a/src/nav2_tutorial/src/utils/parser_utils.py +++ b/src/nav2_tutorial/src/utils/parser_utils.py @@ -1,16 +1,32 @@ import os +import copy import glob import yaml +import rclpy import argparse +from pyproj import Transformer from typing import List, Tuple -from src.utils.gps_utils import latLonYaw2Geopose +from geometry_msgs.msg import PoseStamped +from src.utils.gps_utils import quaternion_from_euler, latLonYaw2Geopose from ament_index_python.packages import get_package_share_directory class YamlWaypointParser: """Parse a set of GPS waypoints from a YAML file.""" - def __init__(self, wps_file_path: str): + def __init__(self, wps_file_path: str, base_lat: float = 0.0, + base_lon: float = 0.0, base_alt: float = 0.0): + """Initialize the parser with a YAML file path and base coordinates. + Args: + wps_file_path (str): Path to the YAML file containing waypoints. + base_lat (float): Latitude of the base point for ENU transformation. + base_lon (float): Longitude of the base point for ENU transformation. + base_alt (float): Altitude of the base point for ENU transformation. + Raises: + FileNotFoundError: If the specified YAML file does not exist. + KeyError: If the 'waypoints' key is missing in the YAML file. + RuntimeError: If the YAML file cannot be parsed. + """ if not os.path.isfile(wps_file_path): raise FileNotFoundError(wps_file_path) @@ -22,6 +38,27 @@ def __init__(self, wps_file_path: str): if "waypoints" not in self.wps_dict: raise KeyError(f"Key 'waypoints' missing from YAML file '{wps_file_path}'.") + + # Raise warning if base coordinates are not set + if base_lat == 0.0 and base_lon == 0.0 and base_alt == 0.0: + print("Warning: Base coordinates are set to (0.0, 0.0, 0.0). " + "This may lead to incorrect ENU transformations. " + "Please set appropriate base coordinates for your map frame.") + + # Transform from WGS-84 (lat, lon, h) to ENU (e, n, u) in the map frame + self._update_base(base_lat, base_lon, base_alt) + + def _update_base(self, base_lat: float, base_lon: float, base_alt: float): + """Update the ENU transformation base point.""" + enu_pipeline = ( + "+proj=pipeline " + "+step +proj=unitconvert +xy_in=deg +xy_out=rad " + "+step +proj=axisswap +order=2,1,3 " # lon,lat order + "+step +proj=cart +ellps=WGS84 " + f"+step +proj=topocentric +ellps=WGS84 " + f" +lat_0={base_lat} +lon_0={base_lon} +h_0={base_alt} " + ) + self._tf_llh2enu = Transformer.from_pipeline(enu_pipeline) @staticmethod def _reverse_yaw(yaw: float) -> float: @@ -41,20 +78,37 @@ def _dedup(wps: List[dict]) -> List[dict]: if abs(dx) > 1e-8 or abs(dy) > 1e-8: # ~1 cm in lat/lon cleaned.append(wp) return cleaned + + def _latlon_to_pose(self, lat: float, lon: float, yaw: float) -> PoseStamped: + """Convert latitude, longitude, and yaw to a PoseStamped in the map frame.""" + x, y, _ = self._tf_llh2enu.transform(lat, lon, 0.0) + pose = PoseStamped() + pose.header.frame_id = "map" + pose.header.stamp = rclpy.time.Time().to_msg() + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = 0.0 + pose.pose.orientation = quaternion_from_euler(0.0, 0.0, yaw) + return pose def get_wps(self, reverse: bool = False) -> List[dict]: - """Return a list of ``GeoPose`` objects, possibly in reverse order.""" - waypoints = self.wps_dict["waypoints"][::-1] if reverse else self.wps_dict["waypoints"] + """Return a list of waypoints as PoseStamped objects. + Args: + reverse (bool): If True, reverse the order of waypoints. + Returns: + List[dict]: A list of waypoints as PoseStamped objects. + """ + # Extract waypoints from the loaded YAML dictionary + waypoints = copy.deepcopy(self.wps_dict["waypoints"]) + if reverse: + waypoints.reverse() + for wp in waypoints: + wp["yaw"] = self._reverse_yaw(wp["yaw"]) + + # Filter out duplicate waypoints waypoints = self._dedup(waypoints) - - - - geopose_wps = [] - for wp in waypoints: - yaw = self._reverse_yaw(wp["yaw"]) if reverse else wp["yaw"] - geopose_wps.append(latLonYaw2Geopose(wp["latitude"], wp["longitude"], yaw)) - return geopose_wps + return [self._latlon_to_pose(wp["latitude"], wp["longitude"], wp["yaw"]) for wp in waypoints] def _latest_file(directory: str, pattern: str = "gps_waypoints_2*.yaml") -> str | None: From 4e6f4eff9a7c5add35cb080517fa13f84febf231 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 10 Jul 2025 10:05:45 +0200 Subject: [PATCH 12/23] Further cleanup --- src/nav2_tutorial/src/precise_wp_follower.py | 393 ++++++++++++++----- src/nav2_tutorial/src/smooth_wp_follower.py | 94 ++--- src/nav2_tutorial/src/utils/nav_utils.py | 47 +++ 3 files changed, 378 insertions(+), 156 deletions(-) diff --git a/src/nav2_tutorial/src/precise_wp_follower.py b/src/nav2_tutorial/src/precise_wp_follower.py index cbf665f..9acd6eb 100644 --- a/src/nav2_tutorial/src/precise_wp_follower.py +++ b/src/nav2_tutorial/src/precise_wp_follower.py @@ -1,131 +1,342 @@ -#!/usr/bin/env python3 -from __future__ import annotations - -import argparse -import glob -import math -import os import sys -import yaml -from typing import List, Tuple +import argparse +from uuid import UUID +# ROS packages import rclpy -from ament_index_python.packages import get_package_share_directory +from rclpy.action import ActionClient +from rclpy.qos import qos_profile_sensor_data, QoSProfile, DurabilityPolicy from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult -from pyproj import Transformer -from src.utils.gps_utils import quaternion_from_euler -from src.utils.parser_utils import YamlWaypointParser, _latest_file, _resolve_ws_paths, _select_yaml -# ROS msgs +# ROS messages from sensor_msgs.msg import NavSatFix from geometry_msgs.msg import PoseStamped -from rclpy.qos import qos_profile_sensor_data +from action_msgs.msg import GoalStatus +from nav_msgs.msg import Path, Odometry +from nav2_msgs.action import FollowPath +from visualization_msgs.msg import Marker, MarkerArray + +# Custom utils +from src.utils.nav_utils import (_pose_dist, _path_length, _closest_wp_index, + _hsv_to_rgb, _get_goal_handle, _get_current_uuid) +from src.utils.parser_utils import YamlWaypointParser, _latest_file, _resolve_ws_paths, _select_yaml + +# Visualization +MARKER_VIZ = True class GpsWpCommander: - def __init__(self, yaml_path: str, *, reverse: bool = False): - self.navigator = BasicNavigator("basic_navigator") - self.wps = YamlWaypointParser(yaml_path).get_wps(reverse=reverse) - if not self.wps: - raise RuntimeError("Waypoint list is empty.") + """Follow a smooth sequence of GPS waypoints.""" - # Build ENU transformer --------------------------------------- + def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1): + self.navigator = BasicNavigator("basic_navigator") + self.reverse = reverse + self.num_loop = num_loop + + # Odometry topic with ENU pose in map frame + self._last_odom_pose: PoseStamped | None = None + self.navigator.create_subscription(Odometry, "/fixposition/odometry_enu", self._odom_cb, 10) + + # Initialize waypoint parser base_lat, base_lon, base_alt = self._get_datum() - enu_pipeline = ( - "+proj=pipeline " - "+step +proj=unitconvert +xy_in=deg +xy_out=rad " - "+step +proj=axisswap +order=2,1,3 " - "+step +proj=cart +ellps=WGS84 " - f"+step +proj=topocentric +ellps=WGS84 +lat_0={base_lat} +lon_0={base_lon} +h_0={base_alt} " - ) - self._tf_llh2enu = Transformer.from_pipeline(enu_pipeline) - - self.max_retries = 1 - - # --------------------- datum helper ------------------------------ - def _get_datum(self, topic: str = "/fixposition/datum", timeout_s: float = 3.0): - datum = None + self.wp_parser = YamlWaypointParser(wps_file_path, base_lat, base_lon, base_alt) + + # Parameters for the GoToPose task + self.odom_timeout_s = 2.0 # wait this long for first /fixposition msg + self.max_retries = 1 # re-attempts for each failed waypoint + self.start_seg_tol = 3.0 # m - max distance we “snap” to the path + self._retries_left = self.max_retries + self._last_goal_uuid: UUID | None = None + + # Statistics + self._total_wps = 0 + self._visited_wps = 0 + + # Marker visualization + if MARKER_VIZ: + latched_qos = QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + + # Path and MarkerArray for visualizing the full trajectory + self.full_traj_pub = self.navigator.create_publisher( + Path, "/gps_full_traj", latched_qos) + self.full_traj_mrk_pub = self.navigator.create_publisher( + MarkerArray, "/gps_full_traj_markers", latched_qos) + + def _get_datum(self, topic: str = "/fixposition/datum", + timeout_s: float = 3.0) -> tuple[float, float, float]: + """ + Block until a NavSatFix is received on topic (max timeout_s). + Returns (lat [deg], lon [deg], alt [m]). Raises RuntimeError on timeout. + """ + datum: list[float] | None = None + def _cb(msg: NavSatFix): nonlocal datum - datum = (msg.latitude, msg.longitude, msg.altitude) - sub = self.navigator.create_subscription(NavSatFix, topic, _cb, qos_profile_sensor_data) - deadline = self.navigator.get_clock().now() + rclpy.duration.Duration(seconds=timeout_s) + datum = [msg.latitude, msg.longitude, msg.altitude] + + sub = self.navigator.create_subscription( + NavSatFix, topic, _cb, qos_profile_sensor_data) + + deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( + seconds=timeout_s) + while rclpy.ok() and datum is None: + rclpy.spin_once(self.navigator, timeout_sec=0.05) if self.navigator.get_clock().now() > deadline: raise RuntimeError(f"No NavSatFix on {topic} within {timeout_s}s") - rclpy.spin_once(self.navigator, timeout_sec=0.05) + self.navigator.destroy_subscription(sub) - return datum + return tuple(datum) - # --------------------- conversion helper ------------------------- - def _latlon_to_pose(self, lat: float, lon: float, yaw: float) -> PoseStamped: - x, y, _ = self._tf_llh2enu.transform(lat, lon, 0.0) - ps = PoseStamped() - ps.header.frame_id = "map" - ps.header.stamp = rclpy.time.Time().to_msg() - ps.pose.position.x, ps.pose.position.y = x, y - ps.pose.position.z = 0.0 - ps.pose.orientation = quaternion_from_euler(0.0, 0.0, yaw) - return ps - - # --------------------- main loop --------------------------------- - def start(self): + def start_ntp(self): + """Start the GoToPose task with the waypoints.""" + # Wait for the robot_localization node to be active self.navigator.waitUntilNav2Active(localizer="robot_localization") - poses = [self._latlon_to_pose(wp["latitude"], wp["longitude"], wp["yaw"]) for wp in self.wps] - self.navigator.get_logger().info(f"Starting precise run with {len(poses)} waypoint(s)…") - - for idx, pose in enumerate(poses, start=1): - retries_left = self.max_retries - while True: - self.navigator.goToPose(pose) - # spin until done or shutdown - while rclpy.ok() and not self.navigator.isTaskComplete(): - rclpy.spin_once(self.navigator, timeout_sec=0.05) - result = self.navigator.getResult() - # -------------------------------------------------- SUCCEEDED - if result == TaskResult.SUCCEEDED: - self.navigator.get_logger().info(f"Waypoint {idx}/{len(poses)} reached") - break - # -------------------------------------------------- FAILED / CANCELED - if result in (TaskResult.FAILED, TaskResult.CANCELED): - if retries_left > 0: - retries_left -= 1 - self.navigator.get_logger().warn( - f"Waypoint {idx} {result.name.lower()} - retrying ({self.max_retries - retries_left}/{self.max_retries})") - continue - self.navigator.get_logger().error(f"Waypoint {idx} failed twice - aborting trajectory") - return - # -------------------------------------------------- other / unknown - self.navigator.get_logger().error(f"Unexpected TaskResult {result} - aborting") + + # Convert the whole YAML path once + all_poses = self.wp_parser.get_wps(reverse=self.reverse) + + # Publish the full trajectory for visualization + self._publish_full_traj(all_poses) + + # Loop through the waypoints + for loop_idx in range(self.num_loop): + if self.num_loop > 1: + self.navigator.get_logger().info( + f"Performing loop {loop_idx + 1} out of {self.num_loop}…") + + self._total_wps = len(all_poses) + total_len = _path_length(all_poses) + self.navigator.get_logger().info( + f"Preparing segments for trajectory of {total_len:0.2f} m " + f"consisting of {self._total_wps} waypoints…") + remaining = all_poses + + # Prime the first window + robot = self._get_robot_pose() + + if self.num_loop == 1: + # Fast-forward to the first pose within start_seg_tol + closest_idx = _closest_wp_index(robot, remaining) + if _pose_dist(robot, remaining[closest_idx]) > self.start_seg_tol: + closest_idx = 0 # fall back to the first wp + + if closest_idx: + self._visited_wps += closest_idx # statistics + skipped = closest_idx + remaining = remaining[closest_idx:] + self.navigator.get_logger().info( + f"Skipped {skipped} waypoint(s); " + f"starting with wp #{skipped+1}/{self._total_wps}") + + # If everything was within the tolerance, finish trajectory follower + if not remaining: + self.navigator.get_logger().info("Trajectory already completed.") return + + self._visited_wps += 1 + self._log_segment(window, + overlap_cnt = 0, + seg_idx = seg_idx) + self.navigator.goToPose(pose) + remaining = remaining[1:] + + # Continuous streaming loop + while rclpy.ok(): + rclpy.spin_once(self.navigator, timeout_sec=0.05) - self.navigator.get_logger().info("All waypoints completed successfully!") + if self.navigator.isTaskComplete(): + status = self.navigator.getResult() + goal_handle = _get_goal_handle(self.navigator) + current_id = _get_current_uuid(self.navigator) -def main(argv: List[str] | None = None): + # If we can’t identify the result reliably, treat it as current + if goal_handle is None or current_id is None: + result_is_current = True + else: + result_id = UUID(bytes=goal_handle.goal_id.uuid) + result_is_current = (result_id == UUID(bytes=current_id)) + + # Stale result from the goal we just pre-empted + if not result_is_current: + continue + + # Status handling + robot = self._get_robot_pose() + + # 1) SUCCEEDED: accept only if really at tail and nothing remains in the global list + if status == TaskResult.SUCCEEDED: + at_tail = (_pose_dist(robot, window[-1]) <= self.advance_tol) + if at_tail and not remaining: + self.navigator.get_logger().info("Trajectory finished") + break + else: + # Success was premature: resend the unreached tail + unreached = [p for p in window + if _pose_dist(robot, p) > self.advance_tol] + if not unreached: + # corner-case: we are at tail but still have poses in ‘remaining’. + # Continue; the normal sliding-window logic will pick them up. + continue + retry_window = [robot] + unreached + self.navigator.get_logger().warn( + f"Premature SUCCEEDED - retrying {len(unreached)} " + f"poses of current segment") + self.navigator.goToPose(retry_window) + window = retry_window + continue + + # 2) FAILED or CANCELLED: Retry the segment + if status in (TaskResult.FAILED, TaskResult.CANCELED): + if self._retries_left > 0: + self._retries_left -= 1 + unreached = [p for p in window + if _pose_dist(robot, p) > self.advance_tol] + if not unreached: + self.navigator.get_logger().error( + "Retry requested but no poses remain in window") + break + retry_window = [robot] + unreached + self.navigator.get_logger().warn( + f"Segment {seg_idx+1} {status.name.lower()} - " + f"retrying {len(unreached)} poses") + self.navigator.goToPose(retry_window) + window = retry_window + continue + else: + self.navigator.get_logger().error( + "Segment failed twice - aborting") + break + + # Live pose + robot = self._get_robot_pose() + + # Assess whether we are close to the window’s tail + if _pose_dist(robot, window[-1]) > self.advance_tol: + continue + + if not remaining: + continue # path finished; let Nav2 coast to the end + + # Drop from ‘remaining’ exactly the poses we just attached + pose = remaining[0] + remaining = remaining[1:] + + # Log stats before cancel / send goal + self._visited_wps += 1 + self._log_segment(new_window, + overlap_cnt = len(overlap), + seg_idx = seg_idx) + + self.navigator.goToPose(new_window) + self._retries_left = self.max_retries + seg_idx += 1 + window = new_window + + def _odom_cb(self, msg: Odometry) -> None: + """Cache the most recent odometry pose as a `PoseStamped`.""" + ps = PoseStamped() + ps.header = msg.header + ps.pose = msg.pose.pose + self._last_odom_pose = ps + + def _get_robot_pose(self) -> PoseStamped: + """ + Return the most recent pose from /fixposition/odometry_enu. + Blocks (spins) until a message arrives or the timeout elapses. + """ + deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( + seconds=self.odom_timeout_s) + + while self._last_odom_pose is None: + if self.navigator.get_clock().now() > deadline: + raise RuntimeError( + f"No Odometry on /fixposition/odometry_enu " + f"after {self.odom_timeout_s:.1f}s") + rclpy.spin_once(self.navigator, timeout_sec=0.05) + + return self._last_odom_pose + + def _log_segment(self, window, + *, overlap_cnt: int, + seg_idx: int) -> None: + """Log the segment statistics.""" + seg_len = _segment_length(window) + num_wps = len(window) + progress = self._visited_wps + + self.navigator.get_logger().info( + f"Segment {seg_idx+1} length: {seg_len:0.2f} m, " + f"Num. waypoints: {num_wps}, " + f"Overlap: {overlap_cnt}, " + f"Progress: {progress}/{self._total_wps}") + + def _publish_full_traj(self, poses: list[PoseStamped]): + """Publish the whole trajectory as a latched Path + MarkerArray.""" + path = Path() + path.header.frame_id = "map" + path.header.stamp = rclpy.time.Time().to_msg() + path.poses = poses + self.full_traj_pub.publish(path) + + # one violet line-strip + tiny white spheres at every waypoint + line = Marker() + line.header = path.header + line.ns = "full_traj" + line.id = 0 + line.type = Marker.LINE_STRIP + line.action = Marker.ADD + line.pose.orientation.w = 1.0 + line.scale.x = 0.04 + line.color.r, line.color.g, line.color.b, line.color.a = 0.7, 0.3, 1.0, 1.0 + line.points = [p.pose.position for p in poses] + + dots = MarkerArray() + for i, p in enumerate(poses): + m = Marker() + m.header = path.header + m.ns, m.id = "full_traj_pts", i + m.type, m.action = Marker.SPHERE, Marker.ADD + m.pose = p.pose + m.scale.x = m.scale.y = m.scale.z = 0.05 + m.color.r = m.color.g = m.color.b = 1.0 + m.color.a = 1.0 + dots.markers.append(m) + + self.full_traj_mrk_pub.publish(MarkerArray( + markers=[line] + dots.markers)) + + +def main(argv: list[str] | None = None): + """Main entry point for the GPS waypoint follower.""" rclpy.init(args=argv) + parser = argparse.ArgumentParser(description="Follow GPS waypoints from a YAML file") - parser.add_argument("yaml_file", nargs="?", default=None, help="Path to YAML waypoint file") - parser.add_argument("--last", action="store_true", help="Use the most recent waypoint file") - parser.add_argument("--reverse", action="store_true", help="Follow the path in reverse order (adds 180° yaw)") + parser.add_argument("yaml_file", nargs="?", default=None, help="Path to the YAML waypoints file") + parser.add_argument("--last", action="store_true", help="Load the most recent waypoints file in the config directory") + parser.add_argument("--reverse", action="store_true", help="Follow the trajectory in reverse order (adds 180° to yaw)") + parser.add_argument("--loop", type=int, default=1, help="Number of times to loop through the waypoints (default: 1)") args = parser.parse_args() try: - yaml_path = _select_yaml(args) - except FileNotFoundError as exc: - print(f"[ERROR] {exc}", file=sys.stderr) + yaml_file_path = _select_yaml(args) + except FileNotFoundError as e: + print(f"[ERROR] {e}", file=sys.stderr) sys.exit(1) + # Feedback to the user + print(f"Loading waypoints file: {yaml_file_path}") try: - commander = GpsWpCommander(yaml_path, reverse=args.reverse) - commander.start() - except KeyboardInterrupt: - print("Interrupted by user - shutting down…", file=sys.stderr) + gps_wpf = GpsWpCommander(yaml_file_path, reverse=args.reverse, num_loop=args.loop) + gps_wpf.start_ntp() except Exception as exc: print(f"[ERROR] {exc}", file=sys.stderr) sys.exit(1) finally: rclpy.shutdown() + if __name__ == "__main__": main() diff --git a/src/nav2_tutorial/src/smooth_wp_follower.py b/src/nav2_tutorial/src/smooth_wp_follower.py index cb69fc1..93acce4 100644 --- a/src/nav2_tutorial/src/smooth_wp_follower.py +++ b/src/nav2_tutorial/src/smooth_wp_follower.py @@ -1,29 +1,27 @@ -import os import sys -import math -import yaml import argparse +from uuid import UUID +# ROS packages import rclpy -from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult - from rclpy.action import ActionClient -from rclpy.qos import qos_profile_sensor_data -from uuid import UUID -from src.utils.nav_utils import _hsv_to_rgb, _get_goal_handle, _get_current_uuid -from src.utils.parser_utils import YamlWaypointParser, _latest_file, _resolve_ws_paths, _select_yaml +from rclpy.qos import qos_profile_sensor_data, QoSProfile, DurabilityPolicy +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult # ROS messages from sensor_msgs.msg import NavSatFix from geometry_msgs.msg import PoseStamped -from nav_msgs.msg import Odometry from action_msgs.msg import GoalStatus +from nav_msgs.msg import Path, Odometry from nav2_msgs.action import FollowPath +from visualization_msgs.msg import Marker, MarkerArray + +# Custom utils +from src.utils.nav_utils import (_pose_dist, _path_length, _segment_length, _closest_wp_index, + _make_window, _hsv_to_rgb, _get_goal_handle, _get_current_uuid) +from src.utils.parser_utils import YamlWaypointParser, _latest_file, _resolve_ws_paths, _select_yaml # Visualization -from nav_msgs.msg import Path -from visualization_msgs.msg import Marker, MarkerArray -from rclpy.qos import QoSProfile, DurabilityPolicy MARKER_VIZ = True @@ -45,7 +43,7 @@ def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1) # Parameters for the GoThroughPoses task self.odom_timeout_s = 2.0 # wait this long for first /fixposition msg - self.max_retries = 1 # re-attempt each failed segment once + self.max_retries = 1 # re-attempts for each failed segment self._retries_left = self.max_retries self._last_goal_uuid: UUID | None = None @@ -116,13 +114,14 @@ def start_ntp(self): # Publish the full trajectory for visualization self._publish_full_traj(all_poses) + # Loop through the waypoints for loop_idx in range(self.num_loop): if self.num_loop > 1: self.navigator.get_logger().info( f"Performing loop {loop_idx + 1} out of {self.num_loop}…") self._total_wps = len(all_poses) - total_len = self._path_length(all_poses) + total_len = _path_length(all_poses) self.navigator.get_logger().info( f"Preparing segments for trajectory of {total_len:0.2f} m " f"consisting of {self._total_wps} waypoints…") @@ -134,8 +133,8 @@ def start_ntp(self): if self.num_loop == 1: # Fast-forward to the first pose within start_seg_tol - closest_idx = self._closest_wp_index(robot, remaining) - if self._dist(robot, remaining[closest_idx]) > self.start_seg_tol: + closest_idx = _closest_wp_index(robot, remaining) + if _pose_dist(robot, remaining[closest_idx]) > self.start_seg_tol: closest_idx = 0 # fall back to the first wp if closest_idx: @@ -151,7 +150,7 @@ def start_ntp(self): self.navigator.get_logger().info("Trajectory already completed.") return - window = self._make_window(robot, remaining) + window = _make_window(robot, remaining, self.seg_len_max) if len(window) == 1: # no waypoint fit inside seg_len_max window.append(remaining[0]) new_wps = 1 @@ -194,14 +193,14 @@ def start_ntp(self): # 1) SUCCEEDED: accept only if really at tail and nothing remains in the global list if status == TaskResult.SUCCEEDED: - at_tail = (self._dist(robot, window[-1]) <= self.advance_tol) + at_tail = (_pose_dist(robot, window[-1]) <= self.advance_tol) if at_tail and not remaining: self.navigator.get_logger().info("Trajectory finished") break else: # Success was premature: resend the unreached tail unreached = [p for p in window - if self._dist(robot, p) > self.advance_tol] + if _pose_dist(robot, p) > self.advance_tol] if not unreached: # corner-case: we are at tail but still have poses in ‘remaining’. # Continue; the normal sliding-window logic will pick them up. @@ -220,7 +219,7 @@ def start_ntp(self): if self._retries_left > 0: self._retries_left -= 1 unreached = [p for p in window - if self._dist(robot, p) > self.advance_tol] + if _pose_dist(robot, p) > self.advance_tol] if not unreached: self.navigator.get_logger().error( "Retry requested but no poses remain in window") @@ -242,19 +241,19 @@ def start_ntp(self): robot = self._get_robot_pose() # Assess whether we are close to the window’s tail - if self._dist(robot, window[-1]) > self.advance_tol: + if _pose_dist(robot, window[-1]) > self.advance_tol: continue # Slide the window forward tail = window[-1] # Overlap: walk backwards inside the old window - dist_rt = self._dist(robot, tail) + dist_rt = _pose_dist(robot, tail) overlap = [] for p in reversed(window): - if (self._dist(p, tail) <= self.overlap_tol and # close to tail - self._dist(robot, p) <= dist_rt + 1e-6): # not behind robot + if (_pose_dist(p, tail) <= self.overlap_tol and # close to tail + _pose_dist(robot, p) <= dist_rt + 1e-6): # not behind robot overlap.append(p) else: break @@ -264,7 +263,7 @@ def start_ntp(self): continue # path finished; let Nav2 coast to the end # Build the forward part of the new window - forward = self._make_window(tail, remaining)[1:] # skip duplicate ‘tail’ + forward = _make_window(tail, remaining, self.seg_len_max)[1:] # skip duplicate ‘tail’ new_window = overlap + forward # Drop from ‘remaining’ exactly the poses we just attached @@ -336,49 +335,12 @@ def _get_robot_pose(self) -> PoseStamped: rclpy.spin_once(self.navigator, timeout_sec=0.05) return self._last_odom_pose - - def _dist(self, a: PoseStamped, b: PoseStamped) -> float: - """Return the Euclidean distance between two PoseStamped messages.""" - dx, dy = a.pose.position.x - b.pose.position.x, a.pose.position.y - b.pose.position.y - return math.hypot(dx, dy) - - def _path_length(self, poses: list[PoseStamped]) -> float: - """Return the total length of the path defined by the poses.""" - return sum(self._dist(a, b) for a, b in zip(poses[:-1], poses[1:])) - - def _make_window(self, start: PoseStamped, todo: list[PoseStamped]) -> list[PoseStamped]: - """Return [start] + as many todo-poses as fit within seg_len_max.""" - window = [start] - length = 0.0 - prev_xy = (start.pose.position.x, start.pose.position.y) - - for p in todo: - step = math.hypot(p.pose.position.x - prev_xy[0], - p.pose.position.y - prev_xy[1]) - if length + step > self.seg_len_max: - break - window.append(p) - length, prev_xy = length + step, (p.pose.position.x, p.pose.position.y) - return window - - def _segment_length(self, poses: list[PoseStamped]) -> float: - """Return the length of the segment defined by the poses.""" - return sum(self._dist(a, b) for a, b in zip(poses[:-1], poses[1:])) - - def _closest_wp_index(self, robot: PoseStamped, poses: list[PoseStamped]) -> int: - """Compute the index of the closest waypoint to the robot.""" - d_min, idx_min = float("inf"), 0 - for i, p in enumerate(poses): - d = self._dist(robot, p) - if d < d_min: - d_min, idx_min = d, i - return idx_min def _log_segment(self, window, *, overlap_cnt: int, seg_idx: int) -> None: """Log the segment statistics.""" - seg_len = self._segment_length(window) + seg_len = _segment_length(window) num_wps = len(window) progress = self._visited_wps @@ -480,7 +442,7 @@ def main(argv: list[str] | None = None): try: yaml_file_path = _select_yaml(args) except FileNotFoundError as e: - print(f"[WARN] {e}", file=sys.stderr) + print(f"[ERROR] {e}", file=sys.stderr) sys.exit(1) # Feedback to the user @@ -491,6 +453,8 @@ def main(argv: list[str] | None = None): except Exception as exc: print(f"[ERROR] {exc}", file=sys.stderr) sys.exit(1) + finally: + rclpy.shutdown() if __name__ == "__main__": diff --git a/src/nav2_tutorial/src/utils/nav_utils.py b/src/nav2_tutorial/src/utils/nav_utils.py index 8f73fa6..2564673 100644 --- a/src/nav2_tutorial/src/utils/nav_utils.py +++ b/src/nav2_tutorial/src/utils/nav_utils.py @@ -1,3 +1,50 @@ +import math +from typing import List +from geometry_msgs.msg import PoseStamped + + +def _pose_dist(self, a: PoseStamped, b: PoseStamped) -> float: + """Return the Euclidean distance between two PoseStamped messages.""" + dx, dy = a.pose.position.x - b.pose.position.x, a.pose.position.y - b.pose.position.y + return math.hypot(dx, dy) + + +def _path_length(self, poses: list[PoseStamped]) -> float: + """Return the total length of the path defined by the poses.""" + return sum(_pose_dist(a, b) for a, b in zip(poses[:-1], poses[1:])) + + +def _segment_length(self, poses: list[PoseStamped]) -> float: + """Return the length of the segment defined by the poses.""" + return sum(_pose_dist(a, b) for a, b in zip(poses[:-1], poses[1:])) + + +def _closest_wp_index(self, robot: PoseStamped, poses: list[PoseStamped]) -> int: + """Compute the index of the closest waypoint to the robot.""" + d_min, idx_min = float("inf"), 0 + for i, p in enumerate(poses): + d = _pose_dist(robot, p) + if d < d_min: + d_min, idx_min = d, i + return idx_min + + +def _make_window(self, start: PoseStamped, todo: list[PoseStamped], seg_len_max: float) -> list[PoseStamped]: + """Return [start] + as many todo-poses as fit within seg_len_max.""" + window = [start] + length = 0.0 + prev_xy = (start.pose.position.x, start.pose.position.y) + + for p in todo: + step = math.hypot(p.pose.position.x - prev_xy[0], + p.pose.position.y - prev_xy[1]) + if length + step > seg_len_max: + break + window.append(p) + length, prev_xy = length + step, (p.pose.position.x, p.pose.position.y) + return window + + def _hsv_to_rgb(h, s=1.0, v=1.0): """HSV to RGB converter.""" i = int(h*6.0) % 6 From 00fb7af302e2bee703bd5a79825918bb1c4399d9 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 10 Jul 2025 10:39:19 +0200 Subject: [PATCH 13/23] Bug fixes --- README.md | 4 +-- scripts/{start_can.sh => setup-can.sh} | 0 src/nav2_tutorial/src/utils/nav_utils.py | 34 ++++++++++----------- src/nav2_tutorial/src/utils/parser_utils.py | 31 ++++++++++--------- 4 files changed, 35 insertions(+), 34 deletions(-) rename scripts/{start_can.sh => setup-can.sh} (100%) diff --git a/README.md b/README.md index 4a8ed5d..88f19b6 100644 --- a/README.md +++ b/README.md @@ -93,9 +93,9 @@ sudo ip link set can0 up type can bitrate 500000 candump can0 ``` -Alternatively, the user can also directly execute the provided script start_can.sh: +Alternatively, the user can also directly execute the provided script setup-can.sh: ```bash -sudo ./scripts/start_can.sh +sudo ./scripts/setup-can.sh ``` Example output from the can0 port: diff --git a/scripts/start_can.sh b/scripts/setup-can.sh similarity index 100% rename from scripts/start_can.sh rename to scripts/setup-can.sh diff --git a/src/nav2_tutorial/src/utils/nav_utils.py b/src/nav2_tutorial/src/utils/nav_utils.py index 2564673..c5a469d 100644 --- a/src/nav2_tutorial/src/utils/nav_utils.py +++ b/src/nav2_tutorial/src/utils/nav_utils.py @@ -3,23 +3,23 @@ from geometry_msgs.msg import PoseStamped -def _pose_dist(self, a: PoseStamped, b: PoseStamped) -> float: +def _pose_dist(a: PoseStamped, b: PoseStamped) -> float: """Return the Euclidean distance between two PoseStamped messages.""" dx, dy = a.pose.position.x - b.pose.position.x, a.pose.position.y - b.pose.position.y return math.hypot(dx, dy) -def _path_length(self, poses: list[PoseStamped]) -> float: +def _path_length(poses: list[PoseStamped]) -> float: """Return the total length of the path defined by the poses.""" return sum(_pose_dist(a, b) for a, b in zip(poses[:-1], poses[1:])) -def _segment_length(self, poses: list[PoseStamped]) -> float: +def _segment_length(poses: list[PoseStamped]) -> float: """Return the length of the segment defined by the poses.""" return sum(_pose_dist(a, b) for a, b in zip(poses[:-1], poses[1:])) -def _closest_wp_index(self, robot: PoseStamped, poses: list[PoseStamped]) -> int: +def _closest_wp_index(robot: PoseStamped, poses: list[PoseStamped]) -> int: """Compute the index of the closest waypoint to the robot.""" d_min, idx_min = float("inf"), 0 for i, p in enumerate(poses): @@ -29,7 +29,7 @@ def _closest_wp_index(self, robot: PoseStamped, poses: list[PoseStamped]) -> int return idx_min -def _make_window(self, start: PoseStamped, todo: list[PoseStamped], seg_len_max: float) -> list[PoseStamped]: +def _make_window(start: PoseStamped, todo: list[PoseStamped], seg_len_max: float) -> list[PoseStamped]: """Return [start] + as many todo-poses as fit within seg_len_max.""" window = [start] length = 0.0 @@ -45,17 +45,17 @@ def _make_window(self, start: PoseStamped, todo: list[PoseStamped], seg_len_max: return window -def _hsv_to_rgb(h, s=1.0, v=1.0): +def _hsv_to_rgb(h: float, s: float = 1.0, v: float = 1.0): """HSV to RGB converter.""" - i = int(h*6.0) % 6 - f = h*6.0 - i - p, q, t = v*(1-s), v*(1-f*s), v*(1-(1-f)*s) - if i == 0: r,g,b = v,t,p - elif i == 1: r,g,b = q,v,p - elif i == 2: r,g,b = p,v,t - elif i == 3: r,g,b = p,q,v - elif i == 4: r,g,b = t,p,v - else: r,g,b = v,p,q + i = int(h * 6.0) % 6 + f = h * 6.0 - i + p, q, t = v * (1 - s), v * (1 - f * s), v * (1 - (1 - f) * s) + if i == 0: r, g, b = v, t, p + elif i == 1: r, g, b = q, v, p + elif i == 2: r, g, b = p, v, t + elif i == 3: r, g, b = p, q, v + elif i == 4: r, g, b = t, p, v + else: r, g, b = v, p, q return r, g, b @@ -70,8 +70,8 @@ def _get_goal_handle(nav): def _get_current_uuid(nav): """Return the UUID (bytes) of the goal that is currently active.""" - for attr in ("_current_nav_through_poses_uuid", # Humble - "_current_ntp_uuid" # Iron/Jazzy + for attr in ("_current_ntp_uuid" # Iron/Jazzy + "_current_nav_through_poses_uuid", # Humble, Galactic ): if hasattr(nav, attr): return getattr(nav, attr) diff --git a/src/nav2_tutorial/src/utils/parser_utils.py b/src/nav2_tutorial/src/utils/parser_utils.py index 24c19d1..b80adc5 100644 --- a/src/nav2_tutorial/src/utils/parser_utils.py +++ b/src/nav2_tutorial/src/utils/parser_utils.py @@ -1,6 +1,7 @@ import os import copy import glob +import math import yaml import rclpy import argparse @@ -129,11 +130,9 @@ def _latest_file(directory: str, pattern: str = "gps_waypoints_2*.yaml") -> str def _resolve_ws_paths() -> tuple[str, str]: - """Return ``(src_traj_dir, share_traj_dir)`` for *nav2_tutorial*. - - *share_traj_dir* is the directory installed by ``ament`` (typically - ``install/share/nav2_tutorial/trajectories``) while *src_traj_dir* points to - the editable source tree (``/src/nav2_tutorial/trajectories``). + """Return (src_traj_dir, share_traj_dir), *share_traj_dir* is the directory + installed by ``ament`` (typically ``install/share/nav2_tutorial/trajectories``) + while *src_traj_dir* points to the editable source tree. """ share_dir = get_package_share_directory("nav2_tutorial") share_traj_dir = os.path.join(share_dir, "trajectories") @@ -169,6 +168,7 @@ def _select_yaml(args: argparse.Namespace) -> str: # 2) --last flag if args.last: + chosen = None src_latest = _latest_file(src_traj_dir) share_latest = _latest_file(share_traj_dir) @@ -179,17 +179,18 @@ def _select_yaml(args: argparse.Namespace) -> str: chosen = src_latest or share_latest # whichever is not None (could still be None) if chosen is None: - raise FileNotFoundError(f"No waypoint files matching 'gps_waypoints_2*.yaml' found in '{src_traj_dir}' or '{share_traj_dir}'.") + raise FileNotFoundError( + f"No waypoint files matching 'gps_waypoints_2*.yaml' found in '{src_traj_dir}' or '{share_traj_dir}'." + ) return chosen # 3) Default lookup "gps_waypoints.yaml" default_name = "gps_waypoints.yaml" - candidate_src = os.path.join(src_traj_dir, default_name) - if os.path.isfile(candidate_src): - return candidate_src - - candidate_share = os.path.join(share_traj_dir, default_name) - if os.path.isfile(candidate_share): - return candidate_share - - raise FileNotFoundError(f"Default waypoint file '{default_name}' not found in '{src_traj_dir}' or '{share_traj_dir}'.") + for base in (src_traj_dir, share_traj_dir): + candidate = os.path.join(base, default_name) + if os.path.isfile(candidate): + return candidate + + raise FileNotFoundError( + f"Default waypoint file '{default_name}' not found in '{src_traj_dir}' or '{share_traj_dir}'." + ) From f4c507e37eb1e1334f16dd796055b882a259c77f Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 10 Jul 2025 11:32:15 +0200 Subject: [PATCH 14/23] Small tweaks to stopping criterium --- src/nav2_tutorial/src/precise_wp_follower.py | 14 ++- src/nav2_tutorial/src/smooth_wp_follower.py | 97 +++++++++----------- 2 files changed, 50 insertions(+), 61 deletions(-) diff --git a/src/nav2_tutorial/src/precise_wp_follower.py b/src/nav2_tutorial/src/precise_wp_follower.py index 9acd6eb..2a74eb5 100644 --- a/src/nav2_tutorial/src/precise_wp_follower.py +++ b/src/nav2_tutorial/src/precise_wp_follower.py @@ -69,17 +69,14 @@ def _get_datum(self, topic: str = "/fixposition/datum", Block until a NavSatFix is received on topic (max timeout_s). Returns (lat [deg], lon [deg], alt [m]). Raises RuntimeError on timeout. """ - datum: list[float] | None = None - + datum = None def _cb(msg: NavSatFix): nonlocal datum - datum = [msg.latitude, msg.longitude, msg.altitude] + datum = (msg.latitude, msg.longitude, msg.altitude) - sub = self.navigator.create_subscription( - NavSatFix, topic, _cb, qos_profile_sensor_data) + sub = self.navigator.create_subscription(NavSatFix, topic, _cb, qos_profile_sensor_data) - deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( - seconds=timeout_s) + deadline = self.navigator.get_clock().now() + rclpy.duration.Duration(seconds=timeout_s) while rclpy.ok() and datum is None: rclpy.spin_once(self.navigator, timeout_sec=0.05) @@ -87,7 +84,7 @@ def _cb(msg: NavSatFix): raise RuntimeError(f"No NavSatFix on {topic} within {timeout_s}s") self.navigator.destroy_subscription(sub) - return tuple(datum) + return datum def start_ntp(self): """Start the GoToPose task with the waypoints.""" @@ -230,6 +227,7 @@ def start_ntp(self): overlap_cnt = len(overlap), seg_idx = seg_idx) + # Send next window self.navigator.goToPose(new_window) self._retries_left = self.max_retries seg_idx += 1 diff --git a/src/nav2_tutorial/src/smooth_wp_follower.py b/src/nav2_tutorial/src/smooth_wp_follower.py index 93acce4..1b4a09d 100644 --- a/src/nav2_tutorial/src/smooth_wp_follower.py +++ b/src/nav2_tutorial/src/smooth_wp_follower.py @@ -28,10 +28,11 @@ class GpsWpCommander: """Follow a smooth sequence of GPS waypoints.""" - def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1): + def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1, stopping: bool = False): self.navigator = BasicNavigator("basic_navigator") self.reverse = reverse self.num_loop = num_loop + self.stopping = stopping # Odometry topic with ENU pose in map frame self._last_odom_pose: PoseStamped | None = None @@ -45,6 +46,7 @@ def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1) self.odom_timeout_s = 2.0 # wait this long for first /fixposition msg self.max_retries = 1 # re-attempts for each failed segment self._retries_left = self.max_retries + self._fp_client = None self._last_goal_uuid: UUID | None = None # Sliding-window parameters @@ -54,13 +56,12 @@ def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1) self.advance_tol = 2.6 # m - when to roll the window forward self.overlap_tol = 1.0 # m - keep poses this close as overlap self.start_seg_tol = 3.0 # m - max distance we “snap” to the path - self._fp_client = None # Statistics self._total_wps = 0 self._visited_wps = 0 - # Marker visualization + # Optional: Marker visualization if MARKER_VIZ: latched_qos = QoSProfile( depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) @@ -83,17 +84,14 @@ def _get_datum(self, topic: str = "/fixposition/datum", Block until a NavSatFix is received on topic (max timeout_s). Returns (lat [deg], lon [deg], alt [m]). Raises RuntimeError on timeout. """ - datum: list[float] | None = None - + datum = None def _cb(msg: NavSatFix): nonlocal datum - datum = [msg.latitude, msg.longitude, msg.altitude] + datum = (msg.latitude, msg.longitude, msg.altitude) - sub = self.navigator.create_subscription( - NavSatFix, topic, _cb, qos_profile_sensor_data) + sub = self.navigator.create_subscription(NavSatFix, topic, _cb, qos_profile_sensor_data) - deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( - seconds=timeout_s) + deadline = self.navigator.get_clock().now() + rclpy.duration.Duration(seconds=timeout_s) while rclpy.ok() and datum is None: rclpy.spin_once(self.navigator, timeout_sec=0.05) @@ -101,7 +99,7 @@ def _cb(msg: NavSatFix): raise RuntimeError(f"No NavSatFix on {topic} within {timeout_s}s") self.navigator.destroy_subscription(sub) - return tuple(datum) + return datum def start_ntp(self): """Start the GoThroughPoses task with the waypoints.""" @@ -135,10 +133,10 @@ def start_ntp(self): # Fast-forward to the first pose within start_seg_tol closest_idx = _closest_wp_index(robot, remaining) if _pose_dist(robot, remaining[closest_idx]) > self.start_seg_tol: - closest_idx = 0 # fall back to the first wp + closest_idx = 0 if closest_idx: - self._visited_wps += closest_idx # statistics + self._visited_wps += closest_idx skipped = closest_idx remaining = remaining[closest_idx:] self.navigator.get_logger().info( @@ -158,9 +156,7 @@ def start_ntp(self): new_wps = len(window) - 1 self._visited_wps += new_wps - self._log_segment(window, - overlap_cnt = 0, - seg_idx = seg_idx) + self._log_segment(window, overlap_cnt=0, seg_idx=seg_idx) self._publish_waypoints(window, seg_idx) self.navigator.goThroughPoses(window) @@ -193,8 +189,7 @@ def start_ntp(self): # 1) SUCCEEDED: accept only if really at tail and nothing remains in the global list if status == TaskResult.SUCCEEDED: - at_tail = (_pose_dist(robot, window[-1]) <= self.advance_tol) - if at_tail and not remaining: + if (_pose_dist(robot, window[-1]) <= self.advance_tol) and not remaining: self.navigator.get_logger().info("Trajectory finished") break else: @@ -271,41 +266,36 @@ def start_ntp(self): # Log stats before cancel / send goal self._visited_wps += len(forward) - self._log_segment(new_window, - overlap_cnt = len(overlap), - seg_idx = seg_idx) - - # # Pre-empt the running action - # self.navigator.cancelTask() - # # Wait until the cancellation is fully processed - # cancel_deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( - # seconds=3.0) # plenty - normally < 0.3 s - # while not self.navigator.isTaskComplete(): - # if self.navigator.get_clock().now() > cancel_deadline: - # self.navigator.get_logger().error("Cancel timeout - aborting") - # return - # rclpy.spin_once(self.navigator, timeout_sec=0.05) - - # # Extra guard: wait until internal follow_path is cancelled - # if self._fp_client is None: - # self._fp_client = ActionClient(self.navigator, FollowPath, "follow_path") - # if not self._fp_client.wait_for_server(timeout_sec=2.0): - # self.navigator.get_logger().warn("follow_path server not available") - - # fp_deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( - # seconds=3.0) - # while True: - # goal_handles = self._fp_client._goal_handles # internal list - # if not goal_handles: - # break # nothing active - # status = goal_handles[0].status - # if status in (GoalStatus.STATUS_SUCCEEDED, GoalStatus.STATUS_CANCELED): - # break - # if self.navigator.get_clock().now() > fp_deadline: - # self.navigator.get_logger().error("follow_path cancel timeout") - # return - # rclpy.spin_once(self.navigator, timeout_sec=0.05) + self._log_segment(new_window, overlap_cnt=len(overlap), seg_idx=seg_idx) + + # Optional: explicit cancellation before sending new window + if self.stopping: + self.navigator.cancelTask() + cancel_deadline = self.navigator.get_clock().now() + rclpy.duration.Duration(seconds=3.0) + while not self.navigator.isTaskComplete(): + if self.navigator.get_clock().now() > cancel_deadline: + self.navigator.get_logger().error("Cancel timeout - aborting") + return + rclpy.spin_once(self.navigator, timeout_sec=0.05) + # extra guard: ensure internal follow_path is cancelled + if self._fp_client is None: + self._fp_client = ActionClient(self.navigator, FollowPath, "follow_path") + if not self._fp_client.wait_for_server(timeout_sec=2.0): + self.navigator.get_logger().warn("follow_path server not available") + fp_deadline = self.navigator.get_clock().now() + rclpy.duration.Duration(seconds=3.0) + while True: + goal_handles = self._fp_client._goal_handles + if not goal_handles: + break + status = goal_handles[0].status + if status in (GoalStatus.STATUS_SUCCEEDED, GoalStatus.STATUS_CANCELED): + break + if self.navigator.get_clock().now() > fp_deadline: + self.navigator.get_logger().error("follow_path cancel timeout - aborting") + return + rclpy.spin_once(self.navigator, timeout_sec=0.05) + # Send next window self._publish_waypoints(new_window, seg_idx) self.navigator.goThroughPoses(new_window) self._retries_left = self.max_retries @@ -437,6 +427,7 @@ def main(argv: list[str] | None = None): parser.add_argument("--last", action="store_true", help="Load the most recent waypoints file in the config directory") parser.add_argument("--reverse", action="store_true", help="Follow the trajectory in reverse order (adds 180° to yaw)") parser.add_argument("--loop", type=int, default=1, help="Number of times to loop through the waypoints (default: 1)") + parser.add_argument("--stopping", action="store_true", help="Stop at the end of every segment") args = parser.parse_args() try: @@ -448,7 +439,7 @@ def main(argv: list[str] | None = None): # Feedback to the user print(f"Loading waypoints file: {yaml_file_path}") try: - gps_wpf = GpsWpCommander(yaml_file_path, reverse=args.reverse, num_loop=args.loop) + gps_wpf = GpsWpCommander(yaml_file_path, reverse=args.reverse, num_loop=args.loop, stopping=args.stopping) gps_wpf.start_ntp() except Exception as exc: print(f"[ERROR] {exc}", file=sys.stderr) From 3479d57ac68b2200f7d26920609f61b03289ed0f Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 10 Jul 2025 13:29:17 +0200 Subject: [PATCH 15/23] Minor patch fixes --- src/nav2_tutorial/src/precise_wp_follower.py | 189 +++++-------------- src/nav2_tutorial/src/smooth_wp_follower.py | 24 +-- 2 files changed, 61 insertions(+), 152 deletions(-) diff --git a/src/nav2_tutorial/src/precise_wp_follower.py b/src/nav2_tutorial/src/precise_wp_follower.py index 2a74eb5..27363c5 100644 --- a/src/nav2_tutorial/src/precise_wp_follower.py +++ b/src/nav2_tutorial/src/precise_wp_follower.py @@ -4,34 +4,30 @@ # ROS packages import rclpy -from rclpy.action import ActionClient from rclpy.qos import qos_profile_sensor_data, QoSProfile, DurabilityPolicy from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult # ROS messages from sensor_msgs.msg import NavSatFix from geometry_msgs.msg import PoseStamped -from action_msgs.msg import GoalStatus from nav_msgs.msg import Path, Odometry -from nav2_msgs.action import FollowPath from visualization_msgs.msg import Marker, MarkerArray # Custom utils -from src.utils.nav_utils import (_pose_dist, _path_length, _closest_wp_index, - _hsv_to_rgb, _get_goal_handle, _get_current_uuid) -from src.utils.parser_utils import YamlWaypointParser, _latest_file, _resolve_ws_paths, _select_yaml +from src.utils.nav_utils import _pose_dist, _path_length, _closest_wp_index +from src.utils.parser_utils import YamlWaypointParser, _select_yaml # Visualization MARKER_VIZ = True class GpsWpCommander: - """Follow a smooth sequence of GPS waypoints.""" + """Follow a precise sequence of GPS waypoints.""" - def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1): - self.navigator = BasicNavigator("basic_navigator") - self.reverse = reverse - self.num_loop = num_loop + def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1) -> None: + self.navigator = BasicNavigator("basic_navigator") + self.reverse = reverse + self.num_loop = max(1, num_loop) # Odometry topic with ENU pose in map frame self._last_odom_pose: PoseStamped | None = None @@ -42,11 +38,9 @@ def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1) self.wp_parser = YamlWaypointParser(wps_file_path, base_lat, base_lon, base_alt) # Parameters for the GoToPose task - self.odom_timeout_s = 2.0 # wait this long for first /fixposition msg - self.max_retries = 1 # re-attempts for each failed waypoint - self.start_seg_tol = 3.0 # m - max distance we “snap” to the path - self._retries_left = self.max_retries - self._last_goal_uuid: UUID | None = None + self.odom_timeout_s = 2.0 # [s] wait for odom on start‑up + self.max_retries = 1 # per‑waypoint retries + self.start_wp_tol = 3.0 # [m] skip wp if already this close # Statistics self._total_wps = 0 @@ -58,7 +52,7 @@ def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1) depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) # Path and MarkerArray for visualizing the full trajectory - self.full_traj_pub = self.navigator.create_publisher( + self.full_traj_pub = self.navigator.create_publisher( Path, "/gps_full_traj", latched_qos) self.full_traj_mrk_pub = self.navigator.create_publisher( MarkerArray, "/gps_full_traj_markers", latched_qos) @@ -86,7 +80,7 @@ def _cb(msg: NavSatFix): self.navigator.destroy_subscription(sub) return datum - def start_ntp(self): + def start_ntp(self) -> None: """Start the GoToPose task with the waypoints.""" # Wait for the robot_localization node to be active self.navigator.waitUntilNav2Active(localizer="robot_localization") @@ -110,13 +104,13 @@ def start_ntp(self): f"consisting of {self._total_wps} waypoints…") remaining = all_poses - # Prime the first window + # Skip early waypoints already within tolerance robot = self._get_robot_pose() if self.num_loop == 1: - # Fast-forward to the first pose within start_seg_tol + # Fast-forward to the first pose within start_wp_tol closest_idx = _closest_wp_index(robot, remaining) - if _pose_dist(robot, remaining[closest_idx]) > self.start_seg_tol: + if _pose_dist(robot, remaining[closest_idx]) > self.start_wp_tol: closest_idx = 0 # fall back to the first wp if closest_idx: @@ -127,114 +121,43 @@ def start_ntp(self): f"Skipped {skipped} waypoint(s); " f"starting with wp #{skipped+1}/{self._total_wps}") - # If everything was within the tolerance, finish trajectory follower - if not remaining: - self.navigator.get_logger().info("Trajectory already completed.") - return - - self._visited_wps += 1 - self._log_segment(window, - overlap_cnt = 0, - seg_idx = seg_idx) - self.navigator.goToPose(pose) - remaining = remaining[1:] - - # Continuous streaming loop - while rclpy.ok(): - rclpy.spin_once(self.navigator, timeout_sec=0.05) - - if self.navigator.isTaskComplete(): - status = self.navigator.getResult() - - goal_handle = _get_goal_handle(self.navigator) - current_id = _get_current_uuid(self.navigator) - - # If we can’t identify the result reliably, treat it as current - if goal_handle is None or current_id is None: - result_is_current = True - else: - result_id = UUID(bytes=goal_handle.goal_id.uuid) - result_is_current = (result_id == UUID(bytes=current_id)) - - # Stale result from the goal we just pre-empted - if not result_is_current: + # Navigate to each remaining waypoint + for local_idx, pose in enumerate(remaining, start=1): + wp_global_idx = self._visited_wps + 1 + + retries_left = self.max_retries + while True: + self.navigator.goToPose(pose) + + # Wait until action finishes (spinning keeps callbacks alive) + while rclpy.ok() and not self.navigator.isTaskComplete(): + rclpy.spin_once(self.navigator, timeout_sec=0.05) + + result = self.navigator.getResult() + + if result == TaskResult.SUCCEEDED: + self.navigator.get_logger().info( + f"Waypoint {wp_global_idx}/{self._total_wps} reached") + break + + if retries_left > 0: + self.navigator.get_logger().warn( + f"Waypoint {wp_global_idx} " + f"{result.name.lower()} - retrying ({retries_left} left)") + retries_left -= 1 continue - # Status handling - robot = self._get_robot_pose() - - # 1) SUCCEEDED: accept only if really at tail and nothing remains in the global list - if status == TaskResult.SUCCEEDED: - at_tail = (_pose_dist(robot, window[-1]) <= self.advance_tol) - if at_tail and not remaining: - self.navigator.get_logger().info("Trajectory finished") - break - else: - # Success was premature: resend the unreached tail - unreached = [p for p in window - if _pose_dist(robot, p) > self.advance_tol] - if not unreached: - # corner-case: we are at tail but still have poses in ‘remaining’. - # Continue; the normal sliding-window logic will pick them up. - continue - retry_window = [robot] + unreached - self.navigator.get_logger().warn( - f"Premature SUCCEEDED - retrying {len(unreached)} " - f"poses of current segment") - self.navigator.goToPose(retry_window) - window = retry_window - continue - - # 2) FAILED or CANCELLED: Retry the segment - if status in (TaskResult.FAILED, TaskResult.CANCELED): - if self._retries_left > 0: - self._retries_left -= 1 - unreached = [p for p in window - if _pose_dist(robot, p) > self.advance_tol] - if not unreached: - self.navigator.get_logger().error( - "Retry requested but no poses remain in window") - break - retry_window = [robot] + unreached - self.navigator.get_logger().warn( - f"Segment {seg_idx+1} {status.name.lower()} - " - f"retrying {len(unreached)} poses") - self.navigator.goToPose(retry_window) - window = retry_window - continue - else: - self.navigator.get_logger().error( - "Segment failed twice - aborting") - break - - # Live pose - robot = self._get_robot_pose() - - # Assess whether we are close to the window’s tail - if _pose_dist(robot, window[-1]) > self.advance_tol: - continue - - if not remaining: - continue # path finished; let Nav2 coast to the end - - # Drop from ‘remaining’ exactly the poses we just attached - pose = remaining[0] - remaining = remaining[1:] + # Exceeded retries → abort entire mission + self.navigator.get_logger().error( + f"Waypoint {wp_global_idx} failed after retry - aborting mission") + raise RuntimeError("Navigation failed") - # Log stats before cancel / send goal self._visited_wps += 1 - self._log_segment(new_window, - overlap_cnt = len(overlap), - seg_idx = seg_idx) - - # Send next window - self.navigator.goToPose(new_window) - self._retries_left = self.max_retries - seg_idx += 1 - window = new_window + + self.navigator.get_logger().info("All waypoints completed - mission finished") def _odom_cb(self, msg: Odometry) -> None: - """Cache the most recent odometry pose as a `PoseStamped`.""" + """Cache the latest odometry pose (PoseStamped).""" ps = PoseStamped() ps.header = msg.header ps.pose = msg.pose.pose @@ -245,8 +168,8 @@ def _get_robot_pose(self) -> PoseStamped: Return the most recent pose from /fixposition/odometry_enu. Blocks (spins) until a message arrives or the timeout elapses. """ - deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( - seconds=self.odom_timeout_s) + deadline = (self.navigator.get_clock().now() + + rclpy.duration.Duration(seconds=self.odom_timeout_s)) while self._last_odom_pose is None: if self.navigator.get_clock().now() > deadline: @@ -257,21 +180,7 @@ def _get_robot_pose(self) -> PoseStamped: return self._last_odom_pose - def _log_segment(self, window, - *, overlap_cnt: int, - seg_idx: int) -> None: - """Log the segment statistics.""" - seg_len = _segment_length(window) - num_wps = len(window) - progress = self._visited_wps - - self.navigator.get_logger().info( - f"Segment {seg_idx+1} length: {seg_len:0.2f} m, " - f"Num. waypoints: {num_wps}, " - f"Overlap: {overlap_cnt}, " - f"Progress: {progress}/{self._total_wps}") - - def _publish_full_traj(self, poses: list[PoseStamped]): + def _publish_full_traj(self, poses: list[PoseStamped]) -> None: """Publish the whole trajectory as a latched Path + MarkerArray.""" path = Path() path.header.frame_id = "map" diff --git a/src/nav2_tutorial/src/smooth_wp_follower.py b/src/nav2_tutorial/src/smooth_wp_follower.py index 1b4a09d..6f4d472 100644 --- a/src/nav2_tutorial/src/smooth_wp_follower.py +++ b/src/nav2_tutorial/src/smooth_wp_follower.py @@ -19,7 +19,7 @@ # Custom utils from src.utils.nav_utils import (_pose_dist, _path_length, _segment_length, _closest_wp_index, _make_window, _hsv_to_rgb, _get_goal_handle, _get_current_uuid) -from src.utils.parser_utils import YamlWaypointParser, _latest_file, _resolve_ws_paths, _select_yaml +from src.utils.parser_utils import YamlWaypointParser, _select_yaml # Visualization MARKER_VIZ = True @@ -28,10 +28,10 @@ class GpsWpCommander: """Follow a smooth sequence of GPS waypoints.""" - def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1, stopping: bool = False): - self.navigator = BasicNavigator("basic_navigator") - self.reverse = reverse - self.num_loop = num_loop + def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1, stopping: bool = False) -> None: + self.navigator = BasicNavigator("basic_navigator") + self.reverse = reverse + self.num_loop = max(1, num_loop) self.stopping = stopping # Odometry topic with ENU pose in map frame @@ -43,7 +43,7 @@ def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1, self.wp_parser = YamlWaypointParser(wps_file_path, base_lat, base_lon, base_alt) # Parameters for the GoThroughPoses task - self.odom_timeout_s = 2.0 # wait this long for first /fixposition msg + self.odom_timeout_s = 2.0 # wait for odom on start‑up self.max_retries = 1 # re-attempts for each failed segment self._retries_left = self.max_retries self._fp_client = None @@ -73,7 +73,7 @@ def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1, MarkerArray, "/gps_waypoints_markers", latched_qos) # Path and MarkerArray for visualizing the full trajectory - self.full_traj_pub = self.navigator.create_publisher( + self.full_traj_pub = self.navigator.create_publisher( Path, "/gps_full_traj", latched_qos) self.full_traj_mrk_pub = self.navigator.create_publisher( MarkerArray, "/gps_full_traj_markers", latched_qos) @@ -303,7 +303,7 @@ def start_ntp(self): window = new_window def _odom_cb(self, msg: Odometry) -> None: - """Cache the most recent odometry pose as a `PoseStamped`.""" + """Cache the latest odometry pose (PoseStamped).""" ps = PoseStamped() ps.header = msg.header ps.pose = msg.pose.pose @@ -314,8 +314,8 @@ def _get_robot_pose(self) -> PoseStamped: Return the most recent pose from /fixposition/odometry_enu. Blocks (spins) until a message arrives or the timeout elapses. """ - deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( - seconds=self.odom_timeout_s) + deadline = (self.navigator.get_clock().now() + + rclpy.duration.Duration(seconds=self.odom_timeout_s)) while self._last_odom_pose is None: if self.navigator.get_clock().now() > deadline: @@ -340,7 +340,7 @@ def _log_segment(self, window, f"Overlap: {overlap_cnt}, " f"Progress: {progress}/{self._total_wps}") - def _publish_waypoints(self, poses: list[PoseStamped], seg_idx: int): + def _publish_waypoints(self, poses: list[PoseStamped], seg_idx: int) -> None: """Publish the current segment as a Path and MarkerArray.""" # Path message path = Path() @@ -382,7 +382,7 @@ def _publish_waypoints(self, poses: list[PoseStamped], seg_idx: int): markers.markers.append(m) self.waypoint_marker_pub.publish(markers) - def _publish_full_traj(self, poses: list[PoseStamped]): + def _publish_full_traj(self, poses: list[PoseStamped]) -> None: """Publish the whole trajectory as a latched Path + MarkerArray.""" path = Path() path.header.frame_id = "map" From 47f2e5f4fb85992829e47f216958d66c25a9b8e9 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 10 Jul 2025 14:49:19 +0200 Subject: [PATCH 16/23] Added tmux documentation --- README.md | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/README.md b/README.md index 88f19b6..b3f1a76 100644 --- a/README.md +++ b/README.md @@ -262,3 +262,39 @@ Or alternatively use the `--mapviz` flag on the `gps_waypoint_follower.launch.py # Debugging trajectories To analyze the trajectories performed by the robot and determine if any issues have occured, the user can perform a recording of all topics by running `ros2 bag record --all`. Then, launch RViz2 with the provided configuration file to easily visualize the debug topics containing the waypoint coordinates, the global and local plans, the costmaps, among other things: `ros2 run rviz2 rviz2 -d /home/dev/ros_ws/src/nav2_tutorial/config/nav2_plan_viz.rviz`. + +# Recommendations +We suggest employing tmux for controlling the robot via SSH, as intermittent disconnections may kill the running process. Next you'll find some basic steps to get familiar with tmux: + +### Installing tmux +To install tmux on your device, please run the following commands: +```bash +sudo apt update && apt install tmux +``` + +### Start a tmux session +After you SSH into the device (using a terminal or VSCode's Remote-SSH), start a new tmux session as: +```bash +tmux new -s mysession +``` +where "mysession" is simply the name given to this tmux session. Note that the prompt will change slightly once inside the tmux session. Inside, you can interact with it as a normal terminal and run any process. + +### Detach from tmux +At any point, the user can "detach" from tmux and the process will keep running in the background. To do this, please press: 'Ctrl + b' (release both) then press 'd'. This detaches from the session and brings you back to the normal shell while the program continues running. + +### Reconnect later +If the SSH connection drops or you log in again later, list running tmux sessions as: +```bash +tmux ls +``` + +The user will be prompted with something like: +```bash +mysession: 1 windows (created Thu Jul 3 14:32:51 2025) [80x24] +``` + +To reattach to the tmux session, simply use: +```bash +tmux attach -t mysession +``` +You’re now back inside the process, exactly where you left off. From 723dffbe88aca3e2450e5370606360935f6efd01 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 10 Jul 2025 17:47:30 +0200 Subject: [PATCH 17/23] Improved logger and stopping conditions --- README.md | 6 +- src/nav2_tutorial/setup.py | 3 + .../src/loggers/gps_periodic_logger.py | 26 +++++++- src/nav2_tutorial/src/smooth_wp_follower.py | 28 ++++++-- src/nav2_tutorial/src/utils/nav_utils.py | 39 ++++++++++- src/nav2_tutorial/src/utils/parser_utils.py | 3 +- src/nav2_tutorial/src/utils/visualize_gps.py | 56 ++++++++++++++++ .../src/utils/visualize_gps_yaml.py | 65 ------------------- 8 files changed, 147 insertions(+), 79 deletions(-) create mode 100644 src/nav2_tutorial/src/utils/visualize_gps.py delete mode 100644 src/nav2_tutorial/src/utils/visualize_gps_yaml.py diff --git a/README.md b/README.md index b3f1a76..ac36fe0 100644 --- a/README.md +++ b/README.md @@ -209,12 +209,12 @@ nav2_tutorial/src/nav2_tutorial/trajectories/gps_waypoints_.yaml ## 📈 Visualizing Logged Trajectories -To quickly visualize GPS waypoint logs, use the `visualize_gps_yaml.py` script: +To quickly visualize GPS waypoint logs, use the `visualize_gps` script: ### Example: ```bash -python3 utils/visualize_gps_yaml.py path/to/gps_waypoints.yaml # simple 2D plot -python3 utils/visualize_gps_yaml.py path/to/gps_waypoints.yaml --map # map overlay (if supported) +ros2 run nav2_tutorial visualize_gps path/to/gps_waypoints.yaml # simple 2D plot +ros2 run nav2_tutorial visualize_gps path/to/gps_waypoints.yaml --map # map overlay (if supported) ``` ### Requirements for Map Overlay: diff --git a/src/nav2_tutorial/setup.py b/src/nav2_tutorial/setup.py index d46c886..6dd4881 100644 --- a/src/nav2_tutorial/setup.py +++ b/src/nav2_tutorial/setup.py @@ -38,6 +38,9 @@ # Datum Setter 'set_datum = src.utils.set_datum:main', 'set_datum_from_tf = src.utils.set_datum_from_tf:main', + + # Visualizer + 'visualize_gps = src.utils.visualize_gps:main', ], }, ) diff --git a/src/nav2_tutorial/src/loggers/gps_periodic_logger.py b/src/nav2_tutorial/src/loggers/gps_periodic_logger.py index 0bda143..25d27db 100644 --- a/src/nav2_tutorial/src/loggers/gps_periodic_logger.py +++ b/src/nav2_tutorial/src/loggers/gps_periodic_logger.py @@ -1,5 +1,5 @@ -#!/usr/bin/env python3 from __future__ import annotations + import os import sys import tty @@ -39,6 +39,7 @@ def __init__(self, logging_file_path: str, interval: float): self._DIST_TOL_M = 0.01 # Save only if robot moved more than 1 cm self._DEG2M_LAT = 110_574.0 # Rough conversion at mid-latitudes self._last_saved_fix: NavSatFix | None = None + self._last_header_ns: int | None = None # Last header stamp [ns] self.create_subscription(NavSatFix, '/fixposition/odometry_llh', self.gps_callback, qos_profile=1) self.create_subscription(Odometry, '/fixposition/odometry_enu', self.yaw_callback, qos_profile=1) @@ -61,12 +62,23 @@ def _log_waypoint(self) -> None: if fix is None: return + # Reject impossible coordinates early if not (-90 <= fix.latitude <= 90 and -180 <= fix.longitude <= 180): return + # Reject out‑of‑order fixes (header time going backwards) + stamp = fix.header.stamp + curr_ns = stamp.sec * 1_000_000_000 + stamp.nanosec + if self._last_header_ns is not None and curr_ns < self._last_header_ns: + delta_us = (self._last_header_ns - curr_ns) / 1_000 + self.get_logger().warn( + f"Skipping out-of-order fix: header_time went backwards by {delta_us:.0f} µs") + return + if self.saved_points == 0: self.get_logger().info("Started collecting waypoints …") + # Distance gate to avoid duplicates / jitter if self._last_saved_fix is not None: dlat = (fix.latitude - self._last_saved_fix.latitude) * self._DEG2M_LAT lon_scale = 111_320.0 * math.cos(math.radians(fix.latitude)) @@ -74,14 +86,18 @@ def _log_waypoint(self) -> None: if math.hypot(dlat, dlon) < self._DIST_TOL_M: return + # Store LLH point self._last_saved_fix = fix + self._last_header_ns = curr_ns + fix_time_iso = datetime.utcfromtimestamp(curr_ns * 1e-9).isoformat() waypoint = { 'latitude': fix.latitude, 'longitude': fix.longitude, 'altitude': fix.altitude, 'yaw': self.last_yaw, - 'logged_at': datetime.now().isoformat() + 'header_time': fix_time_iso, + 'received_time': datetime.now().isoformat() } self.waypoints.append(waypoint) @@ -124,6 +140,7 @@ def _status_message(self) -> None: f"lon={self.last_gps.longitude:.6f}, yaw={self.last_yaw:.2f}" ) + def _default_yaml_path() -> str: share_dir = get_package_share_directory('nav2_tutorial') ws_root = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(share_dir)))) @@ -133,6 +150,7 @@ def _default_yaml_path() -> str: ts = datetime.now().strftime('%Y%m%d_%H%M%S') return os.path.join(target_dir, f'gps_waypoints_{ts}.yaml') + def _parse_arguments(argv: list[str] | None = None) -> tuple[str, float]: parser = argparse.ArgumentParser(description="Periodic GPS Waypoint Logger") parser.add_argument('yaml', nargs='?', help='Path to the YAML log file.') @@ -140,13 +158,16 @@ def _parse_arguments(argv: list[str] | None = None) -> tuple[str, float]: args = parser.parse_args(argv) return args.yaml or _default_yaml_path(), args.interval + def _stdin_ready() -> bool: return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) + def handle_sigterm(signum, frame): global stop_requested stop_requested = True + def main(argv: list[str] | None = None) -> None: yaml_path, interval = _parse_arguments(argv) @@ -178,5 +199,6 @@ def main(argv: list[str] | None = None) -> None: termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) rclpy.shutdown() + if __name__ == '__main__': main() diff --git a/src/nav2_tutorial/src/smooth_wp_follower.py b/src/nav2_tutorial/src/smooth_wp_follower.py index 6f4d472..bde8dbe 100644 --- a/src/nav2_tutorial/src/smooth_wp_follower.py +++ b/src/nav2_tutorial/src/smooth_wp_follower.py @@ -17,7 +17,7 @@ from visualization_msgs.msg import Marker, MarkerArray # Custom utils -from src.utils.nav_utils import (_pose_dist, _path_length, _segment_length, _closest_wp_index, +from src.utils.nav_utils import (_pose_dist, _path_length, _segment_length, _closest_wp_index, _closest_pt_and_remaining_len, _make_window, _hsv_to_rgb, _get_goal_handle, _get_current_uuid) from src.utils.parser_utils import YamlWaypointParser, _select_yaml @@ -186,16 +186,20 @@ def start_ntp(self): # Status handling robot = self._get_robot_pose() + _, s_left = _closest_pt_and_remaining_len(robot, window) # 1) SUCCEEDED: accept only if really at tail and nothing remains in the global list if status == TaskResult.SUCCEEDED: - if (_pose_dist(robot, window[-1]) <= self.advance_tol) and not remaining: + if (s_left <= self.advance_tol) and not remaining: self.navigator.get_logger().info("Trajectory finished") break else: # Success was premature: resend the unreached tail - unreached = [p for p in window - if _pose_dist(robot, p) > self.advance_tol] + start = _closest_wp_index(robot, window) + unreached = window[start:] + if unreached and _pose_dist(robot, unreached[0]) <= self.advance_tol: + unreached = unreached[1:] + if not unreached: # corner-case: we are at tail but still have poses in ‘remaining’. # Continue; the normal sliding-window logic will pick them up. @@ -213,8 +217,11 @@ def start_ntp(self): if status in (TaskResult.FAILED, TaskResult.CANCELED): if self._retries_left > 0: self._retries_left -= 1 - unreached = [p for p in window - if _pose_dist(robot, p) > self.advance_tol] + start = _closest_wp_index(robot, window) + unreached = window[start:] + if unreached and _pose_dist(robot, unreached[0]) <= self.advance_tol: + unreached = unreached[1:] + if not unreached: self.navigator.get_logger().error( "Retry requested but no poses remain in window") @@ -236,7 +243,8 @@ def start_ntp(self): robot = self._get_robot_pose() # Assess whether we are close to the window’s tail - if _pose_dist(robot, window[-1]) > self.advance_tol: + _, s_left = _closest_pt_and_remaining_len(robot, window) + if s_left > self.advance_tol: continue # Slide the window forward @@ -260,6 +268,12 @@ def start_ntp(self): # Build the forward part of the new window forward = _make_window(tail, remaining, self.seg_len_max)[1:] # skip duplicate ‘tail’ new_window = overlap + forward + + # If new window is empty, trigger success condition + if not new_window: + self.navigator.get_logger().info("Window is empty! Triggering next segment...") + remaining.clear() + continue # Drop from ‘remaining’ exactly the poses we just attached remaining = remaining[len(forward):] diff --git a/src/nav2_tutorial/src/utils/nav_utils.py b/src/nav2_tutorial/src/utils/nav_utils.py index c5a469d..987b0b2 100644 --- a/src/nav2_tutorial/src/utils/nav_utils.py +++ b/src/nav2_tutorial/src/utils/nav_utils.py @@ -1,5 +1,5 @@ import math -from typing import List +from typing import List, Tuple from geometry_msgs.msg import PoseStamped @@ -45,6 +45,43 @@ def _make_window(start: PoseStamped, todo: list[PoseStamped], seg_len_max: float return window +def _closest_pt_and_remaining_len(robot: PoseStamped, + poses: List[PoseStamped]) -> Tuple[Tuple[float,float], float]: + """ + Return (closest_xy, s_remaining) where s_remaining is the arc-length + from the projection point to the last pose in *poses*. + """ + rx, ry = robot.pose.position.x, robot.pose.position.y + + # Degenerate case: the path consists of a single pose + if len(poses) == 1: + px, py = poses[0].pose.position.x, poses[0].pose.position.y + remain = math.hypot(rx - px, ry - py) + return (px, py), remain + + # Locate closest point on the polyline + cumlen = [0.0] + best_d2, best_i, best_t = float("inf"), 0, 0.0 + for a, b in zip(poses[:-1], poses[1:]): + ax, ay = a.pose.position.x, a.pose.position.y + bx, by = b.pose.position.x, b.pose.position.y + vx, vy = bx - ax, by - ay + seg_sq = vx*vx + vy*vy + if seg_sq == 0.0: + continue + t = max(0.0, min(1.0, ((rx-ax)*vx + (ry-ay)*vy) / seg_sq)) + qx, qy = ax + t*vx, ay + t*vy + d2 = (rx-qx)**2 + (ry-qy)**2 + if d2 < best_d2: + best_d2, best_i, best_t = d2, len(cumlen)-1, t + cumlen.append(cumlen[-1] + math.sqrt(seg_sq)) + + # Remaining length from projection to the tail + seg_len = cumlen[best_i+1] - cumlen[best_i] + remain = (1.0 - best_t) * seg_len + (cumlen[-1] - cumlen[best_i+1]) + return (qx, qy), remain + + def _hsv_to_rgb(h: float, s: float = 1.0, v: float = 1.0): """HSV to RGB converter.""" i = int(h * 6.0) % 6 diff --git a/src/nav2_tutorial/src/utils/parser_utils.py b/src/nav2_tutorial/src/utils/parser_utils.py index b80adc5..38396b7 100644 --- a/src/nav2_tutorial/src/utils/parser_utils.py +++ b/src/nav2_tutorial/src/utils/parser_utils.py @@ -4,6 +4,7 @@ import math import yaml import rclpy +from rclpy.time import Time import argparse from pyproj import Transformer from typing import List, Tuple @@ -85,7 +86,7 @@ def _latlon_to_pose(self, lat: float, lon: float, yaw: float) -> PoseStamped: x, y, _ = self._tf_llh2enu.transform(lat, lon, 0.0) pose = PoseStamped() pose.header.frame_id = "map" - pose.header.stamp = rclpy.time.Time().to_msg() + pose.header.stamp = Time().to_msg() pose.pose.position.x = x pose.pose.position.y = y pose.pose.position.z = 0.0 diff --git a/src/nav2_tutorial/src/utils/visualize_gps.py b/src/nav2_tutorial/src/utils/visualize_gps.py new file mode 100644 index 0000000..2221f31 --- /dev/null +++ b/src/nav2_tutorial/src/utils/visualize_gps.py @@ -0,0 +1,56 @@ +import os +import yaml +import rclpy +import argparse +import numpy as np +import matplotlib.pyplot as plt +from src.utils.parser_utils import YamlWaypointParser, _select_yaml + + +def _first_waypoint(path): + with open(path) as f: + return yaml.safe_load(f)["waypoints"][0] + +def load_enu(path, base=None): + """Return two numpy arrays (E, N) in meters.""" + if base is None: + wp0 = _first_waypoint(path) + base = (wp0["latitude"], wp0["longitude"], wp0.get("altitude", 0.0)) + + parser = YamlWaypointParser(path, *base) + poses = parser.get_wps() + east = np.array([p.pose.position.x for p in poses]) + north = np.array([p.pose.position.y for p in poses]) + return east, north + +def main(): + rclpy.init() + ap = argparse.ArgumentParser() + ap.add_argument("yaml_file", nargs="?", + help="Waypoint file (if omitted, auto-selection applies)") + ap.add_argument("--last", action="store_true", + help="Pick the lexically newest logfile in trajectories/") + ap.add_argument("--base", type=float, nargs=3, metavar=("LAT","LON","ALT"), + help="Explicit ENU base point; defaults to first waypoint") + args = ap.parse_args() + + base_path, _ = os.path.splitext(args.yaml_file) + output_path = base_path + ".png" + + yaml_path = _select_yaml(args) + east, north = load_enu(yaml_path, base=args.base) + + fig, ax = plt.subplots(figsize=(10, 8)) + ax.plot(east, north, "-o", markersize=3, linewidth=1.2) + + ax.set_title(os.path.basename(yaml_path)) + ax.set_xlabel("East [m]") + ax.set_ylabel("North [m]") + ax.set_aspect("equal") + ax.grid(True) + plt.tight_layout() + plt.savefig(output_path) + print(f"Saved plot to: {output_path}") + +if __name__ == "__main__": + main() diff --git a/src/nav2_tutorial/src/utils/visualize_gps_yaml.py b/src/nav2_tutorial/src/utils/visualize_gps_yaml.py deleted file mode 100644 index d301eae..0000000 --- a/src/nav2_tutorial/src/utils/visualize_gps_yaml.py +++ /dev/null @@ -1,65 +0,0 @@ -#!/usr/bin/env python3 -import argparse -import yaml -import matplotlib.pyplot as plt -import contextlib -import os - -# Optional map overlay -with contextlib.suppress(ImportError): - import contextily as ctx - -def load_waypoints(yaml_path): - with open(yaml_path, 'r') as f: - data = yaml.safe_load(f) - return data.get('waypoints', []) - -def plot_waypoints(waypoints, output_path, use_map=False): - if not waypoints: - print("No waypoints found.") - return - - lats = [wp['latitude'] for wp in waypoints] - lons = [wp['longitude'] for wp in waypoints] - - fig, ax = plt.subplots(figsize=(10, 8)) - ax.plot(lons, lats, marker='o', linestyle='-', color='blue', label='Trajectory') - ax.set_xlabel("Longitude") - ax.set_ylabel("Latitude") - ax.set_title("GPS Waypoint Trajectory") - ax.axis('equal') - ax.grid(True) - ax.legend() - - if use_map: - try: - import geopandas as gpd - from shapely.geometry import Point - - gdf = gpd.GeoDataFrame( - geometry=[Point(lon, lat) for lon, lat in zip(lons, lats)], - crs="EPSG:4326" - ).to_crs(epsg=3857) - - gdf.plot(ax=ax, marker='o', color='red') - ctx.add_basemap(ax, source=ctx.providers.OpenStreetMap.Mapnik) - except ImportError: - print("Map overlay requires 'geopandas' and 'contextily'. Falling back to plain plot.") - - plt.tight_layout() - plt.savefig(output_path) - print(f"Saved plot to: {output_path}") - -def main(): - parser = argparse.ArgumentParser(description="Visualize GPS waypoints from YAML") - parser.add_argument("yaml_file", help="Path to the YAML file containing waypoints") - parser.add_argument("--map", action="store_true", help="Overlay trajectory on a map (requires geopandas + contextily)") - args = parser.parse_args() - - waypoints = load_waypoints(args.yaml_file) - base_path, _ = os.path.splitext(args.yaml_file) - output_path = base_path + ".png" - plot_waypoints(waypoints, output_path, use_map=args.map) - -if __name__ == "__main__": - main() From 3644598d8559da93c26a304e9bae87df01d4c15a Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 16 Jul 2025 14:17:21 +0200 Subject: [PATCH 18/23] Separated build and running processes in Docker --- .devcontainer/docker-compose.yaml | 59 +++++++++++++++++++------------ scripts/build-container.sh | 11 ++++++ scripts/clean-container.sh | 3 -- scripts/logs-container.sh | 6 +++- scripts/restart-container.sh | 10 +++++- scripts/start-container.sh | 9 ++++- 6 files changed, 69 insertions(+), 29 deletions(-) create mode 100644 scripts/build-container.sh delete mode 100755 scripts/clean-container.sh diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index c4f22bd..b620e11 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -1,7 +1,7 @@ version: '3.8' -services: - vrtk: +services: + builder: build: context: . dockerfile: Dockerfile @@ -10,7 +10,13 @@ services: USER_ID: "${HOST_UID}" GROUP_ID: "${HOST_GID}" BASE_IMAGE: "ros:jazzy-perception" - container_name: ros2_vrtk_container + image: ros2_dev:latest + command: /bin/true # Do nothing, just build + restart: "no" + + runner: + image: ros2_dev:latest + container_name: ros2_runner volumes: - ..:/home/dev/ros_ws - /tmp/.X11-unix:/tmp/.X11-unix @@ -18,26 +24,33 @@ services: network_mode: host environment: - DISPLAY=${DISPLAY} - restart: always + command: > + bash -lc " + set -e && \ + source /home/dev/nav2_env/bin/activate && \ + source /opt/ros/jazzy/setup.bash && \ + colcon build --symlink-install --cmake-args -DBUILD_TESTING=OFF && \ + source install/setup.bash && \ + exec ros2 launch nav2_tutorial all_nodes.launch.py + " + restart: unless-stopped stdin_open: true tty: true - entrypoint: - - bash - - -lc - - | - set -e - - # source virtual environment - source /home/dev/nav2_env/bin/activate - - # source ROS - source /opt/ros/jazzy/setup.bash - - # ensure the workspace is built or latest build is sourced - colcon build --symlink-install --cmake-args -DBUILD_TESTING=OFF - - # source the workspace - source install/setup.bash - # launch everything in one go - exec ros2 launch nav2_tutorial all_nodes.launch.py + vrtk: + build: + context: .. + dockerfile: Dockerfile + args: + USER_NAME: "dev" + USER_ID: "${HOST_UID}" + GROUP_ID: "${HOST_GID}" + BASE_IMAGE: "ros:jazzy-perception" + volumes: + - ..:/home/dev/ros_ws + - /tmp/.X11-unix:/tmp/.X11-unix + network_mode: host + environment: + - DISPLAY=${DISPLAY} + stdin_open: true + tty: true diff --git a/scripts/build-container.sh b/scripts/build-container.sh new file mode 100644 index 0000000..05b5f76 --- /dev/null +++ b/scripts/build-container.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +# Path to the docker-compose.yaml +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +COMPOSE_FILE="$SCRIPT_DIR/../.devcontainer/docker-compose.yaml" + +# Stop and remove all running containers first +docker compose -f "$COMPOSE_FILE" down + +# Build the image for the builder service +docker compose -f "$COMPOSE_FILE" build builder diff --git a/scripts/clean-container.sh b/scripts/clean-container.sh deleted file mode 100755 index a654e31..0000000 --- a/scripts/clean-container.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash -docker compose -f ~/dev/nav2_tutorial/.devcontainer/docker-compose.yaml down -docker compose -f ~/dev/nav2_tutorial/.devcontainer/docker-compose.yaml up --build -d diff --git a/scripts/logs-container.sh b/scripts/logs-container.sh index c6e667d..e19f1a2 100755 --- a/scripts/logs-container.sh +++ b/scripts/logs-container.sh @@ -1,2 +1,6 @@ #!/bin/bash -docker compose -f ~/dev/nav2_tutorial/.devcontainer/docker-compose.yaml logs vrtk +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +COMPOSE_FILE="$SCRIPT_DIR/../.devcontainer/docker-compose.yaml" + +echo "Showing logs for runner service..." +docker compose -f "$COMPOSE_FILE" logs -f runner diff --git a/scripts/restart-container.sh b/scripts/restart-container.sh index fdbd396..983537c 100755 --- a/scripts/restart-container.sh +++ b/scripts/restart-container.sh @@ -1,2 +1,10 @@ #!/bin/bash -docker compose -f ~/dev/nav2_tutorial/.devcontainer/docker-compose.yaml restart vrtk +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +COMPOSE_FILE="$SCRIPT_DIR/../.devcontainer/docker-compose.yaml" + +echo "Restarting runner service..." +docker compose -f "$COMPOSE_FILE" down runner +docker compose -f "$COMPOSE_FILE" up -d runner + +echo "Attaching to runner shell..." +docker compose -f "$COMPOSE_FILE" exec runner bash diff --git a/scripts/start-container.sh b/scripts/start-container.sh index a6b471b..aeea262 100755 --- a/scripts/start-container.sh +++ b/scripts/start-container.sh @@ -1,2 +1,9 @@ #!/bin/bash -docker compose -f ~/dev/nav2_tutorial/.devcontainer/docker-compose.yaml exec vrtk bash +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +COMPOSE_FILE="$SCRIPT_DIR/../.devcontainer/docker-compose.yaml" + +echo "Starting runner service..." +docker compose -f "$COMPOSE_FILE" up -d runner + +echo "Attaching to runner shell..." +docker compose -f "$COMPOSE_FILE" exec runner bash \ No newline at end of file From cd783de2e1b0395f165d93f61d36cdde8e1c5ff7 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 16 Jul 2025 14:52:40 +0200 Subject: [PATCH 19/23] Improvements to loggers --- .../src/loggers/gps_keylogger.py | 131 +++++++++++------- .../src/loggers/gps_periodic_logger.py | 12 +- 2 files changed, 92 insertions(+), 51 deletions(-) diff --git a/src/nav2_tutorial/src/loggers/gps_keylogger.py b/src/nav2_tutorial/src/loggers/gps_keylogger.py index 35df771..5c01fd6 100644 --- a/src/nav2_tutorial/src/loggers/gps_keylogger.py +++ b/src/nav2_tutorial/src/loggers/gps_keylogger.py @@ -1,11 +1,14 @@ -#!/usr/bin/env python3 +from __future__ import annotations + import os import sys +import tty +import math +import time import yaml import select import termios -import tty -import time +import argparse from datetime import datetime import rclpy @@ -24,36 +27,32 @@ class GpsKeyLogger(Node): Press 'f' to log, 'q' to quit. """ - def __init__(self, logging_file_path): + def __init__(self, logging_file_path: str): super().__init__('gps_waypoint_keylogger') self.logging_file_path = logging_file_path - self.last_gps = None + self.last_gps: NavSatFix | None = None self.last_yaw = 0.0 self.last_press_time = 0.0 - self.waypoints = [] - self.flush_threshold = 5 + self.saved_points: int = 0 + self.waypoints: list[dict] = [] + self.flush_threshold = 10 + + self._DIST_TOL_M = 0.01 # Save only if robot moved more than 1 cm + self._DEG2M_LAT = 110_574.0 # Rough conversion at mid-latitudes + self._last_saved_fix: NavSatFix | None = None + self._last_header_ns: int | None = None # Last header stamp [ns] self.gps_topic = '/fixposition/odometry_llh' self.odom_topic = '/fixposition/odometry_enu' - self.create_subscription( - NavSatFix, - self.gps_topic, - self.gps_callback, - 1 - ) - self.create_subscription( - Odometry, - self.odom_topic, - self.yaw_callback, - 1 - ) + self.create_subscription(NavSatFix, self.gps_topic, self.gps_callback, 1) + self.create_subscription(Odometry, self.odom_topic, self.yaw_callback, 1) self.get_logger().info( f"Logging to '{self.logging_file_path}'.\n" f"Subscribed to GPS: {self.gps_topic}, Odometry: {self.odom_topic}.\n" - "Press 'f' to log a waypoint; 'q' to quit." + "Press 'f' to log a waypoint; 'q' to quit." ) def gps_callback(self, msg: NavSatFix) -> None: @@ -63,32 +62,62 @@ def yaw_callback(self, msg: Odometry) -> None: _, _, self.last_yaw = euler_from_quaternion(msg.pose.pose.orientation) def log_waypoint(self) -> None: - if self.last_gps is None: + fix = self.last_gps + if fix is None: self.get_logger().warn("No GPS fix yet; skipping log.") return - if not (-90.0 <= self.last_gps.latitude <= 90.0 and -180.0 <= self.last_gps.longitude <= 180.0): + # Reject out-of-range coordinates + if not (-90.0 <= fix.latitude <= 90.0 and -180.0 <= fix.longitude <= 180.0): self.get_logger().warn("Received invalid GPS coordinates.") return + # Reject out‑of‑order fixes (header time going backwards) + stamp = fix.header.stamp + curr_ns = stamp.sec * 1_000_000_000 + stamp.nanosec + if self._last_header_ns is not None and curr_ns < self._last_header_ns: + delta_us = (self._last_header_ns - curr_ns) / 1_000 + self.get_logger().warn( + f"Skipping out-of-order fix: header_time went backwards by {delta_us:.0f} µs") + return + + if self.saved_points == 0: + self.get_logger().info("Started collecting waypoints …") + + # Distance gate to avoid duplicates / jitter + if self._last_saved_fix is not None: + dlat = (fix.latitude - self._last_saved_fix.latitude) * self._DEG2M_LAT + lon_scale = 111_320.0 * math.cos(math.radians(fix.latitude)) + dlon = (fix.longitude - self._last_saved_fix.longitude) * lon_scale + if math.hypot(dlat, dlon) < self._DIST_TOL_M: + return + + # Store LLH point + self._last_saved_fix = fix + self._last_header_ns = curr_ns + fix_time_iso = datetime.utcfromtimestamp(curr_ns * 1e-9).isoformat() + wp = { - 'latitude': self.last_gps.latitude, - 'longitude': self.last_gps.longitude, - 'altitude': self.last_gps.altitude, + 'latitude': fix.latitude, + 'longitude': fix.longitude, + 'altitude': fix.altitude, 'yaw': self.last_yaw, + 'header_time': fix_time_iso, 'logged_at': datetime.now().isoformat() } + self.waypoints.append(wp) + self.saved_points += 1 + if len(self.waypoints) >= self.flush_threshold: + self.flush_to_disk() + self.get_logger().info( f"Logged waypoint #{len(self.waypoints)}: " f"lat={wp['latitude']:.6f}, lon={wp['longitude']:.6f}, yaw={wp['yaw']:.2f}" ) - if len(self.waypoints) >= self.flush_threshold: - self.flush_waypoints_to_disk() - - def flush_waypoints_to_disk(self) -> None: + def flush_to_disk(self) -> None: try: with open(self.logging_file_path, 'r') as f: data = yaml.safe_load(f) or {} @@ -98,9 +127,9 @@ def flush_waypoints_to_disk(self) -> None: self.get_logger().error(f"Failed to read log file: {e}") return - wps = data.get('waypoints', []) - wps.extend(self.waypoints) - data['waypoints'] = wps + existing = data.get('waypoints', []) + existing.extend(self.waypoints) + data['waypoints'] = existing tmp_path = f"{self.logging_file_path}.tmp" try: @@ -113,25 +142,31 @@ def flush_waypoints_to_disk(self) -> None: self.get_logger().error(f"Failed to write log file: {e}") -def stdin_ready(): - return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) +def _default_yaml_path() -> str: + share_dir = get_package_share_directory('nav2_tutorial') + ws_root = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(share_dir)))) + src_traj = os.path.join(ws_root, 'src', 'nav2_tutorial', 'trajectories') + target_dir = src_traj if os.path.isdir(src_traj) else os.path.join(share_dir, 'trajectories') + os.makedirs(target_dir, exist_ok=True) + ts = datetime.now().strftime('%Y%m%d_%H%M%S') + return os.path.join(target_dir, f'gps_waypoints_{ts}.yaml') -def main(argv=None): - rclpy.init(args=argv) - if len(sys.argv) > 1: - yaml_path = sys.argv[1] - else: - share_dir = get_package_share_directory('nav2_tutorial') - ws_root = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(share_dir)))) - src_config = os.path.join(ws_root, 'src', 'nav2_tutorial', 'trajectories') +def _parse_arguments(argv: list[str] | None = None) -> tuple[str, float]: + parser = argparse.ArgumentParser(description="Periodic GPS Waypoint Logger") + parser.add_argument('yaml', nargs='?', help='Path to the YAML log file.') + args = parser.parse_args(argv) + return args.yaml or _default_yaml_path() + - traj_dir = src_config if os.path.isdir(src_config) else os.path.join(share_dir, 'trajectories') - os.makedirs(traj_dir, exist_ok=True) +def _stdin_ready(): + return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) - stamp = datetime.now().strftime('%Y%m%d_%H%M%S') - yaml_path = os.path.join(traj_dir, f'gps_waypoints_{stamp}.yaml') +def main(argv=None): + yaml_path = _parse_arguments(argv) + + rclpy.init(args=argv) node = GpsKeyLogger(yaml_path) fd = sys.stdin.fileno() @@ -141,7 +176,7 @@ def main(argv=None): try: while rclpy.ok(): rclpy.spin_once(node, timeout_sec=0.1) - if stdin_ready(): + if _stdin_ready(): now = time.time() c = sys.stdin.read(1) if c == 'f' and (now - node.last_press_time > 0.2): @@ -155,7 +190,7 @@ def main(argv=None): finally: if node.waypoints: node.get_logger().info("Saving remaining waypoints before exit...") - node.flush_waypoints_to_disk() + node.flush_to_disk() termios.tcsetattr(fd, termios.TCSADRAIN, old) rclpy.shutdown() diff --git a/src/nav2_tutorial/src/loggers/gps_periodic_logger.py b/src/nav2_tutorial/src/loggers/gps_periodic_logger.py index 25d27db..78c985e 100644 --- a/src/nav2_tutorial/src/loggers/gps_periodic_logger.py +++ b/src/nav2_tutorial/src/loggers/gps_periodic_logger.py @@ -25,6 +25,10 @@ stop_requested = False class GpsPeriodicLogger(Node): + """ + ROS2 node to log GPS waypoints to a YAML file based on timer. Press 'q' to quit. + """ + def __init__(self, logging_file_path: str, interval: float): super().__init__('gps_waypoint_periodic_logger') @@ -44,7 +48,7 @@ def __init__(self, logging_file_path: str, interval: float): self.create_subscription(NavSatFix, '/fixposition/odometry_llh', self.gps_callback, qos_profile=1) self.create_subscription(Odometry, '/fixposition/odometry_enu', self.yaw_callback, qos_profile=1) - self.create_timer(self.log_interval, self._log_waypoint) + self.create_timer(self.log_interval, self.log_waypoint) self.create_timer(2.0, self._status_message) self.get_logger().info( @@ -57,13 +61,15 @@ def gps_callback(self, msg: NavSatFix) -> None: def yaw_callback(self, msg: Odometry) -> None: _, _, self.last_yaw = euler_from_quaternion(msg.pose.pose.orientation) - def _log_waypoint(self) -> None: + def log_waypoint(self) -> None: fix = self.last_gps if fix is None: + self.get_logger().warn("No GPS fix yet; skipping log.") return - # Reject impossible coordinates early + # Reject out-of-range coordinates if not (-90 <= fix.latitude <= 90 and -180 <= fix.longitude <= 180): + self.get_logger().warn("Received invalid GPS coordinates.") return # Reject out‑of‑order fixes (header time going backwards) From 35ca00abcc7fb5351bcaab54dcec0c63ed0631ce Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 16 Jul 2025 15:02:51 +0200 Subject: [PATCH 20/23] Added safety measure for out of order messages --- src/nav2_tutorial/src/precise_wp_follower.py | 14 +++++++++++++- src/nav2_tutorial/src/smooth_wp_follower.py | 14 +++++++++++++- 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/src/nav2_tutorial/src/precise_wp_follower.py b/src/nav2_tutorial/src/precise_wp_follower.py index 27363c5..5f696eb 100644 --- a/src/nav2_tutorial/src/precise_wp_follower.py +++ b/src/nav2_tutorial/src/precise_wp_follower.py @@ -30,6 +30,7 @@ def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1) self.num_loop = max(1, num_loop) # Odometry topic with ENU pose in map frame + self._last_odom_ns: int | None = None self._last_odom_pose: PoseStamped | None = None self.navigator.create_subscription(Odometry, "/fixposition/odometry_enu", self._odom_cb, 10) @@ -157,7 +158,18 @@ def start_ntp(self) -> None: self.navigator.get_logger().info("All waypoints completed - mission finished") def _odom_cb(self, msg: Odometry) -> None: - """Cache the latest odometry pose (PoseStamped).""" + """Cache the latest odometry pose, skipping out-of-order messages.""" + stamp = msg.header.stamp + curr_ns = stamp.sec * 1_000_000_000 + stamp.nanosec + + # Reject if header time went backwards + if self._last_odom_ns is not None and curr_ns < self._last_odom_ns: + self.navigator.get_logger().warn( + "Skipping out-of-order odometry message (time jumped backwards)" ) + return + + # Accept the pose + self._last_odom_ns = curr_ns ps = PoseStamped() ps.header = msg.header ps.pose = msg.pose.pose diff --git a/src/nav2_tutorial/src/smooth_wp_follower.py b/src/nav2_tutorial/src/smooth_wp_follower.py index bde8dbe..4e03b99 100644 --- a/src/nav2_tutorial/src/smooth_wp_follower.py +++ b/src/nav2_tutorial/src/smooth_wp_follower.py @@ -35,6 +35,7 @@ def __init__(self, wps_file_path: str, reverse: bool = False, num_loop: int = 1, self.stopping = stopping # Odometry topic with ENU pose in map frame + self._last_odom_ns: int | None = None self._last_odom_pose: PoseStamped | None = None self.navigator.create_subscription(Odometry, "/fixposition/odometry_enu", self._odom_cb, 10) @@ -317,7 +318,18 @@ def start_ntp(self): window = new_window def _odom_cb(self, msg: Odometry) -> None: - """Cache the latest odometry pose (PoseStamped).""" + """Cache the latest odometry pose, skipping out-of-order messages.""" + stamp = msg.header.stamp + curr_ns = stamp.sec * 1_000_000_000 + stamp.nanosec + + # Reject if header time went backwards + if self._last_odom_ns is not None and curr_ns < self._last_odom_ns: + self.navigator.get_logger().warn( + "Skipping out-of-order odometry message (time jumped backwards)" ) + return + + # Accept the pose + self._last_odom_ns = curr_ns ps = PoseStamped() ps.header = msg.header ps.pose = msg.pose.pose From 472dc8d61d92daeb04d7f1c9a1776d81cf02f143 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 16 Jul 2025 16:40:01 +0200 Subject: [PATCH 21/23] Overall improvements and documentation --- README.md | 88 +++++++++++++++++++ src/nav2_tutorial/setup.py | 3 + src/nav2_tutorial/src/dashboard/dashboard.py | 4 +- .../src/loggers/gps_periodic_logger.py | 2 +- src/nav2_tutorial/src/precise_wp_follower.py | 11 ++- src/nav2_tutorial/src/smooth_wp_follower.py | 11 ++- 6 files changed, 111 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index ac36fe0..fb46365 100644 --- a/README.md +++ b/README.md @@ -263,6 +263,13 @@ Or alternatively use the `--mapviz` flag on the `gps_waypoint_follower.launch.py # Debugging trajectories To analyze the trajectories performed by the robot and determine if any issues have occured, the user can perform a recording of all topics by running `ros2 bag record --all`. Then, launch RViz2 with the provided configuration file to easily visualize the debug topics containing the waypoint coordinates, the global and local plans, the costmaps, among other things: `ros2 run rviz2 rviz2 -d /home/dev/ros_ws/src/nav2_tutorial/config/nav2_plan_viz.rviz`. + +# Dashboard +In addition, this repository provides an interactive dashboard to control all of these processes. It can be started as follows: +```bash +ros2 run nav2_tutorial dashboard +``` + # Recommendations We suggest employing tmux for controlling the robot via SSH, as intermittent disconnections may kill the running process. Next you'll find some basic steps to get familiar with tmux: @@ -298,3 +305,84 @@ To reattach to the tmux session, simply use: tmux attach -t mysession ``` You’re now back inside the process, exactly where you left off. + + +# 📖 Helper Scripts Overview + +This project uses a set of bash scripts to manage the Docker ROS2 environment. Here’s what each script does: + +--- + +### 🛠 **`build-container.sh`** + +* **Purpose:** Builds the Docker image using the `builder` service. +* **What it does:** + + * Stops any running containers. + * Rebuilds the Docker image from the Dockerfile. +* **Usage:** + + ```bash + bash scripts/build-container.sh + ``` + +--- + +### 🚀 **`start-container.sh`** + +* **Purpose:** Starts the `runner` container and optionally attaches to its shell. +* **What it does:** + + * Starts the `runner` container in detached mode (keeps `all_nodes.launch.py` running). + * Drops you into a shell inside the container automatically. +* **Usage:** + + ```bash + bash scripts/start-container.sh + ``` + + This will: + + * Start `all_nodes.launch.py` (if not already running). + * Attach you to a shell inside the container. + +--- + +### 🔄 **`restart-container.sh`** + +* **Purpose:** Restarts the `runner` container cleanly. +* **What it does:** + + * Stops the `runner` container. + * Starts it again, automatically relaunching `all_nodes.launch.py`. + * Attaches to a shell in the container after restart. +* **Usage:** + + ```bash + bash scripts/restart-container.sh + ``` + +--- + +### 📜 **`logs-container.sh`** + +* **Purpose:** Streams logs from the `runner` container. +* **What it does:** + + * Shows all output from `all_nodes.launch.py` and other processes in the container. +* **Usage:** + + ```bash + bash scripts/logs-container.sh + ``` + +--- + +## 📝 Notes + +* The `runner` container is designed to persist and automatically restart on system reboot. +* To get a shell inside the container without restarting it: + + ```bash + docker compose exec runner bash + ``` diff --git a/src/nav2_tutorial/setup.py b/src/nav2_tutorial/setup.py index 6dd4881..724fa18 100644 --- a/src/nav2_tutorial/setup.py +++ b/src/nav2_tutorial/setup.py @@ -41,6 +41,9 @@ # Visualizer 'visualize_gps = src.utils.visualize_gps:main', + + # Dashboard + 'dashboard = src.dashboard.dashboard:main', ], }, ) diff --git a/src/nav2_tutorial/src/dashboard/dashboard.py b/src/nav2_tutorial/src/dashboard/dashboard.py index 31c1a37..e4a1cd4 100644 --- a/src/nav2_tutorial/src/dashboard/dashboard.py +++ b/src/nav2_tutorial/src/dashboard/dashboard.py @@ -435,6 +435,8 @@ def handle_map_click(clickData, current_mode): return {"lat": lat, "lon": lon}, dbc.Alert(f"Sent goal: lat={lat:.6f}, lon={lon:.6f}", color="info") return dash.no_update, dash.no_update +def main(): + app.run(debug=True, host="0.0.0.0", port=8055) if __name__ == "__main__": - app.run(debug=True, host="0.0.0.0", port=8055) + main() diff --git a/src/nav2_tutorial/src/loggers/gps_periodic_logger.py b/src/nav2_tutorial/src/loggers/gps_periodic_logger.py index 78c985e..3bf9972 100644 --- a/src/nav2_tutorial/src/loggers/gps_periodic_logger.py +++ b/src/nav2_tutorial/src/loggers/gps_periodic_logger.py @@ -38,7 +38,7 @@ def __init__(self, logging_file_path: str, interval: float): self.last_yaw: float = 0.0 self.saved_points: int = 0 self.waypoints: list[dict] = [] - self.flush_threshold = 10 + self.flush_threshold = 100 self._DIST_TOL_M = 0.01 # Save only if robot moved more than 1 cm self._DEG2M_LAT = 110_574.0 # Rough conversion at mid-latitudes diff --git a/src/nav2_tutorial/src/precise_wp_follower.py b/src/nav2_tutorial/src/precise_wp_follower.py index 5f696eb..5a21d81 100644 --- a/src/nav2_tutorial/src/precise_wp_follower.py +++ b/src/nav2_tutorial/src/precise_wp_follower.py @@ -110,9 +110,14 @@ def start_ntp(self) -> None: if self.num_loop == 1: # Fast-forward to the first pose within start_wp_tol - closest_idx = _closest_wp_index(robot, remaining) - if _pose_dist(robot, remaining[closest_idx]) > self.start_wp_tol: - closest_idx = 0 # fall back to the first wp + first_inside = None + for i, pose in enumerate(remaining): + if _pose_dist(robot, pose) <= self.start_wp_tol: + first_inside = i + break + + # Fall back to the very beginning if none are close enough + closest_idx = first_inside if first_inside is not None else 0 if closest_idx: self._visited_wps += closest_idx # statistics diff --git a/src/nav2_tutorial/src/smooth_wp_follower.py b/src/nav2_tutorial/src/smooth_wp_follower.py index 4e03b99..db9b251 100644 --- a/src/nav2_tutorial/src/smooth_wp_follower.py +++ b/src/nav2_tutorial/src/smooth_wp_follower.py @@ -132,9 +132,14 @@ def start_ntp(self): if self.num_loop == 1: # Fast-forward to the first pose within start_seg_tol - closest_idx = _closest_wp_index(robot, remaining) - if _pose_dist(robot, remaining[closest_idx]) > self.start_seg_tol: - closest_idx = 0 + first_inside = None + for i, pose in enumerate(remaining): + if _pose_dist(robot, pose) <= self.start_seg_tol: + first_inside = i + break + + # Fall back to the very beginning if none are close enough + closest_idx = first_inside if first_inside is not None else 0 if closest_idx: self._visited_wps += closest_idx From 500fdf92fb4cfdbb5b1e44f6423f4b95df64932b Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 16 Jul 2025 16:44:01 +0200 Subject: [PATCH 22/23] Added small warning --- README.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index fb46365..6dccb30 100644 --- a/README.md +++ b/README.md @@ -235,19 +235,19 @@ ros2 launch nav2_tutorial gps_waypoint_follower.launch.py ``` Note: You can use the `all_nodes.launch.py` file to achieve this. For navigation, this repository provides three waypoint following methods: Precise, Smooth, and Interactive. We can only choose one method each time we execute it. -* Precise GPS Waypoint Follower -The `precise_wp_follower` script streams all the logged points from a YAML file and makes the robot follow them point-by-point, stopping at every waypoint when the accuracy threshold is met. +* Smooth GPS Waypoint Follower (Recommended): +The `smooth_wp_follower` script streams all the logged points from a YAML file and divides them in segments, pre-computing a smooth trajectory for the robot to follow. This ensure constant speed throughout the waypoint following task. ```bash -ros2 run nav2_tutorial precise_wp_follower +ros2 run nav2_tutorial smooth_wp_follower ``` -* Smooth GPS Waypoint Follower -The `smooth_wp_follower` script streams all the logged points from a YAML file and divides them in segments, pre-computing a smooth trajectory for the robot to follow. This ensure constant speed throughout the waypoint following task. +* Precise GPS Waypoint Follower: +The `precise_wp_follower` script streams all the logged points from a YAML file and makes the robot follow them point-by-point, stopping at every waypoint when the accuracy threshold is met. Note: Avoid combining this method with the periodic logger at high rates. Logging too frequently creates waypoints that are very close together, which can cause the robot to stop and start excessively at each point, leading to unstable behavior. ```bash -ros2 run nav2_tutorial smooth_wp_follower +ros2 run nav2_tutorial precise_wp_follower ``` -* Interactive GPS Waypoint Follower +* Interactive GPS Waypoint Follower: The `interactive_wp_follower` script listens to the mapviz topic for the next waypoint objective. ```bash ros2 run nav2_tutorial interactive_wp_follower From cdf03b2c0bf6ab746a326e8da5ee5cb52120a72d Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 16 Jul 2025 17:12:42 +0200 Subject: [PATCH 23/23] Dashboard updates --- .../src/dashboard/current_pose_logger.py | 29 ++++++++++++ src/nav2_tutorial/src/dashboard/dashboard.py | 47 +++++++++++++++---- 2 files changed, 66 insertions(+), 10 deletions(-) create mode 100644 src/nav2_tutorial/src/dashboard/current_pose_logger.py diff --git a/src/nav2_tutorial/src/dashboard/current_pose_logger.py b/src/nav2_tutorial/src/dashboard/current_pose_logger.py new file mode 100644 index 0000000..29603c5 --- /dev/null +++ b/src/nav2_tutorial/src/dashboard/current_pose_logger.py @@ -0,0 +1,29 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import NavSatFix +import json + +OUTPUT_PATH = '/tmp/current_position.json' + +class PositionLogger(Node): + def __init__(self): + super().__init__('position_logger') + self.subscription = self.create_subscription( + NavSatFix, + '/fixposition/odometry_llh', + self.listener_callback, + 10) + def listener_callback(self, msg): + pos = {'lat': msg.latitude, 'lon': msg.longitude} + with open(OUTPUT_PATH, 'w') as f: + json.dump(pos, f) + +def main(): + rclpy.init() + node = PositionLogger() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/nav2_tutorial/src/dashboard/dashboard.py b/src/nav2_tutorial/src/dashboard/dashboard.py index e4a1cd4..d8881e3 100644 --- a/src/nav2_tutorial/src/dashboard/dashboard.py +++ b/src/nav2_tutorial/src/dashboard/dashboard.py @@ -4,6 +4,7 @@ import os import subprocess import glob +import json import threading import queue import yaml @@ -17,6 +18,16 @@ SMOOTH_SCRIPT = '/home/dev/ros_ws/src/nav2_tutorial/src/smooth_wp_follower.py' INTERACTIVE_SCRIPT = '/home/dev/ros_ws/src/nav2_tutorial/src/interactive_wp_follower.py' CLICKED_SCRIPT = "/home/dev/ros_ws/src/nav2_tutorial/src/dashboard/send_clicked_point.py" +LIVE_CURR_POSE = '/tmp/current_position.json' + + +def load_current_position(): + try: + with open(LIVE_CURR_POSE, 'r') as f: + pos = json.load(f) + return pos['lat'], pos['lon'] + except Exception: + return None, None def get_trajectories(): return sorted(glob.glob(os.path.join(TRAJECTORY_DIR, '*.yaml'))) @@ -158,6 +169,7 @@ def get_combined_live_log(): dcc.Store(id="reverse-store", data=False), dcc.Store(id="clicked-point", data=None), dcc.Store(id="mode-store", data="precise"), + dcc.Interval(id='live-position-interval', interval=200, n_intervals=0) ]) # ----- CALLBACKS ----- @@ -169,9 +181,12 @@ def get_combined_live_log(): Input("using-last-store", "data"), Input("show-orientation", "value"), Input("clicked-point", "data"), + Input('live-position-interval', 'n_intervals'), State("mode-store", "data"), ) -def update_trajectory_map(selected_file, using_last, show_orientation, clicked_point, mode): +def update_trajectory_map(selected_file, using_last, show_orientation, clicked_point, mode, n_intervals): + data = [] + if using_last or selected_file == "USING_LAST": files = get_trajectories() if not files: @@ -187,15 +202,27 @@ def update_trajectory_map(selected_file, using_last, show_orientation, clicked_p "data": [], "layout": {"title": "No trajectory selected", "height": 500} } + + # Load live pose + lat, lon = load_current_position() + if lat is not None and lon is not None: + data.append({ + "type": "scattermapbox", + "lat": [lat], + "lon": [lon], + "mode": "markers", + "marker": {"size": 12, "color": "lime"}, + "name": "Current Position" + }) + + # Load trajectory data lats, lons, yaws = load_trajectory_from_yaml(yaml_path) if not lats or not lons: return { "data": [], "layout": {"title": "No waypoints in file", "height": 500} } - - # Trajectory data - data = [{ + data.append({ "type": "scattermapbox", "lat": lats, "lon": lons, @@ -203,17 +230,17 @@ def update_trajectory_map(selected_file, using_last, show_orientation, clicked_p "marker": {"size": 8, "color": "red"}, "line": {"width": 2, "color": "blue"}, "name": "Trajectory" - }] + }) # Add orientation arrows if requested if show_orientation: arrow_scale = 0.000009 arrow_lats, arrow_lons = [], [] - for lat, lon, yaw in zip(lats, lons, yaws): + for lat_val, lon_val, yaw in zip(lats, lons, yaws): dlat = math.cos(yaw) * arrow_scale - dlon = math.sin(yaw) * arrow_scale / max(math.cos(math.radians(lat)), 1e-6) - arrow_lats += [lat, lat + dlat, None] - arrow_lons += [lon, lon + dlon, None] + dlon = math.sin(yaw) * arrow_scale / max(math.cos(math.radians(lat_val)), 1e-6) + arrow_lats += [lat_val, lat_val + dlat, None] + arrow_lons += [lon_val, lon_val + dlon, None] data.append({ "type": "scattermapbox", "lat": arrow_lats, @@ -235,7 +262,7 @@ def update_trajectory_map(selected_file, using_last, show_orientation, clicked_p lat_center=lat_center, lon_center=lon_center, lat_range=0.0003, # Adjust as needed (≈32m at mid latitudes) - lon_range=0.0005, # ≈40m + lon_range=0.0005, # ≈40m n=60 # 40x40 = 1600 clickable points ) data.append({