diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 67b7ec3..9cf0edd 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 && pip install 'setuptools<69' 'pip<24'" + +# 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,10 @@ 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 +ENV PYTHONPATH=/home/dev/nav2_env/lib/python3.12/site-packages:$PYTHONPATH + # 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..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,23 +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 ROS - source /opt/ros/jazzy/setup.bash - # ensure the workspace is built or latest build is sourced - colcon build --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/.devcontainer/requirements.txt b/.devcontainer/requirements.txt new file mode 100644 index 0000000..17fff89 --- /dev/null +++ b/.devcontainer/requirements.txt @@ -0,0 +1,11 @@ +catkin_pkg +empy==3.3.4 +lark +numpy +pyyaml +tk +matplotlib +geopandas +pyproj +dash +dash-bootstrap-components \ No newline at end of file diff --git a/README.md b/README.md index 7ad4352..6dccb30 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 ``` @@ -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: @@ -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) @@ -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: @@ -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 @@ -258,3 +258,131 @@ 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`. + + +# 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: + +### 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. + + +# πŸ“– 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/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/logs-container.sh b/scripts/logs-container.sh new file mode 100755 index 0000000..e19f1a2 --- /dev/null +++ b/scripts/logs-container.sh @@ -0,0 +1,6 @@ +#!/bin/bash +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 new file mode 100755 index 0000000..983537c --- /dev/null +++ b/scripts/restart-container.sh @@ -0,0 +1,10 @@ +#!/bin/bash +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_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/scripts/start-container.sh b/scripts/start-container.sh new file mode 100755 index 0000000..aeea262 --- /dev/null +++ b/scripts/start-container.sh @@ -0,0 +1,9 @@ +#!/bin/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 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/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 diff --git a/src/nav2_tutorial/setup.py b/src/nav2_tutorial/setup.py index d46c886..724fa18 100644 --- a/src/nav2_tutorial/setup.py +++ b/src/nav2_tutorial/setup.py @@ -38,6 +38,12 @@ # 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', + + # Dashboard + 'dashboard = src.dashboard.dashboard:main', ], }, ) 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 new file mode 100644 index 0000000..d8881e3 --- /dev/null +++ b/src/nav2_tutorial/src/dashboard/dashboard.py @@ -0,0 +1,469 @@ +import dash +import dash_bootstrap_components as dbc +from dash import html, dcc, Output, Input, State, ctx +import os +import subprocess +import glob +import json +import threading +import queue +import yaml +import math +import numpy as np + +# ----- 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/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'))) + +def load_trajectory_from_yaml(yaml_path): + try: + 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 [], [], [] + +# --- 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 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, + text=True, + bufsize=1 + ) + try: + for line in process.stdout: + process_log_queue.put(line) + except Exception as e: + 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() + +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_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]) + +app.layout = dbc.Container([ + html.H2("Trajectory Dashboard"), + dbc.Row([ + dbc.Col([ + dcc.Graph(id="trajectory-map", style={"height": "520px", "margin-bottom": "8px"}), + 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([ + dbc.Col([ + html.H4("Trajectory Controls"), + 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": "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"}), + ], width=3), + dbc.Col([ + 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([ + 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), + ]), + 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"), + dcc.Interval(id='live-position-interval', interval=200, n_intervals=0) +]) + +# ----- CALLBACKS ----- + +# Trajectory map +@app.callback( + Output("trajectory-map", "figure"), + Input("trajectory-dropdown", "value"), + 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, n_intervals): + data = [] + + 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} + } + + # 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} + } + data.append({ + "type": "scattermapbox", + "lat": lats, + "lon": lons, + "mode": "lines+markers", + "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_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_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, + "lon": arrow_lons, + "mode": "lines", + "line": {"width": 2, "color": "orange"}, + "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({ + "type": "scattermapbox", + "lat": [clicked_point["lat"]], + "lon": [clicked_point["lon"]], + "mode": "markers", + "marker": { + "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": 18, + }, + "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"), + Output("trajectory-dropdown", "value"), + Output("trajectory-dropdown", "options"), + 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" + 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: launches follower +@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 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" + elif dropdown_value: + arg = dropdown_value + traj_display = os.path.basename(dropdown_value) + else: + return dbc.Alert("Please select a trajectory!", color="warning") + 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: + start_process(cmd, "follower") + 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 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 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 logger or follower +@app.callback( + Output("status", "children", allow_duplicate=True), + Input("stop-process-btn", "n_clicks"), + prevent_initial_call=True +) +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") + +# Combined live output +@app.callback( + Output('live-output', 'children'), + Input('live-output-interval', 'n_intervals') +) +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 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)]) + +@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 + +def main(): + app.run(debug=True, host="0.0.0.0", port=8055) + +if __name__ == "__main__": + main() 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): 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 479b5ea..3bf9972 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 @@ -7,6 +7,7 @@ import time import yaml import select +import signal import termios import argparse from datetime import datetime @@ -20,8 +21,14 @@ from src.utils.gps_utils import euler_from_quaternion +# Global variables +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') @@ -31,16 +38,17 @@ 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 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) - 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( @@ -53,17 +61,30 @@ 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 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) + 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)) @@ -71,14 +92,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) @@ -121,6 +146,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)))) @@ -130,6 +156,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.') @@ -137,9 +164,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) @@ -150,8 +184,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) diff --git a/src/nav2_tutorial/src/precise_wp_follower.py b/src/nav2_tutorial/src/precise_wp_follower.py index 655092d..5a21d81 100644 --- a/src/nav2_tutorial/src/precise_wp_follower.py +++ b/src/nav2_tutorial/src/precise_wp_follower.py @@ -1,178 +1,265 @@ -import os import sys -import time -import math -import yaml -import glob import argparse -from datetime import datetime +from uuid import UUID +# ROS packages import rclpy -from nav2_simple_commander.robot_navigator import BasicNavigator -from ament_index_python.packages import get_package_share_directory - -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 +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 Path, Odometry +from visualization_msgs.msg import Marker, MarkerArray + +# Custom utils +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: - """Use the Nav2 *GPS Waypoint Follower* to follow a set of waypoints.""" + """Follow a precise 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) -> None: 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.reverse = reverse + 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) + + # 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 GoToPose task + 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 + 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 = 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: + 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") + + self.navigator.destroy_subscription(sub) + return datum + + 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") - 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") + + # 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 + + # Skip early waypoints already within tolerance + robot = self._get_robot_pose() + + if self.num_loop == 1: + # Fast-forward to the first pose within start_wp_tol + 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 + 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}") + + # 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 + + # Exceeded retries β†’ abort entire mission + self.navigator.get_logger().error( + f"Waypoint {wp_global_idx} failed after retry - aborting mission") + raise RuntimeError("Navigation failed") + + self._visited_wps += 1 + + self.navigator.get_logger().info("All waypoints completed - mission finished") + + def _odom_cb(self, msg: Odometry) -> None: + """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 + 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 _publish_full_traj(self, poses: list[PoseStamped]) -> None: + """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 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_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 print(f"Loading waypoints file: {yaml_file_path}") - try: - gps_wpf = GpsWpCommander(yaml_file_path, reverse=args.reverse) - gps_wpf.start_wpf() + 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__": diff --git a/src/nav2_tutorial/src/smooth_wp_follower.py b/src/nav2_tutorial/src/smooth_wp_follower.py index 139f853..db9b251 100644 --- a/src/nav2_tutorial/src/smooth_wp_follower.py +++ b/src/nav2_tutorial/src/smooth_wp_follower.py @@ -1,271 +1,80 @@ -import os import sys -import math -import yaml -import glob import argparse +from uuid import UUID +# ROS packages 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 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 - -# Visualization -from nav_msgs.msg import Path from visualization_msgs.msg import Marker, MarkerArray -from rclpy.qos import QoSProfile, DurabilityPolicy -MARKER_VIZ = True +# Custom utils +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 -def _hsv_to_rgb(h, s=1.0, v=1.0): - """h ∈ [0,1] β†’ (r,g,b) ∈ [0,1]Β³ (simple HSVβ†’RGB)""" - 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 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.""" - 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 - -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 +# Visualization +MARKER_VIZ = True class GpsWpCommander: """Follow a smooth sequence of GPS waypoints.""" - def __init__(self, wps_file_path: str, reverse: bool = False): - self.navigator = BasicNavigator("basic_navigator") - self.wp_parser = YamlWaypointParser(wps_file_path) - self.reverse = reverse + 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 + 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) - # 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 for cart - "+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 + # 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 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 self._last_goal_uuid: UUID | None = None # 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 - self._fp_client = None + # 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 # 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 + # Optional: 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 - self.full_traj_pub = self.navigator.create_publisher( + # 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) @@ -273,266 +82,259 @@ 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 - + 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) 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 - 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: - 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 - 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 - return pose + self.navigator.destroy_subscription(sub) + return datum def start_ntp(self): + """Start the GoThroughPoses 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 - # ------------------------------------------------------------ - 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"]) - ] + # 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) - 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:] + # 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"Skipped {skipped} waypoint(s); " - f"starting with wp #{skipped+1}/{self._total_wps}") + f"Preparing segments for trajectory of {total_len:0.2f} m " + f"consisting of {self._total_wps} waypoints…") + remaining = all_poses + seg_idx = 0 - # if everything was within the tolerance, we’re already done! - if not remaining: - self.navigator.get_logger().info("Trajectory already completed.") - return + # Prime the first window + robot = self._get_robot_pose() - 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.num_loop == 1: + # Fast-forward to the first pose within start_seg_tol + first_inside = None + for i, pose in enumerate(remaining): + if _pose_dist(robot, pose) <= self.start_seg_tol: + first_inside = i + break - # ------------------------------------------------------------ - # 3) Continuous streaming loop - # ------------------------------------------------------------ - while rclpy.ok(): - rclpy.spin_once(self.navigator, timeout_sec=0.05) + # Fall back to the very beginning if none are close enough + closest_idx = first_inside if first_inside is not None else 0 - 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 + if closest_idx: + self._visited_wps += closest_idx + 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 = _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 + 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 + + # 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() + _, 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 (s_left <= self.advance_tol) and not remaining: + self.navigator.get_logger().info("Trajectory finished") + break + else: + # Success was premature: resend the unreached tail + 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. + 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 + + # 2) FAILED or CANCELLED: Retry the segment + if status in (TaskResult.FAILED, TaskResult.CANCELED): + if self._retries_left > 0: + self._retries_left -= 1 + 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") + 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 - 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: + 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 - - # -------- overlap: walk *backwards* inside the old window -------- - dist_rt = self._dist(robot, tail) # robot β†’ tail + # Assess whether we are close to the window’s tail + _, s_left = _closest_pt_and_remaining_len(robot, window) + if s_left > self.advance_tol: + continue + + # Slide the window forward + tail = window[-1] - 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: walk backwards inside the old window + dist_rt = _pose_dist(robot, tail) - if not remaining: - continue # path finished; let Nav2 coast to the end + overlap = [] + for p in reversed(window): + 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 + overlap.reverse() - # -------- build the forward part of the new window --------------- - forward = self._make_window(tail, remaining)[1:] # skip dup β€˜tail’ - new_window = overlap + forward + if not remaining: + continue # path finished; let Nav2 coast to the end - # 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) + # 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 - # # ───────────────────────────────────────────────────────────── - # # 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 + # 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):] + + # Log stats before cancel / send goal + self._visited_wps += len(forward) + 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 + seg_idx += 1 + window = new_window def _odom_cb(self, msg: Odometry) -> None: - """Cache the most recent odometry pose as a `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 @@ -543,8 +345,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: @@ -554,8 +356,23 @@ def _get_robot_pose(self) -> PoseStamped: rclpy.spin_once(self.navigator, timeout_sec=0.05) return self._last_odom_pose - - def _publish_waypoints(self, poses: list[PoseStamped], seg_idx: int): + + 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_waypoints(self, poses: list[PoseStamped], seg_idx: int) -> None: + """Publish the current segment as a Path and MarkerArray.""" # Path message path = Path() path.header.frame_id = "map" @@ -575,11 +392,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 @@ -596,46 +412,8 @@ def _publish_waypoints(self, poses: list[PoseStamped], seg_idx: int): 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: - 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 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 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: - seg_len = self._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" @@ -669,40 +447,36 @@ 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: - 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): + """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)") + parser.add_argument("--stopping", action="store_true", help="Stop at the end of every segment") args = parser.parse_args() 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 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, stopping=args.stopping) gps_wpf.start_ntp() 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 new file mode 100644 index 0000000..987b0b2 --- /dev/null +++ b/src/nav2_tutorial/src/utils/nav_utils.py @@ -0,0 +1,115 @@ +import math +from typing import List, Tuple +from geometry_msgs.msg import PoseStamped + + +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(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(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(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(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 _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 + 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_ntp_uuid" # Iron/Jazzy + "_current_nav_through_poses_uuid", # Humble, Galactic + ): + 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 new file mode 100644 index 0000000..38396b7 --- /dev/null +++ b/src/nav2_tutorial/src/utils/parser_utils.py @@ -0,0 +1,197 @@ +import os +import copy +import glob +import math +import yaml +import rclpy +from rclpy.time import Time +import argparse +from pyproj import Transformer +from typing import List, Tuple +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, 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) + + 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}'.") + + # 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: + """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 _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 = 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 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) + + 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: + """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), *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") + + # 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: + chosen = None + 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" + 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}'." + ) 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()