From 97190cd75147948c39defbcf896c27ceb5bf7a87 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 12:07:45 +0000 Subject: [PATCH 01/50] Initial CAN test --- scripts/systemd/can-interface.service | 20 ++++++++++++++++++++ scripts/systemd/can-interface.sh | 4 ++++ 2 files changed, 24 insertions(+) create mode 100644 scripts/systemd/can-interface.service create mode 100755 scripts/systemd/can-interface.sh diff --git a/scripts/systemd/can-interface.service b/scripts/systemd/can-interface.service new file mode 100644 index 0000000..a6d0ddb --- /dev/null +++ b/scripts/systemd/can-interface.service @@ -0,0 +1,20 @@ +# place in /etc/systemd/system, then +# systemctl daemon-reload +# systemctl enable can-interface +# systemctl start can-interface + +[Unit] +Description=CAN interface setup (%N) +StartLimitIntervalSec=20 +StartLimitBurst=3 + +[Service] +Type=oneshot +RemainAfterExit=yes +ExecStart=/usr/local/bin/can-interface.sh +SyslogIdentifier=%N +TimeoutStartSec=10 +RestartSec=5 + +[Install] +WantedBy=multi-user.target diff --git a/scripts/systemd/can-interface.sh b/scripts/systemd/can-interface.sh new file mode 100755 index 0000000..bbde741 --- /dev/null +++ b/scripts/systemd/can-interface.sh @@ -0,0 +1,4 @@ +#!/bin/bash +set -e +modprobe gs_usb can-utils +ip link set can0 up type can bitrate 500000 From 55866560a0f8579afa9cee79589d03fabe11e8cb Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 12:09:07 +0000 Subject: [PATCH 02/50] Added restart always to Docker container --- .devcontainer/docker-compose.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index 3e46da0..7f59075 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -27,3 +27,4 @@ services: stdin_open: true tty: true network_mode: "host" + restart: always From 0b2de0d577aa169deb69723dd84c22439db88482 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 12:13:16 +0000 Subject: [PATCH 03/50] Added installation script --- scripts/systemd/setup-interface.sh | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 scripts/systemd/setup-interface.sh diff --git a/scripts/systemd/setup-interface.sh b/scripts/systemd/setup-interface.sh new file mode 100644 index 0000000..620f960 --- /dev/null +++ b/scripts/systemd/setup-interface.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash +set -e + +if [[ $EUID -ne 0 ]]; then + echo "Please run as root or with sudo." + exit 1 +fi + +# install the executable +install -Dm755 can-interface.sh \ + /usr/local/bin/can-interface + +# install the systemd unit +install -Dm644 can-interface.service \ + /etc/systemd/system/can-interface.service + +# reload and start +systemctl daemon-reload +systemctl enable can-interface.service +systemctl start can-interface.service + +echo "Successfully installed and started the can-interface." From c8f325639fd577b919f88031e77caea2af228522 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 12:14:03 +0000 Subject: [PATCH 04/50] Made file executable --- scripts/systemd/setup-interface.sh | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 scripts/systemd/setup-interface.sh diff --git a/scripts/systemd/setup-interface.sh b/scripts/systemd/setup-interface.sh old mode 100644 new mode 100755 From 748506ab9f6ffcef2572beda1f14eb7971c391f6 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 12:15:44 +0000 Subject: [PATCH 05/50] Fixed typo --- scripts/systemd/setup-interface.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/systemd/setup-interface.sh b/scripts/systemd/setup-interface.sh index 620f960..9c813c1 100755 --- a/scripts/systemd/setup-interface.sh +++ b/scripts/systemd/setup-interface.sh @@ -8,7 +8,7 @@ fi # install the executable install -Dm755 can-interface.sh \ - /usr/local/bin/can-interface + /usr/local/bin/can-interface.sh # install the systemd unit install -Dm644 can-interface.service \ From b10e853aaae85256e9a2e329c400450764eb87d7 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 12:30:24 +0000 Subject: [PATCH 06/50] Updated docker-compose --- .devcontainer/docker-compose.yaml | 2 ++ scripts/systemd/can-interface.sh | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index 7f59075..9db6c98 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -28,3 +28,5 @@ services: tty: true network_mode: "host" restart: always + capabilities: + - CAP_NET_ADMIN diff --git a/scripts/systemd/can-interface.sh b/scripts/systemd/can-interface.sh index bbde741..9248e1f 100755 --- a/scripts/systemd/can-interface.sh +++ b/scripts/systemd/can-interface.sh @@ -1,4 +1,4 @@ #!/bin/bash set -e -modprobe gs_usb can-utils +#modprobe gs_usb can-utils ip link set can0 up type can bitrate 500000 From ef75b9a8ac8d64013977c4a64c1bf857e0b6d870 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 12:58:03 +0000 Subject: [PATCH 07/50] Updated dependencies and services --- .devcontainer/docker-compose.yaml | 82 ++++++++++++++++++--------- README.md | 8 +++ scripts/systemd/can-interface.service | 20 ------- scripts/systemd/can-interface.sh | 4 -- scripts/systemd/setup-interface.sh | 22 ------- 5 files changed, 64 insertions(+), 72 deletions(-) delete mode 100644 scripts/systemd/can-interface.service delete mode 100755 scripts/systemd/can-interface.sh delete mode 100755 scripts/systemd/setup-interface.sh diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index 9db6c98..dae2ee4 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -1,32 +1,62 @@ +version: '3.8' + +# shared bits between all of your ROS-2 services +x-ros-common: &ros-common + 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 + working_dir: /home/dev/ros_ws + environment: + - DISPLAY=${DISPLAY} + network_mode: host + restart: always + services: app: - build: - context: . - dockerfile: Dockerfile - args: - USER_NAME: "dev" - USER_ID: "${HOST_UID}" - GROUP_ID: "${HOST_GID}" - BASE_IMAGE: "ros:jazzy-perception" + <<: *ros-common container_name: ros2_dev_container - volumes: - - ..:/home/dev/ros_ws - - /tmp/.X11-unix:/tmp/.X11-unix # Mount X11 socket - working_dir: /home/dev/ros_ws - environment: - - DISPLAY=${DISPLAY} # Pass the DISPLAY variable - # - NVIDIA_VISIBLE_DEVICES=all - # - NVIDIA_DRIVER_CAPABILITIES=compute,utility - # deploy: - # resources: - # reservations: - # devices: - # - driver: nvidia - # count: 1 - # capabilities: [gpu] stdin_open: true tty: true - network_mode: "host" - restart: always - capabilities: + cap_add: - CAP_NET_ADMIN + + scout: + <<: *ros-common + container_name: ros2_scout + user: root # so we can run ip link + cap_add: + - CAP_NET_ADMIN # grant just enough to do 'ip link' + entrypoint: + - bash + - -c + - | + set -e + # bring up CANbus + ip link set can0 up type can bitrate 500000 + # source both workspaces + source /opt/ros/jazzy/setup.bash + source install/setup.bash + # exec so it gets proper PID 1 & signals + exec ros2 launch scout_base scout_mini_base.launch.py + + driver: + <<: *ros-common + container_name: ros2_driver + depends_on: + - scout # wait for CAN bring-up first + entrypoint: + - bash + - -c + - | + set -e + source /opt/ros/jazzy/setup.bash + source install/setup.bash + exec ros2 launch nav2_tutorial fp_driver_node.launch config:=fp_driver_config.yaml diff --git a/README.md b/README.md index dc8e418..46dc0ee 100644 --- a/README.md +++ b/README.md @@ -103,6 +103,14 @@ can0 311 [8] 00 00 25 C6 FF FF F0 A4 ... ``` +Note: To automatically establish the connection to the Agilex, you can edit the /etc/modules file as such: +``` +# /etc/modules: kernel modules to load at boot time. +gs_usb +can-utils +``` + + ### Control the Scout robot using the keyboard In terminal 1, run these commands to start the base node for the Scout robot: diff --git a/scripts/systemd/can-interface.service b/scripts/systemd/can-interface.service deleted file mode 100644 index a6d0ddb..0000000 --- a/scripts/systemd/can-interface.service +++ /dev/null @@ -1,20 +0,0 @@ -# place in /etc/systemd/system, then -# systemctl daemon-reload -# systemctl enable can-interface -# systemctl start can-interface - -[Unit] -Description=CAN interface setup (%N) -StartLimitIntervalSec=20 -StartLimitBurst=3 - -[Service] -Type=oneshot -RemainAfterExit=yes -ExecStart=/usr/local/bin/can-interface.sh -SyslogIdentifier=%N -TimeoutStartSec=10 -RestartSec=5 - -[Install] -WantedBy=multi-user.target diff --git a/scripts/systemd/can-interface.sh b/scripts/systemd/can-interface.sh deleted file mode 100755 index 9248e1f..0000000 --- a/scripts/systemd/can-interface.sh +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/bash -set -e -#modprobe gs_usb can-utils -ip link set can0 up type can bitrate 500000 diff --git a/scripts/systemd/setup-interface.sh b/scripts/systemd/setup-interface.sh deleted file mode 100755 index 9c813c1..0000000 --- a/scripts/systemd/setup-interface.sh +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env bash -set -e - -if [[ $EUID -ne 0 ]]; then - echo "Please run as root or with sudo." - exit 1 -fi - -# install the executable -install -Dm755 can-interface.sh \ - /usr/local/bin/can-interface.sh - -# install the systemd unit -install -Dm644 can-interface.service \ - /etc/systemd/system/can-interface.service - -# reload and start -systemctl daemon-reload -systemctl enable can-interface.service -systemctl start can-interface.service - -echo "Successfully installed and started the can-interface." From 29378510a868d776d3358653ec074404e79a53a6 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 13:23:19 +0000 Subject: [PATCH 08/50] Updated docker-compose --- .devcontainer/docker-compose.yaml | 33 +++++++++++++++++-------------- README.md | 2 +- 2 files changed, 19 insertions(+), 16 deletions(-) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index dae2ee4..f41eec0 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -1,6 +1,4 @@ -version: '3.8' - -# shared bits between all of your ROS-2 services +# Shared components between all other services x-ros-common: &ros-common build: context: . @@ -14,37 +12,42 @@ x-ros-common: &ros-common - ..:/home/dev/ros_ws - /tmp/.X11-unix:/tmp/.X11-unix working_dir: /home/dev/ros_ws - environment: - - DISPLAY=${DISPLAY} network_mode: host restart: always + stdin_open: true + tty: true services: app: <<: *ros-common container_name: ros2_dev_container - stdin_open: true - tty: true - cap_add: - - CAP_NET_ADMIN + environment: + - DISPLAY=${DISPLAY} scout: <<: *ros-common container_name: ros2_scout - user: root # so we can run ip link cap_add: - - CAP_NET_ADMIN # grant just enough to do 'ip link' + - CAP_NET_ADMIN + restart: on-failure entrypoint: - bash - -c - | set -e - # bring up CANbus - ip link set can0 up type can bitrate 500000 - # source both workspaces + + echo "[scout] Bringing up CAN interface..." + if ip link set can0 up type can bitrate 500000; then + echo "[scout] CAN0 up at 500kbit" + else + echo "[scout] Could not bring up can0! -- continuing anyway" + fi + + echo "[scout] Sourcing ROS 2 jazzyp workspace..." source /opt/ros/jazzy/setup.bash source install/setup.bash - # exec so it gets proper PID 1 & signals + + echo "[scout] Launching scout_base..." exec ros2 launch scout_base scout_mini_base.launch.py driver: diff --git a/README.md b/README.md index 46dc0ee..9f25df0 100644 --- a/README.md +++ b/README.md @@ -25,7 +25,7 @@ The user can also compile the provided Docker container in the .devcontainer fol ``` docker compose -f .devcontainer/docker-compose.yaml build docker compose -f .devcontainer/docker-compose.yaml up -d -docker exec -it ros2_dev_container bash +docker compose exec app bash ``` Alternatively, the user can compile it directly using the Dev Containers extension in VSCode. From 60c2df2ffd70496a59efbdbf563401e38b74427c Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 13:29:51 +0000 Subject: [PATCH 09/50] More updates to docker-compose --- .devcontainer/docker-compose.yaml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index f41eec0..3bdb76a 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -10,7 +10,6 @@ x-ros-common: &ros-common BASE_IMAGE: "ros:jazzy-perception" volumes: - ..:/home/dev/ros_ws - - /tmp/.X11-unix:/tmp/.X11-unix working_dir: /home/dev/ros_ws network_mode: host restart: always @@ -21,12 +20,15 @@ services: app: <<: *ros-common container_name: ros2_dev_container + volumes: + - /tmp/.X11-unix:/tmp/.X11-unix environment: - DISPLAY=${DISPLAY} scout: <<: *ros-common container_name: ros2_scout + user: root cap_add: - CAP_NET_ADMIN restart: on-failure From f90ff788105ef53d03a02bb9cd4df7b4e52c100e Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 13:37:21 +0000 Subject: [PATCH 10/50] Minor bug --- .devcontainer/docker-compose.yaml | 1 + README.md | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index 3bdb76a..8834fc4 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -21,6 +21,7 @@ services: <<: *ros-common container_name: ros2_dev_container volumes: + - ..:/home/dev/ros_ws - /tmp/.X11-unix:/tmp/.X11-unix environment: - DISPLAY=${DISPLAY} diff --git a/README.md b/README.md index 9f25df0..4d8a643 100644 --- a/README.md +++ b/README.md @@ -29,6 +29,12 @@ docker compose exec app bash ``` Alternatively, the user can compile it directly using the Dev Containers extension in VSCode. +To check the status of the different services, you can run the following commands: +``` +docker compose logs scout +docker compose logs driver +``` + ## Step 2: Set up Fixposition ROS Driver To use the ROS driver with the Scout robot, the following changes must be applied: From 83905cf30c0e6e59504fffccf362ce635a993c94 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 13:49:59 +0000 Subject: [PATCH 11/50] Small updates --- .devcontainer/docker-compose.yaml | 9 ++++++++- README.md | 4 ++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index 8834fc4..70e2f42 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -25,6 +25,13 @@ services: - /tmp/.X11-unix:/tmp/.X11-unix environment: - DISPLAY=${DISPLAY} + entrypoint: + - bash + - -c + - | + set -e + source /opt/ros/jazzy/setup.bash + source install/setup.bash scout: <<: *ros-common @@ -46,7 +53,7 @@ services: echo "[scout] Could not bring up can0! -- continuing anyway" fi - echo "[scout] Sourcing ROS 2 jazzyp workspace..." + echo "[scout] Sourcing ROS2 workspace..." source /opt/ros/jazzy/setup.bash source install/setup.bash diff --git a/README.md b/README.md index 4d8a643..e4c1b45 100644 --- a/README.md +++ b/README.md @@ -116,6 +116,10 @@ gs_usb can-utils ``` +In addition, in case the error messsage 'RTNETLINK answers: Device or resource busy' appears, please run the following command: +``` +sudo ip link set can0 down +``` ### Control the Scout robot using the keyboard From ee3637a791ec9e05ac1e43d8ef412667959a1d09 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 13:57:38 +0000 Subject: [PATCH 12/50] Test to fix missing messages --- .devcontainer/docker-compose.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index 70e2f42..b6eaa21 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -15,6 +15,10 @@ x-ros-common: &ros-common restart: always stdin_open: true tty: true + environment: + - ROS_DOMAIN_ID=0 + - RMW_IMPLEMENTATION=rmw_fastrtps_cpp + - FASTRTPS_DEFAULT_PROFILES_FILE=/home/dev/ros_ws/config/fastdds_profile.xml services: app: From e60c601772ed0ef3fda997c17c5eb3a310e9b099 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 14:00:50 +0000 Subject: [PATCH 13/50] Removed unnecessary line --- .devcontainer/docker-compose.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index b6eaa21..2a00ed6 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -18,7 +18,6 @@ x-ros-common: &ros-common environment: - ROS_DOMAIN_ID=0 - RMW_IMPLEMENTATION=rmw_fastrtps_cpp - - FASTRTPS_DEFAULT_PROFILES_FILE=/home/dev/ros_ws/config/fastdds_profile.xml services: app: From 0275759c57031d81f50c29649f664c189641c768 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 14:05:32 +0000 Subject: [PATCH 14/50] More tests --- .devcontainer/docker-compose.yaml | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index 2a00ed6..56b32ec 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -1,4 +1,6 @@ -# Shared components between all other services +version: '3.8' + +# Shared components between all services x-ros-common: &ros-common build: context: . @@ -17,7 +19,7 @@ x-ros-common: &ros-common tty: true environment: - ROS_DOMAIN_ID=0 - - RMW_IMPLEMENTATION=rmw_fastrtps_cpp + - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp services: app: @@ -35,6 +37,7 @@ services: set -e source /opt/ros/jazzy/setup.bash source install/setup.bash + exec bash scout: <<: *ros-common @@ -48,7 +51,6 @@ services: - -c - | set -e - echo "[scout] Bringing up CAN interface..." if ip link set can0 up type can bitrate 500000; then echo "[scout] CAN0 up at 500kbit" @@ -67,7 +69,7 @@ services: <<: *ros-common container_name: ros2_driver depends_on: - - scout # wait for CAN bring-up first + - scout entrypoint: - bash - -c @@ -75,4 +77,5 @@ services: set -e source /opt/ros/jazzy/setup.bash source install/setup.bash - exec ros2 launch nav2_tutorial fp_driver_node.launch config:=fp_driver_config.yaml + exec ros2 launch nav2_tutorial fp_driver_node.launch \ + config:=fp_driver_config.yaml From e008e7796ba334f7055ee411002ab0d2d9c12e75 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 14:08:07 +0000 Subject: [PATCH 15/50] Add missing dependency --- .devcontainer/Dockerfile | 1 + 1 file changed, 1 insertion(+) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 5007329..b945b81 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -33,6 +33,7 @@ RUN apt update && \ ros-jazzy-navigation2 \ ros-jazzy-nav2-bringup \ ros-jazzy-teleop-twist-keyboard \ + ros-jazzy-rmw-cyclonedds-cpp \ sudo && \ \ # Detect device architecture. From e321286ffa938b193363f604020b2c46347cc423 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 16:18:30 +0000 Subject: [PATCH 16/50] More tests --- .devcontainer/docker-compose.yaml | 20 ++++++++------------ README.md | 3 +++ 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index 56b32ec..f7911bd 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -1,19 +1,17 @@ version: '3.8' -# Shared components between all services +# Shared bits for all three services x-ros-common: &ros-common build: context: . - dockerfile: Dockerfile - args: - USER_NAME: "dev" - USER_ID: "${HOST_UID}" - GROUP_ID: "${HOST_GID}" - BASE_IMAGE: "ros:jazzy-perception" + dockerfile: Dockerfile # make sure it installs rmw_cyclonedds_cpp volumes: - ..:/home/dev/ros_ws + - /dev/shm:/dev/shm # ← share the host SHM for CycloneDDS working_dir: /home/dev/ros_ws network_mode: host + ipc: host + pid: host restart: always stdin_open: true tty: true @@ -32,7 +30,7 @@ services: - DISPLAY=${DISPLAY} entrypoint: - bash - - -c + - -lc - | set -e source /opt/ros/jazzy/setup.bash @@ -48,9 +46,8 @@ services: restart: on-failure entrypoint: - bash - - -c + - -lc - | - set -e echo "[scout] Bringing up CAN interface..." if ip link set can0 up type can bitrate 500000; then echo "[scout] CAN0 up at 500kbit" @@ -72,9 +69,8 @@ services: - scout entrypoint: - bash - - -c + - -lc - | - set -e source /opt/ros/jazzy/setup.bash source install/setup.bash exec ros2 launch nav2_tutorial fp_driver_node.launch \ diff --git a/README.md b/README.md index e4c1b45..ebbfb18 100644 --- a/README.md +++ b/README.md @@ -35,6 +35,7 @@ docker compose logs scout docker compose logs driver ``` + ## Step 2: Set up Fixposition ROS Driver To use the ROS driver with the Scout robot, the following changes must be applied: @@ -67,6 +68,7 @@ converter: use_z: false # Transmit the z axis of the input velocity ``` + ## Step 3: Build ROS2 workspace Build the ROS2 workspace. ``` @@ -91,6 +93,7 @@ sudo modprobe gs_usb can-utils 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: ``` sudo ./scripts/start_can.sh From a6173b431a0106ebbdfd618e4c45379cfe5e1c77 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 16:26:37 +0000 Subject: [PATCH 17/50] Testing single executable --- .devcontainer/Dockerfile | 1 - .devcontainer/docker-compose.yaml | 87 ++++++-------------- src/nav2_tutorial/launch/all_nodes.launch.py | 37 +++++++++ 3 files changed, 61 insertions(+), 64 deletions(-) create mode 100644 src/nav2_tutorial/launch/all_nodes.launch.py diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index b945b81..5007329 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -33,7 +33,6 @@ RUN apt update && \ ros-jazzy-navigation2 \ ros-jazzy-nav2-bringup \ ros-jazzy-teleop-twist-keyboard \ - ros-jazzy-rmw-cyclonedds-cpp \ sudo && \ \ # Detect device architecture. diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index f7911bd..7f1f146 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -1,77 +1,38 @@ version: '3.8' -# Shared bits for all three services -x-ros-common: &ros-common - build: - context: . - dockerfile: Dockerfile # make sure it installs rmw_cyclonedds_cpp - volumes: - - ..:/home/dev/ros_ws - - /dev/shm:/dev/shm # ← share the host SHM for CycloneDDS - working_dir: /home/dev/ros_ws - network_mode: host - ipc: host - pid: host - restart: always - stdin_open: true - tty: true - environment: - - ROS_DOMAIN_ID=0 - - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp - services: - app: - <<: *ros-common - container_name: ros2_dev_container + ros2: + build: + context: . + dockerfile: Dockerfile + args: + USER_NAME: "dev" + USER_ID: "${HOST_UID}" + GROUP_ID: "${HOST_GID}" + BASE_IMAGE: "ros:jazzy-perception" + container_name: ros2_all_in_one volumes: - ..:/home/dev/ros_ws - /tmp/.X11-unix:/tmp/.X11-unix + network_mode: host environment: - DISPLAY=${DISPLAY} + restart: always + stdin_open: true + tty: true entrypoint: - bash - -lc - | - set -e - source /opt/ros/jazzy/setup.bash - source install/setup.bash - exec bash - - scout: - <<: *ros-common - container_name: ros2_scout - user: root - cap_add: - - CAP_NET_ADMIN - restart: on-failure - entrypoint: - - bash - - -lc - - | - echo "[scout] Bringing up CAN interface..." - if ip link set can0 up type can bitrate 500000; then - echo "[scout] CAN0 up at 500kbit" - else - echo "[scout] Could not bring up can0! -- continuing anyway" - fi - - echo "[scout] Sourcing ROS2 workspace..." - source /opt/ros/jazzy/setup.bash - source install/setup.bash + set -e + # bring up CAN + echo "[entrypoint] Bringing up can0" + ip link set can0 up type can bitrate 500000 || \ + echo "[entrypoint] can't bring up can0, continuing" - echo "[scout] Launching scout_base..." - exec ros2 launch scout_base scout_mini_base.launch.py + # source both installs + source /opt/ros/jazzy/setup.bash + source install/setup.bash - driver: - <<: *ros-common - container_name: ros2_driver - depends_on: - - scout - entrypoint: - - bash - - -lc - - | - source /opt/ros/jazzy/setup.bash - source install/setup.bash - exec ros2 launch nav2_tutorial fp_driver_node.launch \ - config:=fp_driver_config.yaml + # launch everything in one go + exec ros2 launch your_meta_package all_nodes.launch.py diff --git a/src/nav2_tutorial/launch/all_nodes.launch.py b/src/nav2_tutorial/launch/all_nodes.launch.py new file mode 100644 index 0000000..7874da4 --- /dev/null +++ b/src/nav2_tutorial/launch/all_nodes.launch.py @@ -0,0 +1,37 @@ +# launch/all_nodes.launch.py + +import os +from launch import LaunchDescription +from launch.actions import GroupAction, IncludeLaunchDescription, ExecuteProcess +from launch.launch_description_sources import PythonLaunchDescriptionSource + +def generate_launch_description(): + pkg_share = os.path.join( + get_package_share_directory('your_meta_package'), 'launch') + + return LaunchDescription([ + # 1) CAN bring-up (just an OS command) + ExecuteProcess( + cmd=[ + 'ip', 'link', 'set', 'can0', 'up', + 'type', 'can', 'bitrate', '500000' + ], + output='screen', + shell=False + ), + + # 2) Scout node + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_share, 'scout_mini_base.launch.py') + ), + ), + + # 3) Driver node + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_share, 'fp_driver_node.launch.py') + ), + launch_arguments={'config': 'config/fp_driver_config.yaml'}.items(), + ), + ]) From 340604c0bba85ca2a12c1aca5d7e4f4227c9a53a Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 24 Apr 2025 16:29:16 +0000 Subject: [PATCH 18/50] Added missing dependency --- src/nav2_tutorial/launch/all_nodes.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/nav2_tutorial/launch/all_nodes.launch.py b/src/nav2_tutorial/launch/all_nodes.launch.py index 7874da4..176b675 100644 --- a/src/nav2_tutorial/launch/all_nodes.launch.py +++ b/src/nav2_tutorial/launch/all_nodes.launch.py @@ -4,6 +4,7 @@ from launch import LaunchDescription from launch.actions import GroupAction, IncludeLaunchDescription, ExecuteProcess from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory def generate_launch_description(): pkg_share = os.path.join( From 20e71971ec5fdf8bb95cf0cc30d36fd2b0495257 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 08:31:46 +0000 Subject: [PATCH 19/50] Test automated Docker service --- .devcontainer/docker-compose.yaml | 32 ++++++++++++++++++++++++++++++- 1 file changed, 31 insertions(+), 1 deletion(-) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index 7f1f146..d19302b 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -1,6 +1,36 @@ version: '3.8' services: + app: + build: + context: . + dockerfile: Dockerfile + args: + USER_NAME: "dev" + USER_ID: "${HOST_UID}" + GROUP_ID: "${HOST_GID}" + BASE_IMAGE: "ros:jazzy-perception" + container_name: ros2_dev_container + volumes: + - ..:/home/dev/ros_ws + - /tmp/.X11-unix:/tmp/.X11-unix # Mount X11 socket + working_dir: /home/dev/ros_ws + network_mode: "host" + environment: + - DISPLAY=${DISPLAY} # Pass the DISPLAY variable + # - NVIDIA_VISIBLE_DEVICES=all + # - NVIDIA_DRIVER_CAPABILITIES=compute,utility + # deploy: + # resources: + # reservations: + # devices: + # - driver: nvidia + # count: 1 + # capabilities: [gpu] + restart: always + stdin_open: true + tty: true + ros2: build: context: . @@ -35,4 +65,4 @@ services: source install/setup.bash # launch everything in one go - exec ros2 launch your_meta_package all_nodes.launch.py + exec ros2 launch nav2_tutorial all_nodes.launch.py From 7dc7ee23b7781e9fc0ba00f12f0f86d747acbaad Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 11:53:01 +0000 Subject: [PATCH 20/50] Testing can0 boot --- .devcontainer/docker-compose.yaml | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index d19302b..a08f4d3 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -41,6 +41,10 @@ services: GROUP_ID: "${HOST_GID}" BASE_IMAGE: "ros:jazzy-perception" container_name: ros2_all_in_one + cap_add: + - NET_ADMIN + devices: + - /dev/can0:/dev/can0 volumes: - ..:/home/dev/ros_ws - /tmp/.X11-unix:/tmp/.X11-unix @@ -60,8 +64,16 @@ services: ip link set can0 up type can bitrate 500000 || \ echo "[entrypoint] can't bring up can0, continuing" - # source both installs + # source ROS source /opt/ros/jazzy/setup.bash + + # if the install folder does not exist, build it + if [ ! -d install ]; then + echo "[entrypoint] install/ not found, running colcon build…" + #colcon build --symlink-install + fi + + # source the workspace source install/setup.bash # launch everything in one go From 94627a4c03ffb81fc7edfa548c307f65c192d75d Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 11:55:58 +0000 Subject: [PATCH 21/50] Removed device mount --- .devcontainer/docker-compose.yaml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index a08f4d3..76e7822 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -43,8 +43,6 @@ services: container_name: ros2_all_in_one cap_add: - NET_ADMIN - devices: - - /dev/can0:/dev/can0 volumes: - ..:/home/dev/ros_ws - /tmp/.X11-unix:/tmp/.X11-unix From dec6a3bea1c90dcc35eaa9fc6c8e4fffb8dca1b3 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 12:08:18 +0000 Subject: [PATCH 22/50] Fixed minor bugs --- .devcontainer/docker-compose.yaml | 3 ++- src/nav2_tutorial/launch/all_nodes.launch.py | 12 ++++++------ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index 76e7822..560a139 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -46,6 +46,7 @@ services: volumes: - ..:/home/dev/ros_ws - /tmp/.X11-unix:/tmp/.X11-unix + working_dir: /home/dev/ros_ws network_mode: host environment: - DISPLAY=${DISPLAY} @@ -68,7 +69,7 @@ services: # if the install folder does not exist, build it if [ ! -d install ]; then echo "[entrypoint] install/ not found, running colcon build…" - #colcon build --symlink-install + colcon build --cmake-args -DBUILD_TESTING=OFF fi # source the workspace diff --git a/src/nav2_tutorial/launch/all_nodes.launch.py b/src/nav2_tutorial/launch/all_nodes.launch.py index 176b675..93bdf5a 100644 --- a/src/nav2_tutorial/launch/all_nodes.launch.py +++ b/src/nav2_tutorial/launch/all_nodes.launch.py @@ -1,17 +1,17 @@ -# launch/all_nodes.launch.py - import os +from ament_index_python.packages import get_package_share_directory + from launch import LaunchDescription -from launch.actions import GroupAction, IncludeLaunchDescription, ExecuteProcess +from launch.actions import IncludeLaunchDescription, ExecuteProcess from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory + def generate_launch_description(): pkg_share = os.path.join( - get_package_share_directory('your_meta_package'), 'launch') + get_package_share_directory('nav2_tutorial'), 'launch') return LaunchDescription([ - # 1) CAN bring-up (just an OS command) + # 1) CAN bring-up ExecuteProcess( cmd=[ 'ip', 'link', 'set', 'can0', 'up', From d202c4c7c0573ddfd4b5069ebc36fa47d977cbe9 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 12:11:40 +0000 Subject: [PATCH 23/50] Fixed launch paths --- src/nav2_tutorial/launch/all_nodes.launch.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/nav2_tutorial/launch/all_nodes.launch.py b/src/nav2_tutorial/launch/all_nodes.launch.py index 93bdf5a..66efb45 100644 --- a/src/nav2_tutorial/launch/all_nodes.launch.py +++ b/src/nav2_tutorial/launch/all_nodes.launch.py @@ -7,8 +7,8 @@ def generate_launch_description(): - pkg_share = os.path.join( - get_package_share_directory('nav2_tutorial'), 'launch') + scout_pkg_share = get_package_share_directory('scout_mini_base') + driver_pkg_share = os.path.join(get_package_share_directory('nav2_tutorial'), 'launch') return LaunchDescription([ # 1) CAN bring-up @@ -24,14 +24,14 @@ def generate_launch_description(): # 2) Scout node IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_share, 'scout_mini_base.launch.py') + os.path.join(scout_pkg_share, 'scout_mini_base.launch.py') ), ), # 3) Driver node IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_share, 'fp_driver_node.launch.py') + os.path.join(driver_pkg_share, 'fp_driver_node.launch.py') ), launch_arguments={'config': 'config/fp_driver_config.yaml'}.items(), ), From f6d34c077c61076e6235fed0fb7846d0103d3fed Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 12:12:43 +0000 Subject: [PATCH 24/50] Force compilation --- .devcontainer/docker-compose.yaml | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index 560a139..bdccdd1 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -66,11 +66,8 @@ services: # source ROS source /opt/ros/jazzy/setup.bash - # if the install folder does not exist, build it - if [ ! -d install ]; then - echo "[entrypoint] install/ not found, running colcon build…" - colcon build --cmake-args -DBUILD_TESTING=OFF - fi + # ensure the workspace is built or latest build is sourced + colcon build --cmake-args -DBUILD_TESTING=OFF # source the workspace source install/setup.bash From dd1ad4bbc635b70182cc1356599762ca06eec38d Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 12:13:41 +0000 Subject: [PATCH 25/50] Fixed path --- src/nav2_tutorial/launch/all_nodes.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/nav2_tutorial/launch/all_nodes.launch.py b/src/nav2_tutorial/launch/all_nodes.launch.py index 66efb45..88a26e5 100644 --- a/src/nav2_tutorial/launch/all_nodes.launch.py +++ b/src/nav2_tutorial/launch/all_nodes.launch.py @@ -7,7 +7,7 @@ def generate_launch_description(): - scout_pkg_share = get_package_share_directory('scout_mini_base') + scout_pkg_share = get_package_share_directory('scout_base') driver_pkg_share = os.path.join(get_package_share_directory('nav2_tutorial'), 'launch') return LaunchDescription([ From 531dc513a39970d735c761fd68d0f8acace45970 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 12:15:30 +0000 Subject: [PATCH 26/50] Updated launch path --- src/nav2_tutorial/launch/all_nodes.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/nav2_tutorial/launch/all_nodes.launch.py b/src/nav2_tutorial/launch/all_nodes.launch.py index 88a26e5..9595aa7 100644 --- a/src/nav2_tutorial/launch/all_nodes.launch.py +++ b/src/nav2_tutorial/launch/all_nodes.launch.py @@ -7,7 +7,7 @@ def generate_launch_description(): - scout_pkg_share = get_package_share_directory('scout_base') + scout_pkg_share = os.path.join(get_package_share_directory('scout_base'), 'launch') driver_pkg_share = os.path.join(get_package_share_directory('nav2_tutorial'), 'launch') return LaunchDescription([ From 2ad9bd3c46f84cab0978e94271e917c840677318 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 12:30:38 +0000 Subject: [PATCH 27/50] Testing different approach --- .devcontainer/docker-compose.yaml | 6 +----- src/nav2_tutorial/launch/all_nodes.launch.py | 2 +- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index bdccdd1..ef94896 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -42,7 +42,7 @@ services: BASE_IMAGE: "ros:jazzy-perception" container_name: ros2_all_in_one cap_add: - - NET_ADMIN + - NET_RAW volumes: - ..:/home/dev/ros_ws - /tmp/.X11-unix:/tmp/.X11-unix @@ -58,10 +58,6 @@ services: - -lc - | set -e - # bring up CAN - echo "[entrypoint] Bringing up can0" - ip link set can0 up type can bitrate 500000 || \ - echo "[entrypoint] can't bring up can0, continuing" # source ROS source /opt/ros/jazzy/setup.bash diff --git a/src/nav2_tutorial/launch/all_nodes.launch.py b/src/nav2_tutorial/launch/all_nodes.launch.py index 9595aa7..a5f74df 100644 --- a/src/nav2_tutorial/launch/all_nodes.launch.py +++ b/src/nav2_tutorial/launch/all_nodes.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): scout_pkg_share = os.path.join(get_package_share_directory('scout_base'), 'launch') - driver_pkg_share = os.path.join(get_package_share_directory('nav2_tutorial'), 'launch') + driver_pkg_share = os.path.join(get_package_share_directory('fixposition_driver_ros2'), 'launch') return LaunchDescription([ # 1) CAN bring-up From c8f42953f53866e35f523fe438f886d5c30cb852 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 12:34:06 +0000 Subject: [PATCH 28/50] Minor bug fixes --- .devcontainer/docker-compose.yaml | 2 -- src/nav2_tutorial/launch/all_nodes.launch.py | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index ef94896..80c1ade 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -41,8 +41,6 @@ services: GROUP_ID: "${HOST_GID}" BASE_IMAGE: "ros:jazzy-perception" container_name: ros2_all_in_one - cap_add: - - NET_RAW volumes: - ..:/home/dev/ros_ws - /tmp/.X11-unix:/tmp/.X11-unix diff --git a/src/nav2_tutorial/launch/all_nodes.launch.py b/src/nav2_tutorial/launch/all_nodes.launch.py index a5f74df..9595aa7 100644 --- a/src/nav2_tutorial/launch/all_nodes.launch.py +++ b/src/nav2_tutorial/launch/all_nodes.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): scout_pkg_share = os.path.join(get_package_share_directory('scout_base'), 'launch') - driver_pkg_share = os.path.join(get_package_share_directory('fixposition_driver_ros2'), 'launch') + driver_pkg_share = os.path.join(get_package_share_directory('nav2_tutorial'), 'launch') return LaunchDescription([ # 1) CAN bring-up From 7699389594afaca25f1acfd03a121ec3eaa0d58d Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 12:38:04 +0000 Subject: [PATCH 29/50] Corrected small typo --- src/nav2_tutorial/launch/all_nodes.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/nav2_tutorial/launch/all_nodes.launch.py b/src/nav2_tutorial/launch/all_nodes.launch.py index 9595aa7..d6c78d5 100644 --- a/src/nav2_tutorial/launch/all_nodes.launch.py +++ b/src/nav2_tutorial/launch/all_nodes.launch.py @@ -31,7 +31,7 @@ def generate_launch_description(): # 3) Driver node IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(driver_pkg_share, 'fp_driver_node.launch.py') + os.path.join(driver_pkg_share, 'fp_driver_node.launch') ), launch_arguments={'config': 'config/fp_driver_config.yaml'}.items(), ), From c7dec4eaaf14bda0f9eb0f8be65bcb1cb70b84ab Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 12:42:13 +0000 Subject: [PATCH 30/50] Changed to XML source --- src/nav2_tutorial/launch/all_nodes.launch.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/nav2_tutorial/launch/all_nodes.launch.py b/src/nav2_tutorial/launch/all_nodes.launch.py index d6c78d5..b0f0009 100644 --- a/src/nav2_tutorial/launch/all_nodes.launch.py +++ b/src/nav2_tutorial/launch/all_nodes.launch.py @@ -3,7 +3,7 @@ from launch import LaunchDescription from launch.actions import IncludeLaunchDescription, ExecuteProcess -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.launch_description_sources import PythonLaunchDescriptionSource, XMLLaunchDescriptionSource def generate_launch_description(): @@ -30,8 +30,8 @@ def generate_launch_description(): # 3) Driver node IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(driver_pkg_share, 'fp_driver_node.launch') + XMLLaunchDescriptionSource( + os.path.join(driver_pkg_share, 'fp_driver_node.launch') ), launch_arguments={'config': 'config/fp_driver_config.yaml'}.items(), ), From 042457593cf9e2c136ad35a4cc3c85b4942e44f3 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 12:44:41 +0000 Subject: [PATCH 31/50] Update launch file --- src/nav2_tutorial/launch/all_nodes.launch.py | 6 ++--- .../launch/fp_driver_node.launch | 11 -------- .../launch/fp_driver_node.launch.py | 26 +++++++++++++++++++ 3 files changed, 29 insertions(+), 14 deletions(-) delete mode 100644 src/nav2_tutorial/launch/fp_driver_node.launch create mode 100644 src/nav2_tutorial/launch/fp_driver_node.launch.py diff --git a/src/nav2_tutorial/launch/all_nodes.launch.py b/src/nav2_tutorial/launch/all_nodes.launch.py index b0f0009..9595aa7 100644 --- a/src/nav2_tutorial/launch/all_nodes.launch.py +++ b/src/nav2_tutorial/launch/all_nodes.launch.py @@ -3,7 +3,7 @@ from launch import LaunchDescription from launch.actions import IncludeLaunchDescription, ExecuteProcess -from launch.launch_description_sources import PythonLaunchDescriptionSource, XMLLaunchDescriptionSource +from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): @@ -30,8 +30,8 @@ def generate_launch_description(): # 3) Driver node IncludeLaunchDescription( - XMLLaunchDescriptionSource( - os.path.join(driver_pkg_share, 'fp_driver_node.launch') + PythonLaunchDescriptionSource( + os.path.join(driver_pkg_share, 'fp_driver_node.launch.py') ), launch_arguments={'config': 'config/fp_driver_config.yaml'}.items(), ), diff --git a/src/nav2_tutorial/launch/fp_driver_node.launch b/src/nav2_tutorial/launch/fp_driver_node.launch deleted file mode 100644 index 1e851c1..0000000 --- a/src/nav2_tutorial/launch/fp_driver_node.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - diff --git a/src/nav2_tutorial/launch/fp_driver_node.launch.py b/src/nav2_tutorial/launch/fp_driver_node.launch.py new file mode 100644 index 0000000..f8dfcef --- /dev/null +++ b/src/nav2_tutorial/launch/fp_driver_node.launch.py @@ -0,0 +1,26 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import FindPackageShare, LaunchConfiguration + +def generate_launch_description(): + node_name = LaunchConfiguration('node_name', default='fixposition_driver_ros2') + config_arg = LaunchConfiguration('config', default='fp_driver_config.yaml') + launcher = LaunchConfiguration('launcher', default='') + + pkg_share = FindPackageShare('nav2_tutorial').find('nav2_tutorial') + config_file = os.path.join(pkg_share, 'config', config_arg.perform({})) + + return LaunchDescription([ + Node( + package='fixposition_driver_ros2', + executable='fixposition_driver_ros2_exec', + name=node_name, + output='screen', + respawn=True, + respawn_delay=5, + prefix=launcher, + parameters=[config_file], + arguments=['--ros-args', '--log-level', f'{node_name.get()}:=info'] + ) + ]) From 62ce9f74ab34bc16f839837e820f73ffb9b8f22a Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 13:54:24 +0000 Subject: [PATCH 32/50] Reverted to XML configuration --- .devcontainer/Dockerfile | 1 + src/nav2_tutorial/launch/all_nodes.launch.py | 6 ++--- .../launch/fp_driver_node.launch | 11 ++++++++ .../launch/fp_driver_node.launch.py | 26 ------------------- 4 files changed, 15 insertions(+), 29 deletions(-) create mode 100644 src/nav2_tutorial/launch/fp_driver_node.launch delete mode 100644 src/nav2_tutorial/launch/fp_driver_node.launch.py diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 5007329..bdc1de3 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -33,6 +33,7 @@ RUN apt update && \ ros-jazzy-navigation2 \ ros-jazzy-nav2-bringup \ ros-jazzy-teleop-twist-keyboard \ + ros-jazzy-launch-xml \ sudo && \ \ # Detect device architecture. diff --git a/src/nav2_tutorial/launch/all_nodes.launch.py b/src/nav2_tutorial/launch/all_nodes.launch.py index 9595aa7..a0da997 100644 --- a/src/nav2_tutorial/launch/all_nodes.launch.py +++ b/src/nav2_tutorial/launch/all_nodes.launch.py @@ -4,6 +4,7 @@ from launch import LaunchDescription from launch.actions import IncludeLaunchDescription, ExecuteProcess from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_xml.launch_description_sources import XMLLaunchDescriptionSource def generate_launch_description(): @@ -30,9 +31,8 @@ def generate_launch_description(): # 3) Driver node IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(driver_pkg_share, 'fp_driver_node.launch.py') + XMLLaunchDescriptionSource( + os.path.join(driver_pkg_share, 'fp_driver_node.launch') ), - launch_arguments={'config': 'config/fp_driver_config.yaml'}.items(), ), ]) diff --git a/src/nav2_tutorial/launch/fp_driver_node.launch b/src/nav2_tutorial/launch/fp_driver_node.launch new file mode 100644 index 0000000..1e851c1 --- /dev/null +++ b/src/nav2_tutorial/launch/fp_driver_node.launch @@ -0,0 +1,11 @@ + + + + + + + + + diff --git a/src/nav2_tutorial/launch/fp_driver_node.launch.py b/src/nav2_tutorial/launch/fp_driver_node.launch.py deleted file mode 100644 index f8dfcef..0000000 --- a/src/nav2_tutorial/launch/fp_driver_node.launch.py +++ /dev/null @@ -1,26 +0,0 @@ -import os -from launch import LaunchDescription -from launch_ros.actions import Node -from launch.substitutions import FindPackageShare, LaunchConfiguration - -def generate_launch_description(): - node_name = LaunchConfiguration('node_name', default='fixposition_driver_ros2') - config_arg = LaunchConfiguration('config', default='fp_driver_config.yaml') - launcher = LaunchConfiguration('launcher', default='') - - pkg_share = FindPackageShare('nav2_tutorial').find('nav2_tutorial') - config_file = os.path.join(pkg_share, 'config', config_arg.perform({})) - - return LaunchDescription([ - Node( - package='fixposition_driver_ros2', - executable='fixposition_driver_ros2_exec', - name=node_name, - output='screen', - respawn=True, - respawn_delay=5, - prefix=launcher, - parameters=[config_file], - arguments=['--ros-args', '--log-level', f'{node_name.get()}:=info'] - ) - ]) From 23f9bc35cabda305b88349d3b33695e5508ce711 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 14:17:23 +0000 Subject: [PATCH 33/50] Removed can0 setup in launch file --- src/nav2_tutorial/launch/all_nodes.launch.py | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/src/nav2_tutorial/launch/all_nodes.launch.py b/src/nav2_tutorial/launch/all_nodes.launch.py index a0da997..9609600 100644 --- a/src/nav2_tutorial/launch/all_nodes.launch.py +++ b/src/nav2_tutorial/launch/all_nodes.launch.py @@ -12,24 +12,14 @@ def generate_launch_description(): driver_pkg_share = os.path.join(get_package_share_directory('nav2_tutorial'), 'launch') return LaunchDescription([ - # 1) CAN bring-up - ExecuteProcess( - cmd=[ - 'ip', 'link', 'set', 'can0', 'up', - 'type', 'can', 'bitrate', '500000' - ], - output='screen', - shell=False - ), - - # 2) Scout node + # 1) Scout node IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(scout_pkg_share, 'scout_mini_base.launch.py') ), ), - # 3) Driver node + # 2) Driver node IncludeLaunchDescription( XMLLaunchDescriptionSource( os.path.join(driver_pkg_share, 'fp_driver_node.launch') From 9760709431ebf7010bd1de2b55c85ddfbd10d423 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 14:30:20 +0000 Subject: [PATCH 34/50] Removed app service --- .devcontainer/docker-compose.yaml | 36 +++---------------------------- 1 file changed, 3 insertions(+), 33 deletions(-) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index 80c1ade..14eebe6 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -1,7 +1,7 @@ version: '3.8' -services: - app: +services: + vrtk: build: context: . dockerfile: Dockerfile @@ -10,37 +10,7 @@ services: USER_ID: "${HOST_UID}" GROUP_ID: "${HOST_GID}" BASE_IMAGE: "ros:jazzy-perception" - container_name: ros2_dev_container - volumes: - - ..:/home/dev/ros_ws - - /tmp/.X11-unix:/tmp/.X11-unix # Mount X11 socket - working_dir: /home/dev/ros_ws - network_mode: "host" - environment: - - DISPLAY=${DISPLAY} # Pass the DISPLAY variable - # - NVIDIA_VISIBLE_DEVICES=all - # - NVIDIA_DRIVER_CAPABILITIES=compute,utility - # deploy: - # resources: - # reservations: - # devices: - # - driver: nvidia - # count: 1 - # capabilities: [gpu] - restart: always - stdin_open: true - tty: true - - ros2: - build: - context: . - dockerfile: Dockerfile - args: - USER_NAME: "dev" - USER_ID: "${HOST_UID}" - GROUP_ID: "${HOST_GID}" - BASE_IMAGE: "ros:jazzy-perception" - container_name: ros2_all_in_one + container_name: ros2_vrtk_container volumes: - ..:/home/dev/ros_ws - /tmp/.X11-unix:/tmp/.X11-unix From 7d218f8507bdb5ba2530c2ca98c3cafa17f4b9f2 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 14:42:46 +0000 Subject: [PATCH 35/50] Updated source step --- .devcontainer/Dockerfile | 5 +++-- README.md | 5 ++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index bdc1de3..23f859b 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -71,8 +71,9 @@ RUN echo "alias ls='ls --color=auto'" >> /etc/bash.bashrc && \ # Switch to the new "dev" user. USER ${USER_NAME} -# Add the ROS environment setup to the dev user's .bashrc. -RUN echo "source /opt/ros/jazzy/setup.bash" >> /home/${USER_NAME}/.bashrc +# Make interactive shells auto-source ROS and ROS workspace +RUN echo 'source /opt/ros/jazzy/setup.bash' >> ~/.bashrc \ + && echo 'source ~/ros_ws/install/setup.bash' >> ~/.bashrc # Ensure the user's local bin is in PATH. ENV PATH=/home/${USER_NAME}/.local/bin:${PATH} diff --git a/README.md b/README.md index ebbfb18..52a1c3d 100644 --- a/README.md +++ b/README.md @@ -25,14 +25,13 @@ The user can also compile the provided Docker container in the .devcontainer fol ``` docker compose -f .devcontainer/docker-compose.yaml build docker compose -f .devcontainer/docker-compose.yaml up -d -docker compose exec app bash +docker compose -f ~/dev/nav2_tutorial/.devcontainer/docker-compose.yaml exec vrtk bash ``` Alternatively, the user can compile it directly using the Dev Containers extension in VSCode. To check the status of the different services, you can run the following commands: ``` -docker compose logs scout -docker compose logs driver +docker compose logs vrtk ``` From 1a6df34ebb8a1f3bffd1b4b7085fd59256574cde Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 2 May 2025 15:54:42 +0000 Subject: [PATCH 36/50] Improveed logged waypoint follower --- .devcontainer/devcontainer.json | 2 +- .../src/logged_waypoint_follower.py | 50 +++++++++++++++---- 2 files changed, 41 insertions(+), 11 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 6e187ca..31e1e02 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -2,7 +2,7 @@ { "name": "ROS2 Dev Container", "dockerComposeFile": ["../.devcontainer/docker-compose.yaml"], - "service": "app", + "service": "vrtk", "workspaceFolder": "/home/dev/ros_ws", "extensions": [ "ms-python.python", diff --git a/src/nav2_tutorial/src/logged_waypoint_follower.py b/src/nav2_tutorial/src/logged_waypoint_follower.py index d11a8e1..89751b7 100644 --- a/src/nav2_tutorial/src/logged_waypoint_follower.py +++ b/src/nav2_tutorial/src/logged_waypoint_follower.py @@ -2,6 +2,8 @@ import sys import time import yaml +import argparse +import glob import rclpy from nav2_simple_commander.robot_navigator import BasicNavigator @@ -30,12 +32,12 @@ def get_wps(self): return gepose_wps -class GpsWpCommander(): +class GpsWpCommander: """ Class to use nav2 gps waypoint follower to follow a set of waypoints logged in a yaml file """ - def __init__(self, wps_file_path): + def __init__(self, wps_file_path: str): self.navigator = BasicNavigator("basic_navigator") self.wp_parser = YamlWaypointParser(wps_file_path) @@ -46,7 +48,7 @@ def start_wpf(self): self.navigator.waitUntilNav2Active(localizer='robot_localization') wps = self.wp_parser.get_wps() self.navigator.followGpsWaypoints(wps) - while (not self.navigator.isTaskComplete()): + while not self.navigator.isTaskComplete(): time.sleep(0.1) print("Wps completed successfully") @@ -54,17 +56,45 @@ def start_wpf(self): def main(): rclpy.init() - # Allow to pass the waypoints file as an argument - default_yaml_file_path = os.path.join(get_package_share_directory( - "nav2_tutorial"), "config", "gps_waypoints.yaml") - if len(sys.argv) > 1: - yaml_file_path = sys.argv[1] + 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' + ) + args = parser.parse_args() + + config_dir = os.path.join( + get_package_share_directory('nav2_tutorial'), 'config' + ) + + if args.last: + pattern = os.path.join(config_dir, 'gps_waypoints_*.yaml') + files = glob.glob(pattern) + if not files: + print(f"No files matching {pattern}", file=sys.stderr) + sys.exit(1) + # filenames with timestamps sort lexically + files.sort() + yaml_file_path = files[-1] + print(f"Loading most recent waypoints file: {yaml_file_path}") else: - yaml_file_path = default_yaml_file_path + if args.yaml_file: + yaml_file_path = args.yaml_file + else: + yaml_file_path = os.path.join( + config_dir, 'gps_waypoints.yaml' + ) + print(f"Loading waypoints file: {yaml_file_path}") gps_wpf = GpsWpCommander(yaml_file_path) gps_wpf.start_wpf() -if __name__ == "__main__": +if __name__ == '__main__': main() From 6288f69b6cc5d35acaeaab7e740bdcd4a04a0ad6 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 4 Jun 2025 16:23:12 +0200 Subject: [PATCH 37/50] Minor traj handler update --- .gitignore | 3 + .../config/fp_driver_config.yaml | 2 +- src/nav2_tutorial/setup.py | 1 + .../src/logged_waypoint_follower.py | 182 ++++++++++++------ src/nav2_tutorial/src/terminal_logger.py | 10 +- .../gps_waypoints.yaml | 0 6 files changed, 135 insertions(+), 63 deletions(-) rename src/nav2_tutorial/{config => trajectories}/gps_waypoints.yaml (100%) diff --git a/.gitignore b/.gitignore index d78f6c5..4994d4d 100644 --- a/.gitignore +++ b/.gitignore @@ -3,6 +3,9 @@ log/ build/ install/ +# Ignore saved trajectories +src/nav2_tutorial/trajectories/gps_waypoints_* + # Byte-compiled / optimized / DLL files __pycache__/ *.py[cod] diff --git a/src/nav2_tutorial/config/fp_driver_config.yaml b/src/nav2_tutorial/config/fp_driver_config.yaml index 2ee87c1..c2f451d 100644 --- a/src/nav2_tutorial/config/fp_driver_config.yaml +++ b/src/nav2_tutorial/config/fp_driver_config.yaml @@ -4,7 +4,7 @@ # - Serial (:) # stream: serial:///dev/ttyUSB0:115200 # - TCP client (:) - stream: tcpcli://10.0.2.1:21000 + stream: tcpcli://10.1.1.2:21000 # Wait time in [s] to attempt reconnecting after connection lost reconnect_delay: 5.0 # Delay warning threshold [s]. Note that this only works if your system time is synced to the VRTK2. This must be a float. diff --git a/src/nav2_tutorial/setup.py b/src/nav2_tutorial/setup.py index ca8197e..ad88e6d 100644 --- a/src/nav2_tutorial/setup.py +++ b/src/nav2_tutorial/setup.py @@ -14,6 +14,7 @@ ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name, 'launch'), glob('launch/*.launch*')), (os.path.join('share', package_name, 'config'), glob('config/*')), + (os.path.join('share', package_name, 'trajectories'), glob('trajectories/*')), (os.path.join('share', package_name, 'urdf'), glob('urdf/*')), ], install_requires=['setuptools'], diff --git a/src/nav2_tutorial/src/logged_waypoint_follower.py b/src/nav2_tutorial/src/logged_waypoint_follower.py index 89751b7..9d628e5 100644 --- a/src/nav2_tutorial/src/logged_waypoint_follower.py +++ b/src/nav2_tutorial/src/logged_waypoint_follower.py @@ -2,8 +2,9 @@ import sys import time import yaml -import argparse import glob +import argparse +from datetime import datetime import rclpy from nav2_simple_commander.robot_navigator import BasicNavigator @@ -12,19 +13,108 @@ from src.utils.gps_utils import latLonYaw2Geopose -class YamlWaypointParser: +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. """ - Parse a set of gps waypoints from a yaml file + 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) - def __init__(self, wps_file_path: str) -> None: - with open(wps_file_path, 'r') as wps_file: - self.wps_dict = yaml.safe_load(wps_file) + 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}'.") def get_wps(self): - """ - Get an array of geographic_msgs/msg/GeoPose objects from the yaml file - """ + """Return a list of ``geographic_msgs/msg/GeoPose`` objects parsed from the YAML.""" gepose_wps = [] for wp in self.wps_dict["waypoints"]: latitude, longitude, yaw = wp["latitude"], wp["longitude"], wp["yaw"] @@ -33,68 +123,46 @@ def get_wps(self): class GpsWpCommander: - """ - Class to use nav2 gps waypoint follower to follow a set of waypoints logged in a yaml file - """ + """Use the Nav2 *GPS Waypoint Follower* to follow a set of waypoints.""" def __init__(self, wps_file_path: str): self.navigator = BasicNavigator("basic_navigator") self.wp_parser = YamlWaypointParser(wps_file_path) def start_wpf(self): - """ - Function to start the waypoint following - """ - self.navigator.waitUntilNav2Active(localizer='robot_localization') + """Block until all waypoints have been reached.""" + self.navigator.waitUntilNav2Active(localizer="robot_localization") wps = self.wp_parser.get_wps() self.navigator.followGpsWaypoints(wps) while not self.navigator.isTaskComplete(): time.sleep(0.1) - print("Wps completed successfully") - - -def main(): - rclpy.init() - - 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' - ) + print("Waypoints completed successfully") + + +def main(argv: list[str] | None = None): + rclpy.init(args=argv) + + parser = argparse.ArgumentParser(description="Follow GPS waypoints from a YAML file") + parser.add_argument("yaml_file", nargs="?", default=None, help="Path to the YAML waypoints file") + parser.add_argument("--last", action="store_true", help="Load the most recent waypoints file in the config directory") args = parser.parse_args() - config_dir = os.path.join( - get_package_share_directory('nav2_tutorial'), 'config' - ) + try: + yaml_file_path = _select_yaml(args) + except FileNotFoundError as e: + print(f"[WARN] {e}", file=sys.stderr) + sys.exit(1) - if args.last: - pattern = os.path.join(config_dir, 'gps_waypoints_*.yaml') - files = glob.glob(pattern) - if not files: - print(f"No files matching {pattern}", file=sys.stderr) - sys.exit(1) - # filenames with timestamps sort lexically - files.sort() - yaml_file_path = files[-1] - print(f"Loading most recent waypoints file: {yaml_file_path}") - else: - if args.yaml_file: - yaml_file_path = args.yaml_file - else: - yaml_file_path = os.path.join( - config_dir, 'gps_waypoints.yaml' - ) - print(f"Loading waypoints file: {yaml_file_path}") + # Feedback to the user + print(f"Loading waypoints file: {yaml_file_path}") - gps_wpf = GpsWpCommander(yaml_file_path) - gps_wpf.start_wpf() + try: + gps_wpf = GpsWpCommander(yaml_file_path) + gps_wpf.start_wpf() + except Exception as exc: + print(f"[ERROR] {exc}", file=sys.stderr) + sys.exit(1) -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/src/nav2_tutorial/src/terminal_logger.py b/src/nav2_tutorial/src/terminal_logger.py index 86b1122..f59bf49 100644 --- a/src/nav2_tutorial/src/terminal_logger.py +++ b/src/nav2_tutorial/src/terminal_logger.py @@ -112,18 +112,18 @@ def main(argv=None): ws_root = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(share_dir)))) # Build the path to your src/config - src_config = os.path.join(ws_root, 'src', 'nav2_tutorial', 'config') + src_config = os.path.join(ws_root, 'src', 'nav2_tutorial', 'trajectories') if os.path.isdir(src_config): - config_dir = src_config + traj_dir = src_config else: # Fallback to the installed share/config - config_dir = os.path.join(share_dir, 'config') + traj_dir = os.path.join(share_dir, 'trajectories') - os.makedirs(config_dir, exist_ok=True) + os.makedirs(traj_dir, exist_ok=True) stamp = datetime.now().strftime('%Y%m%d_%H%M%S') - yaml_path = os.path.join(config_dir, f'gps_waypoints_{stamp}.yaml') + yaml_path = os.path.join(traj_dir, f'gps_waypoints_{stamp}.yaml') node = GpsKeyLogger(yaml_path) diff --git a/src/nav2_tutorial/config/gps_waypoints.yaml b/src/nav2_tutorial/trajectories/gps_waypoints.yaml similarity index 100% rename from src/nav2_tutorial/config/gps_waypoints.yaml rename to src/nav2_tutorial/trajectories/gps_waypoints.yaml From 3ab26b6384ea69088d43d0e83ce8f28702aba60e Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 4 Jun 2025 16:36:10 +0200 Subject: [PATCH 38/50] Added reverse trajectory --- .../src/logged_waypoint_follower.py | 27 +++++++++++++------ 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/src/nav2_tutorial/src/logged_waypoint_follower.py b/src/nav2_tutorial/src/logged_waypoint_follower.py index 9d628e5..655092d 100644 --- a/src/nav2_tutorial/src/logged_waypoint_follower.py +++ b/src/nav2_tutorial/src/logged_waypoint_follower.py @@ -1,6 +1,7 @@ import os import sys import time +import math import yaml import glob import argparse @@ -113,26 +114,35 @@ def __init__(self, wps_file_path: str): if "waypoints" not in self.wps_dict: raise KeyError(f"Key 'waypoints' missing from YAML file '{wps_file_path}'.") - def get_wps(self): - """Return a list of ``geographic_msgs/msg/GeoPose`` objects parsed from the YAML.""" + @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 self.wps_dict["waypoints"]: - latitude, longitude, yaw = wp["latitude"], wp["longitude"], wp["yaw"] - gepose_wps.append(latLonYaw2Geopose(latitude, longitude, yaw)) + 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 class GpsWpCommander: """Use the Nav2 *GPS Waypoint Follower* to follow a set of waypoints.""" - def __init__(self, wps_file_path: str): + 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 start_wpf(self): """Block until all waypoints have been reached.""" self.navigator.waitUntilNav2Active(localizer="robot_localization") - wps = self.wp_parser.get_wps() + wps = self.wp_parser.get_wps(reverse=self.reverse) self.navigator.followGpsWaypoints(wps) while not self.navigator.isTaskComplete(): time.sleep(0.1) @@ -145,6 +155,7 @@ def main(argv: list[str] | None = None): 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)") args = parser.parse_args() try: @@ -157,7 +168,7 @@ def main(argv: list[str] | None = None): print(f"Loading waypoints file: {yaml_file_path}") try: - gps_wpf = GpsWpCommander(yaml_file_path) + gps_wpf = GpsWpCommander(yaml_file_path, reverse=args.reverse) gps_wpf.start_wpf() except Exception as exc: print(f"[ERROR] {exc}", file=sys.stderr) From dcb4a740fd2d92d9bf741a643457e3286dc8e346 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 4 Jun 2025 18:49:14 +0200 Subject: [PATCH 39/50] Improved loggers --- README.md | 16 +- src/nav2_tutorial/setup.py | 2 + .../src/logged_smooth_follower.py | 244 ++++++++++++++++++ src/nav2_tutorial/src/periodic_logger.py | 196 ++++++++++++++ 4 files changed, 455 insertions(+), 3 deletions(-) create mode 100644 src/nav2_tutorial/src/logged_smooth_follower.py create mode 100644 src/nav2_tutorial/src/periodic_logger.py diff --git a/README.md b/README.md index 52a1c3d..4c2d8e4 100644 --- a/README.md +++ b/README.md @@ -161,7 +161,7 @@ ros2 run nav2_tutorial interactive_waypoint_follower First, the user must populate the predefined gps_waypoints.yaml file with the waypoints the robot has to follow. The user can either provide the waypoints manually or use the provided waypoint logging tool as shown below: ``` -ros2 run nav2_tutorial gps_waypoint_logger /path/to/file +ros2 run nav2_tutorial gps_waypoint_logger ``` If your terminal does not support X11 forwarding, you can use the following script: @@ -169,7 +169,17 @@ If your terminal does not support X11 forwarding, you can use the following scri ros2 run nav2_tutorial terminal_logger ``` -Then, call the logged_waypoint_follower script to make the robot follow the logged waypoints. +Lastly, you can use a periodic logger to have denser points: ``` -ros2 run nav2_tutorial logged_waypoint_follower /path/to/file +ros2 run nav2_tutorial periodic_logger +``` + +Then, call the logged_waypoint_follower script to make the robot follow the logged waypoints: +``` +ros2 run nav2_tutorial logged_waypoint_follower +``` + +For a smooth version using a pre-computed trajectory (instead of point-by-point), call the logged_smooth_follower script: +``` +ros2 run nav2_tutorial logged_smooth_follower ``` diff --git a/src/nav2_tutorial/setup.py b/src/nav2_tutorial/setup.py index ad88e6d..4b8630f 100644 --- a/src/nav2_tutorial/setup.py +++ b/src/nav2_tutorial/setup.py @@ -27,9 +27,11 @@ entry_points={ 'console_scripts': [ 'logged_waypoint_follower = src.logged_waypoint_follower:main', + 'logged_smooth_follower = src.logged_smooth_follower:main', 'interactive_waypoint_follower = src.interactive_waypoint_follower:main', 'gps_waypoint_logger = src.gps_waypoint_logger:main', 'terminal_logger = src.terminal_logger:main', + 'periodic_logger = src.periodic_logger:main', 'set_datum = src.set_datum:main' ], }, diff --git a/src/nav2_tutorial/src/logged_smooth_follower.py b/src/nav2_tutorial/src/logged_smooth_follower.py new file mode 100644 index 0000000..fd8e4f3 --- /dev/null +++ b/src/nav2_tutorial/src/logged_smooth_follower.py @@ -0,0 +1,244 @@ +import os +import sys +import time +import math +import yaml +import glob +import argparse +from datetime import datetime + +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 + +from geometry_msgs.msg import PoseStamped +from robot_localization.srv import FromLL + + +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 π, 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 + + +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 + + # FromLL client (WGS-84 → map frame) – create it on the navigator node + self._fromll_cli = self.navigator.create_client(FromLL, "/fromLL") + self.navigator.get_logger().info("Waiting for /fromLL service…") + if not self._fromll_cli.wait_for_service(timeout_sec=10.0): + raise RuntimeError("robot_localization /fromLL service is not available") + + # Convert (lat, lon, yaw) to PoseStamped in the map frame + def _latlon_to_pose(self, lat: float, lon: float, yaw: float) -> PoseStamped: + req = FromLL.Request() + req.ll_point.latitude = lat + req.ll_point.longitude = lon + req.ll_point.altitude = 0.0 + + future = self._fromll_cli.call_async(req) + rclpy.spin_until_future_complete(self.navigator, future) + map_pt = future.result().map_point + + pose = PoseStamped() + pose.header.frame_id = "map" + pose.header.stamp = self.navigator.get_clock().now().to_msg() + pose.pose.position.x = map_pt.x + pose.pose.position.y = map_pt.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 + + def start_ntp(self): + """Send one NavigateThroughPoses goal and block until done.""" + self.navigator.waitUntilNav2Active(localizer="robot_localization") + + raw_wps = self.wp_parser.wps_dict["waypoints"] + if self.reverse: + raw_wps = raw_wps[::-1] + + poses = [] + for wp in raw_wps: + yaw = (self.wp_parser._reverse_yaw(wp["yaw"]) + if self.reverse else wp["yaw"]) + poses.append(self._latlon_to_pose( + wp["latitude"], wp["longitude"], yaw)) + + self.navigator.goThroughPoses(poses) + + while not self.navigator.isTaskComplete(): + rclpy.spin_once(self.navigator, timeout_sec=0.1) + + print("Waypoints completed successfully (continuous mode)") + + +def main(argv: list[str] | None = None): + rclpy.init(args=argv) + + parser = argparse.ArgumentParser(description="Follow GPS waypoints from a YAML file") + parser.add_argument("yaml_file", nargs="?", default=None, help="Path to the YAML waypoints file") + parser.add_argument("--last", action="store_true", help="Load the most recent waypoints file in the config directory") + parser.add_argument("--reverse", action="store_true", help="Follow the trajectory in reverse order (adds 180° to yaw)") + args = parser.parse_args() + + try: + yaml_file_path = _select_yaml(args) + except FileNotFoundError as e: + print(f"[WARN] {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_ntp() + except Exception as exc: + print(f"[ERROR] {exc}", file=sys.stderr) + sys.exit(1) + + +if __name__ == "__main__": + main() diff --git a/src/nav2_tutorial/src/periodic_logger.py b/src/nav2_tutorial/src/periodic_logger.py new file mode 100644 index 0000000..76f6df5 --- /dev/null +++ b/src/nav2_tutorial/src/periodic_logger.py @@ -0,0 +1,196 @@ +#!/usr/bin/env python3 +""" +Periodic GPS waypoint logger for ROS2 +============================================================== + +* Saves a waypoint every *interval* seconds (default: 0.2s). +* Logs when logging starts (first waypoint collected). +* Prints a throttled status message containing the most recent waypoint every 2s. +* Press **q** in the console to stop recording gracefully. + +Usage examples:: + + ros2 run gps_periodic_logger.py # default + ros2 run gps_periodic_logger.py -i 0.1 # faster + ros2 run gps_periodic_logger.py logs/points.yaml +""" + +from __future__ import annotations + +import argparse +import os +import select +import sys +import termios +import tty +from datetime import datetime + +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry +from sensor_msgs.msg import NavSatFix + +import yaml +from ament_index_python.packages import get_package_share_directory + +from src.utils.gps_utils import euler_from_quaternion + + +class GpsPeriodicLogger(Node): + """ROS2 node that periodically logs GPS waypoints to a YAML file.""" + + def __init__(self, logging_file_path: str, interval: float): + super().__init__('gps_waypoint_periodic_logger') + + self.logging_file_path = logging_file_path + self.log_interval = interval + self.last_gps: NavSatFix | None = None + self.last_yaw: float = 0.0 + self.saved_points: int = 0 + + # ------------------------------------------------------------------ + # Topic subscriptions + # ------------------------------------------------------------------ + 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, + ) + + # ------------------------------------------------------------------ + # Timers: waypoint logger + throttled status at 2 s + # ------------------------------------------------------------------ + self.create_timer(self.log_interval, self._log_waypoint) + self.create_timer(2.0, self._status_message) + + self.get_logger().info( + f"Will log a waypoint every {self.log_interval:.3f} s to '{self.logging_file_path}'." + ) + + # ------------------------------------------------------------------ + # Topic callbacks + # ------------------------------------------------------------------ + def gps_callback(self, msg: NavSatFix) -> None: + self.last_gps = msg + + def yaw_callback(self, msg: Odometry) -> None: + _, _, self.last_yaw = euler_from_quaternion(msg.pose.pose.orientation) + + # ------------------------------------------------------------------ + # Internal helpers + # ------------------------------------------------------------------ + def _log_waypoint(self) -> None: + """Append the current pose to YAML.""" + if self.last_gps is None: + return # wait for first fix + + if self.saved_points == 0: + # First time we succeed in logging – announce start of collection. + self.get_logger().info("Started collecting waypoints …") + + waypoint = { + 'latitude': self.last_gps.latitude, + 'longitude': self.last_gps.longitude, + 'yaw': self.last_yaw, + } + + # Read existing file + try: + with open(self.logging_file_path, 'r') as fh: + data: dict = yaml.safe_load(fh) or {} + except FileNotFoundError: + data = {} + except Exception as exc: + self.get_logger().error(f"Failed to read log file: {exc}") + return + + waypoints = data.get('waypoints', []) + waypoints.append(waypoint) + data['waypoints'] = waypoints + + # Write back updated YAML + try: + with open(self.logging_file_path, 'w') as fh: + yaml.dump(data, fh, default_flow_style=False) + except Exception as exc: + self.get_logger().error(f"Failed to write log file: {exc}") + return + + self.saved_points += 1 + + def _status_message(self) -> None: + """Emit a throttled info log with the most recently recorded waypoint.""" + if self.saved_points == 0: + return + + self.get_logger().info( + f"Total waypoints saved: {self.saved_points} | " + f"Last -> lat={self.last_gps.latitude:.6f}, " + 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') + + # share/nav2_tutorial ➜ share ➜ install ➜ ws root + 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 _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.') + parser.add_argument('-i', '--interval', type=float, default=0.2, help='Save interval in seconds (default 0.2).') + args = parser.parse_args(argv) + yaml_path = args.yaml or _default_yaml_path() + return yaml_path, args.interval + + +def _stdin_ready() -> bool: + return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) + + +def main(argv: list[str] | None = None) -> None: + yaml_path, interval = _parse_arguments(argv) + + rclpy.init(args=argv) + node = GpsPeriodicLogger(yaml_path, interval) + + # Configure terminal for non‑blocking single‑character input + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + tty.setcbreak(fd) + + try: + while rclpy.ok(): + # Spin (but return regularly so we can poll the keyboard) + rclpy.spin_once(node, timeout_sec=0.1) + + if _stdin_ready(): + ch = sys.stdin.read(1) + if ch == 'q': + node.get_logger().info("'q' pressed - stopping waypoint recording.") + break + except KeyboardInterrupt: + node.get_logger().info("Interrupted by user, shutting down.") + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + rclpy.shutdown() + + +if __name__ == '__main__': + main() From 7ab26915203c3a530b196db4bad9d20b506b05b2 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Tue, 10 Jun 2025 14:50:06 +0200 Subject: [PATCH 40/50] Working smooth slow-motion --- src/nav2_tutorial/config/nav2_params.yaml | 102 +++------------------- 1 file changed, 11 insertions(+), 91 deletions(-) diff --git a/src/nav2_tutorial/config/nav2_params.yaml b/src/nav2_tutorial/config/nav2_params.yaml index de09534..72c2289 100644 --- a/src/nav2_tutorial/config/nav2_params.yaml +++ b/src/nav2_tutorial/config/nav2_params.yaml @@ -1,11 +1,4 @@ -# GPS WPF CHANGES: -# - amcl params where removed. They are not needed because global localization is provided -# by an ekf node on robot_localization fusing gps data with local odometry sources -# - static layer is removed from both costmaps, in this tutorial we assume there is no map -# of the environment -# - global costmap is set to be rolling to allow the robot to traverse big environment by -# following successive relatively close waypoints that fit in a smaller rolling costmap - +# Nav2 parameters bt_navigator: ros__parameters: global_frame: map @@ -18,62 +11,6 @@ bt_navigator: plugin: "nav2_bt_navigator::NavigateToPoseNavigator" navigate_through_poses: plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" - # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: - # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml - # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - # plugin_lib_names: - # - nav2_compute_path_to_pose_action_bt_node - # - nav2_compute_path_through_poses_action_bt_node - # - nav2_smooth_path_action_bt_node - # - nav2_follow_path_action_bt_node - # - nav2_spin_action_bt_node - # - nav2_wait_action_bt_node - # - nav2_assisted_teleop_action_bt_node - # - nav2_back_up_action_bt_node - # - nav2_drive_on_heading_bt_node - # - nav2_clear_costmap_service_bt_node - # - nav2_is_stuck_condition_bt_node - # - nav2_goal_reached_condition_bt_node - # - nav2_goal_updated_condition_bt_node - # - nav2_globally_updated_goal_condition_bt_node - # - nav2_is_path_valid_condition_bt_node - # - nav2_are_error_codes_active_condition_bt_node - # - nav2_would_a_controller_recovery_help_condition_bt_node - # - nav2_would_a_planner_recovery_help_condition_bt_node - # - nav2_would_a_smoother_recovery_help_condition_bt_node - # - nav2_initial_pose_received_condition_bt_node - # - nav2_reinitialize_global_localization_service_bt_node - # - nav2_rate_controller_bt_node - # - nav2_distance_controller_bt_node - # - nav2_speed_controller_bt_node - # - nav2_truncate_path_action_bt_node - # - nav2_truncate_path_local_action_bt_node - # - nav2_goal_updater_node_bt_node - # - nav2_recovery_node_bt_node - # - nav2_pipeline_sequence_bt_node - # - nav2_round_robin_node_bt_node - # - nav2_transform_available_condition_bt_node - # - nav2_time_expired_condition_bt_node - # - nav2_path_expiring_timer_condition - # - nav2_distance_traveled_condition_bt_node - # - nav2_single_trigger_bt_node - # - nav2_goal_updated_controller_bt_node - # - nav2_is_battery_low_condition_bt_node - # - nav2_navigate_through_poses_action_bt_node - # - nav2_navigate_to_pose_action_bt_node - # - nav2_remove_passed_goals_action_bt_node - # - nav2_planner_selector_bt_node - # - nav2_controller_selector_bt_node - # - nav2_goal_checker_selector_bt_node - # - nav2_controller_cancel_bt_node - # - nav2_path_longer_on_approach_bt_node - # - nav2_wait_cancel_bt_node - # - nav2_spin_cancel_bt_node - # - nav2_back_up_cancel_bt_node - # - nav2_assisted_teleop_cancel_bt_node - # - nav2_drive_on_heading_cancel_bt_node - # - nav2_is_battery_charging_condition_bt_node error_code_names: - compute_path_error_code - follow_path_error_code @@ -95,8 +32,8 @@ controller_server: # Progress checker parameters progress_checker: plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.5 - movement_time_allowance: 10.0 + required_movement_radius: 0.2 + movement_time_allowance: 20.0 # Goal checker parameters #precise_goal_checker: # plugin: "nav2_controller::SimpleGoalChecker" @@ -120,8 +57,6 @@ controller_server: min_speed_xy: 0.0 max_speed_xy: 0.26 min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 acc_lim_x: 2.5 acc_lim_y: 0.0 acc_lim_theta: 3.2 @@ -131,7 +66,7 @@ controller_server: vx_samples: 20 vy_samples: 5 vtheta_samples: 20 - sim_time: 1.2 + sim_time: 5.0 linear_granularity: 0.05 angular_granularity: 0.025 transform_tolerance: 0.2 @@ -141,17 +76,16 @@ controller_server: stateful: True critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 + PathAlign.scale: 100.0 PathAlign.forward_point_distance: 0.1 GoalAlign.scale: 24.0 GoalAlign.forward_point_distance: 0.1 PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 + GoalDist.scale: 100.0 + RotateToGoal.scale: 10.0 RotateToGoal.slowing_factor: 5.0 RotateToGoal.lookahead_time: -1.0 -# GPS WPF CHANGE: Remove static layer local_costmap: local_costmap: ros__parameters: @@ -191,8 +125,6 @@ local_costmap: obstacle_min_range: 0.0 always_send_full_costmap: True -# GPS WPF CHANGE: Remove static layer -# GPS WPF CHANGE: Set rolling global costmap with 50x50 size. See note below global_costmap: global_costmap: ros__parameters: @@ -201,15 +133,11 @@ global_costmap: global_frame: map robot_base_frame: base_link robot_radius: 0.22 - resolution: 0.1 - # When using GPS navigation you will potentially traverse huge environments which are not practical to - # fit on a big static costmap. Thus it is recommended to use a rolling global costmap large enough to - # contain each pair of successive waypoints. See: https://github.com/ros-planning/navigation2/issues/2174 + resolution: 0.05 rolling_window: True width: 50 height: 50 track_unknown_space: true - # no static map plugins: ["obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" @@ -225,21 +153,13 @@ global_costmap: raytrace_min_range: 0.0 obstacle_max_range: 2.5 obstacle_min_range: 0.0 - # outdoors there will probably be more inf points - inf_is_valid: true + inf_is_valid: true # outdoors there will probably be more inf points inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 inflation_radius: 0.55 always_send_full_costmap: True -# The yaml_filename does not need to be specified since it going to be set by defaults in launch. -# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py -# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. -# map_server: -# ros__parameters: -# yaml_filename: "" - map_saver: ros__parameters: save_map_timeout: 5.0 @@ -301,8 +221,8 @@ waypoint_follower: waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: True - waypoint_pause_duration: 200 + enabled: False + waypoint_pause_duration: 0 velocity_smoother: ros__parameters: From e36821688a0f11d3827860867be5cb1f22db13a1 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 18 Jun 2025 13:54:51 +0200 Subject: [PATCH 41/50] Improved waypoint follower --- src/nav2_tutorial/config/nav2_params.yaml | 202 +++++---------- .../config/nav2_params_sensors.yaml | 239 ++++++++++++++++++ src/nav2_tutorial/launch/navigation.launch.py | 6 +- .../src/logged_smooth_follower.py | 49 +++- 4 files changed, 353 insertions(+), 143 deletions(-) create mode 100644 src/nav2_tutorial/config/nav2_params_sensors.yaml diff --git a/src/nav2_tutorial/config/nav2_params.yaml b/src/nav2_tutorial/config/nav2_params.yaml index 72c2289..5757000 100644 --- a/src/nav2_tutorial/config/nav2_params.yaml +++ b/src/nav2_tutorial/config/nav2_params.yaml @@ -21,144 +21,76 @@ controller_server: min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.3 + failure_tolerance: 0.0 progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] - current_goal_checker: "goal_checker" - current_progress_checker: "progress_checker" - enable_stamped_cmd_vel: false - # Progress checker parameters progress_checker: plugin: "nav2_controller::SimpleProgressChecker" required_movement_radius: 0.2 movement_time_allowance: 20.0 - # Goal checker parameters - #precise_goal_checker: - # plugin: "nav2_controller::SimpleGoalChecker" - # xy_goal_tolerance: 0.25 - # yaw_goal_tolerance: 0.25 - # stateful: True goal_checker: - stateful: True plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 - # DWB parameters - FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.26 - max_vel_y: 0.0 - max_vel_theta: 0.5 - min_speed_xy: 0.0 - max_speed_xy: 0.26 - min_speed_theta: 0.0 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -4.2 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 5.0 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.25 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 100.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 100.0 - RotateToGoal.scale: 10.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 + FollowPath: + plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + desired_linear_vel: 0.8 + lookahead_dist: 1.0 + min_lookahead_dist: 0.2 + max_lookahead_dist: 2.0 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 1.0 + transform_tolerance: 0.5 + use_velocity_scaled_lookahead_dist: false + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 0.6 + use_collision_detection: false + max_allowed_time_to_collision_up_to_carrot: 1.0 + use_regulated_linear_velocity_scaling: false + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 0.25 + use_cost_regulated_linear_velocity_scaling: false + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.2 + use_rotate_to_heading: false + allow_reversing: false + rotate_to_heading_min_angle: 0.785 + max_angular_accel: 3.2 + max_robot_pose_search_dist: 50.0 + stateful: true -local_costmap: - local_costmap: +global_costmap: + global_costmap: ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 - global_frame: odom + global_frame: map robot_base_frame: base_link - rolling_window: true - width: 3 - height: 3 + width: 50 + height: 50 resolution: 0.05 - robot_radius: 0.22 - plugins: ["voxel_layer", "inflation_layer"] + rolling_window: true + # no sensors, so only inflation + plugins: ["inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 inflation_radius: 0.55 - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - always_send_full_costmap: True -global_costmap: - global_costmap: +local_costmap: + local_costmap: ros__parameters: - update_frequency: 1.0 - publish_frequency: 1.0 - global_frame: map + global_frame: odom robot_base_frame: base_link - robot_radius: 0.22 + width: 3 + height: 3 resolution: 0.05 - rolling_window: True - width: 50 - height: 50 - track_unknown_space: true - plugins: ["obstacle_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - inf_is_valid: true # outdoors there will probably be more inf points + rolling_window: true + plugins: ["inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 inflation_radius: 0.55 - always_send_full_costmap: True + map_saver: ros__parameters: @@ -169,22 +101,22 @@ map_saver: planner_server: ros__parameters: - expected_planner_frequency: 20.0 + expected_planner_frequency: 1.0 planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" - tolerance: 0.5 + tolerance: 1.0 use_astar: false allow_unknown: true -smoother_server: - ros__parameters: - smoother_plugins: ["simple_smoother"] - simple_smoother: - plugin: "nav2_smoother::SimpleSmoother" - tolerance: 1.0e-10 - max_its: 1000 - do_refinement: True +# smoother_server: +# ros__parameters: +# smoother_plugins: ["simple_smoother"] +# simple_smoother: +# plugin: "nav2_smoother::SimpleSmoother" +# tolerance: 1.0e-10 +# max_its: 1000 +# do_refinement: True behavior_server: ros__parameters: @@ -207,7 +139,7 @@ behavior_server: local_frame: odom global_frame: map robot_base_frame: base_link - transform_tolerance: 0.1 + transform_tolerance: 0.5 simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 @@ -224,16 +156,16 @@ waypoint_follower: enabled: False waypoint_pause_duration: 0 -velocity_smoother: - ros__parameters: - smoothing_frequency: 20.0 - scale_velocities: False - feedback: "OPEN_LOOP" - max_velocity: [0.26, 0.0, 1.0] - min_velocity: [-0.26, 0.0, -1.0] - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] - odom_topic: "odom" - odom_duration: 0.1 - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 \ No newline at end of file +# velocity_smoother: +# ros__parameters: +# smoothing_frequency: 20.0 +# scale_velocities: False +# feedback: "OPEN_LOOP" +# max_velocity: [1.0, 0.0, 1.0] +# min_velocity: [-1.0, 0.0, -1.0] +# max_accel: [2.5, 0.0, 3.2] +# max_decel: [-2.5, 0.0, -3.2] +# odom_topic: "odom" +# odom_duration: 0.1 +# deadband_velocity: [0.0, 0.0, 0.0] +# velocity_timeout: 1.0 \ No newline at end of file diff --git a/src/nav2_tutorial/config/nav2_params_sensors.yaml b/src/nav2_tutorial/config/nav2_params_sensors.yaml new file mode 100644 index 0000000..72c2289 --- /dev/null +++ b/src/nav2_tutorial/config/nav2_params_sensors.yaml @@ -0,0 +1,239 @@ +# Nav2 parameters +bt_navigator: + ros__parameters: + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + error_code_names: + - compute_path_error_code + - follow_path_error_code + +controller_server: + ros__parameters: + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + current_goal_checker: "goal_checker" + current_progress_checker: "progress_checker" + enable_stamped_cmd_vel: false + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.2 + movement_time_allowance: 20.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 0.5 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -4.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 5.0 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 100.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 100.0 + RotateToGoal.scale: 10.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.05 + rolling_window: True + width: 50 + height: 50 + track_unknown_space: true + plugins: ["obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + inf_is_valid: true # outdoors there will probably be more inf points + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_saver: + ros__parameters: + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner::NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors::Spin" + backup: + plugin: "nav2_behaviors::BackUp" + drive_on_heading: + plugin: "nav2_behaviors::DriveOnHeading" + wait: + plugin: "nav2_behaviors::Wait" + assisted_teleop: + plugin: "nav2_behaviors::AssistedTeleop" + local_frame: odom + global_frame: map + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + enable_stamped_cmd_vel: false + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: False + waypoint_pause_duration: 0 + +velocity_smoother: + ros__parameters: + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 \ No newline at end of file diff --git a/src/nav2_tutorial/launch/navigation.launch.py b/src/nav2_tutorial/launch/navigation.launch.py index 3b3fc0c..e82b809 100644 --- a/src/nav2_tutorial/launch/navigation.launch.py +++ b/src/nav2_tutorial/launch/navigation.launch.py @@ -42,14 +42,12 @@ def generate_launch_description(): lifecycle_nodes = [ 'controller_server', - 'smoother_server', + # 'smoother_server', 'planner_server', 'behavior_server', - 'velocity_smoother', -# 'collision_monitor', + # 'velocity_smoother', 'bt_navigator', 'waypoint_follower', -# 'docking_server', ] # Map fully qualified names to relative ones so the node's namespace can be prepended. diff --git a/src/nav2_tutorial/src/logged_smooth_follower.py b/src/nav2_tutorial/src/logged_smooth_follower.py index fd8e4f3..9c19e36 100644 --- a/src/nav2_tutorial/src/logged_smooth_follower.py +++ b/src/nav2_tutorial/src/logged_smooth_follower.py @@ -14,7 +14,13 @@ from src.utils.gps_utils import latLonYaw2Geopose from geometry_msgs.msg import PoseStamped -from robot_localization.srv import FromLL +from robot_localization.srv import FromLL + +# Visualization +from nav_msgs.msg import Path +from visualization_msgs.msg import Marker, MarkerArray +from rclpy.qos import QoSProfile, DurabilityPolicy +MARKER_VIZ = True def quaternion_from_euler(roll: float, pitch: float, yaw: float): @@ -138,9 +144,8 @@ def __init__(self, wps_file_path: str): @staticmethod def _reverse_yaw(yaw: float) -> float: - """Return yaw rotated by π, wrapped to [-π, π].""" + """Return yaw rotated by pi, wrapped to [-pi, pi].""" new_yaw = yaw + math.pi - # Wrap to [-π, π] return (new_yaw + math.pi) % (2 * math.pi) - math.pi def get_wps(self, reverse: bool = False): @@ -166,6 +171,18 @@ def __init__(self, wps_file_path: str, reverse: bool = False): self.navigator.get_logger().info("Waiting for /fromLL service…") if not self._fromll_cli.wait_for_service(timeout_sec=10.0): raise RuntimeError("robot_localization /fromLL service is not available") + + if MARKER_VIZ: + latched_qos = QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + + # Path for visualizing waypoints + 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) # Convert (lat, lon, yaw) to PoseStamped in the map frame def _latlon_to_pose(self, lat: float, lon: float, yaw: float) -> PoseStamped: @@ -180,7 +197,7 @@ def _latlon_to_pose(self, lat: float, lon: float, yaw: float) -> PoseStamped: pose = PoseStamped() pose.header.frame_id = "map" - pose.header.stamp = self.navigator.get_clock().now().to_msg() + pose.header.stamp = rclpy.time.Time().to_msg() pose.pose.position.x = map_pt.x pose.pose.position.y = map_pt.y pose.pose.position.z = 0.0 @@ -206,12 +223,36 @@ def start_ntp(self): poses.append(self._latlon_to_pose( wp["latitude"], wp["longitude"], yaw)) + self._publish_waypoints(poses) self.navigator.goThroughPoses(poses) while not self.navigator.isTaskComplete(): rclpy.spin_once(self.navigator, timeout_sec=0.1) print("Waypoints completed successfully (continuous mode)") + + def _publish_waypoints(self, poses): + # Path message + path = Path() + path.header.frame_id = "map" + path.header.stamp = rclpy.time.Time().to_msg() + path.poses = poses + self.waypoint_path_pub.publish(path) + + # Optional coloured spheres + markers = MarkerArray() + for i, p in enumerate(poses): + m = Marker() + m.header = path.header + m.ns = "wps" + m.id = i + m.type = Marker.SPHERE + m.action = Marker.ADD + m.pose = p.pose + m.scale.x = m.scale.y = m.scale.z = 0.05 + m.color.r, m.color.g, m.color.b, m.color.a = (0.1, 0.9, 0.1, 1.0) + markers.markers.append(m) + self.waypoint_marker_pub.publish(markers) def main(argv: list[str] | None = None): From 7170a27969fa01dccb8b27e4941a4b7faa139716 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 18 Jun 2025 14:29:56 +0200 Subject: [PATCH 42/50] Working around office --- src/nav2_tutorial/config/nav2_params.yaml | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/nav2_tutorial/config/nav2_params.yaml b/src/nav2_tutorial/config/nav2_params.yaml index 5757000..b9ca7e5 100644 --- a/src/nav2_tutorial/config/nav2_params.yaml +++ b/src/nav2_tutorial/config/nav2_params.yaml @@ -23,9 +23,8 @@ controller_server: min_theta_velocity_threshold: 0.001 failure_tolerance: 0.0 progress_checker_plugins: ["progress_checker"] - goal_checker_plugins: ["goal_checker"] # "precise_goal_checker" + goal_checker_plugins: ["goal_checker"] controller_plugins: ["FollowPath"] - progress_checker: plugin: "nav2_controller::SimpleProgressChecker" required_movement_radius: 0.2 @@ -59,7 +58,7 @@ controller_server: allow_reversing: false rotate_to_heading_min_angle: 0.785 max_angular_accel: 3.2 - max_robot_pose_search_dist: 50.0 + max_robot_pose_search_dist: 100.0 stateful: true global_costmap: @@ -67,12 +66,11 @@ global_costmap: ros__parameters: global_frame: map robot_base_frame: base_link - width: 50 - height: 50 + width: 100 + height: 100 resolution: 0.05 rolling_window: true - # no sensors, so only inflation - plugins: ["inflation_layer"] + plugins: ["inflation_layer"] # no sensors, so only inflation inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" inflation_radius: 0.55 @@ -91,7 +89,6 @@ local_costmap: plugin: "nav2_costmap_2d::InflationLayer" inflation_radius: 0.55 - map_saver: ros__parameters: save_map_timeout: 5.0 From b480bb1f5372b77caf9a5e2d230080205ea9a0f7 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Tue, 24 Jun 2025 17:22:36 +0200 Subject: [PATCH 43/50] Improved navigation for long sequences --- src/nav2_tutorial/config/nav2_params.yaml | 15 +- .../src/logged_smooth_follower.py | 145 ++++++++++++++++-- 2 files changed, 145 insertions(+), 15 deletions(-) diff --git a/src/nav2_tutorial/config/nav2_params.yaml b/src/nav2_tutorial/config/nav2_params.yaml index b9ca7e5..d1344d5 100644 --- a/src/nav2_tutorial/config/nav2_params.yaml +++ b/src/nav2_tutorial/config/nav2_params.yaml @@ -6,6 +6,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + transform_tolerance: 0.3 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator::NavigateToPoseNavigator" @@ -36,7 +37,7 @@ controller_server: stateful: True FollowPath: plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" - desired_linear_vel: 0.8 + desired_linear_vel: 1.3 lookahead_dist: 1.0 min_lookahead_dist: 0.2 max_lookahead_dist: 2.0 @@ -54,11 +55,11 @@ controller_server: use_cost_regulated_linear_velocity_scaling: false regulated_linear_scaling_min_radius: 0.9 regulated_linear_scaling_min_speed: 0.2 - use_rotate_to_heading: false + use_rotate_to_heading: true allow_reversing: false rotate_to_heading_min_angle: 0.785 max_angular_accel: 3.2 - max_robot_pose_search_dist: 100.0 + max_robot_pose_search_dist: 50.0 stateful: true global_costmap: @@ -66,28 +67,30 @@ global_costmap: ros__parameters: global_frame: map robot_base_frame: base_link - width: 100 - height: 100 resolution: 0.05 rolling_window: true + width: 50 + height: 50 plugins: ["inflation_layer"] # no sensors, so only inflation inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" inflation_radius: 0.55 + always_send_full_costmap: True local_costmap: local_costmap: ros__parameters: global_frame: odom robot_base_frame: base_link + rolling_window: true width: 3 height: 3 resolution: 0.05 - rolling_window: true plugins: ["inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" inflation_radius: 0.55 + always_send_full_costmap: True map_saver: ros__parameters: diff --git a/src/nav2_tutorial/src/logged_smooth_follower.py b/src/nav2_tutorial/src/logged_smooth_follower.py index 9c19e36..6e3518d 100644 --- a/src/nav2_tutorial/src/logged_smooth_follower.py +++ b/src/nav2_tutorial/src/logged_smooth_follower.py @@ -8,13 +8,14 @@ from datetime import datetime import rclpy -from nav2_simple_commander.robot_navigator import BasicNavigator +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 geometry_msgs.msg import PoseStamped from robot_localization.srv import FromLL +from nav_msgs.msg import Odometry # Visualization from nav_msgs.msg import Path @@ -166,6 +167,18 @@ def __init__(self, wps_file_path: str, reverse: bool = False): self.wp_parser = YamlWaypointParser(wps_file_path) self.reverse = reverse + # Odometry topic with ENU pose in map frame + self._last_odom_pose: PoseStamped | None = None + self.navigator.create_subscription(Odometry, "/fixposition/odometry_enu", self._odom_cb, 10) + + # Parameters for the NavigateThroughPoses task + self.max_window = 50.0 # meters (square side length) + self.safety_margin = 1.0 # meters (keep a little inside the edge) + self.max_seg_len = 100.0 # meters of path length per sub-goal + self.radius = self.max_window/2 - self.safety_margin # ≈ 24 m + self.odom_timeout_s = 2.0 # wait this long for first /fixposition msg + self.max_retries = 1 # re-attempt each failed segment once + # FromLL client (WGS-84 → map frame) – create it on the navigator node self._fromll_cli = self.navigator.create_client(FromLL, "/fromLL") self.navigator.get_logger().info("Waiting for /fromLL service…") @@ -209,7 +222,10 @@ def _latlon_to_pose(self, lat: float, lon: float, yaw: float) -> PoseStamped: return pose def start_ntp(self): - """Send one NavigateThroughPoses goal and block until done.""" + """Send *multiple* NavigateThroughPoses goals whenever the full list of + waypoints would not fit inside the rolling global cost-map. Each sub-goal + is guaranteed to stay inside a radius *R* around the pose that was current + when the sub-goal was dispatched.""" self.navigator.waitUntilNav2Active(localizer="robot_localization") raw_wps = self.wp_parser.wps_dict["waypoints"] @@ -223,13 +239,124 @@ def start_ntp(self): poses.append(self._latlon_to_pose( wp["latitude"], wp["longitude"], yaw)) - self._publish_waypoints(poses) - self.navigator.goThroughPoses(poses) - - while not self.navigator.isTaskComplete(): - rclpy.spin_once(self.navigator, timeout_sec=0.1) - - print("Waypoints completed successfully (continuous mode)") + seg_idx = 0 + remaining = poses + retries_left = self.max_retries + + while remaining: + # Current robot pose in map frame (start-of-segment centre) + try: + cur_pose = self._get_robot_pose() + except Exception as e: + self.navigator.get_logger().error(f"Failed to get robot pose: {e}") + return + cx, cy = cur_pose.pose.position.x, cur_pose.pose.position.y + + # Build one segment whose every pose lies inside the radius R + segment = [] + cum_length = 0.0 # running distance + prev_xy = (cx, cy) # start measuring from robot + + for p in remaining: + dx = p.pose.position.x - cx + dy = p.pose.position.y - cy + + # Window test **in map axes** (square, not circle) + half_win = self.max_window/2 - self.safety_margin + inside_window = (abs(dx) <= half_win) and (abs(dy) <= half_win) + + # Distance if we *would* append this pose + step = math.hypot(p.pose.position.x - prev_xy[0], + p.pose.position.y - prev_xy[1]) + inside_length = (cum_length + step) <= self.max_seg_len + + if not (inside_window and inside_length): + # Adding this pose would violate at least one rule + if segment: # we already have a valid chunk + break # finish the segment here + else: + # Even the *first* pose is out of bounds → force-split + # so we can advance and recompute next time + self.navigator.get_logger().warn( + f"Pose {(p.pose.position.x, p.pose.position.y)} " + "is out of the current 50x50 m window - " + "sending it alone.") + segment.append(p) + break + + # Safe to add + segment.append(p) + cum_length += step + prev_xy = (p.pose.position.x, p.pose.position.y) + + # Publish & send this segment + self._publish_waypoints(segment) + self.navigator.goThroughPoses(segment) + + seg_idx += 1 + self.navigator.get_logger().info(f"Segment {seg_idx}: {len(segment)} poses") + + while not self.navigator.isTaskComplete(): + rclpy.spin_once(self.navigator, timeout_sec=0.1) + + # Assess the result of the NavigateThroughPoses task + result = self.navigator.getResult() + + # Path successfully completed + if result == TaskResult.SUCCEEDED: + remaining = remaining[len(segment):] + retries_left = self.max_retries + continue + + # Otherwise: FAILED or CANCELED + self.navigator.get_logger().warn( + f"Segment {seg_idx} did not reach the goal (status={result}).") + + if retries_left <= 0: + self.navigator.get_logger().error( + "Exceeded retry budget — aborting mission.") + return + + retries_left -= 1 + + # Re-insert the un-reached waypoints *and* a fresh “start-now” pose + try: + cur_pose = self._get_robot_pose() + except Exception as e: + self.navigator.get_logger().error(f"Cannot recover: {e}") + return + + self.navigator.get_logger().info( + "Re-planning from current position…") + + # Re-insert the current pose to start from it + remaining = [cur_pose] + segment + remaining[len(segment):] + + print("Waypoints completed successfully (segmented mode)") + + def _odom_cb(self, msg: Odometry) -> None: + """Cache the most recent odometry pose as a `PoseStamped`.""" + ps = PoseStamped() + ps.header = msg.header + ps.pose = msg.pose.pose + self._last_odom_pose = ps + + def _get_robot_pose(self) -> PoseStamped: + """ + Return the most recent pose from /fixposition/odometry_enu. + Blocks (spins) until a message arrives or the timeout elapses. + """ + deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( + seconds=self.odom_timeout_s) + + while self._last_odom_pose is None: + if self.navigator.get_clock().now() > deadline: + raise RuntimeError( + f"No Odometry on /fixposition/odometry_enu " + f"after {self.odom_timeout_s:.1f}s") + rclpy.spin_once(self.navigator, timeout_sec=0.05) + + return self._last_odom_pose def _publish_waypoints(self, poses): # Path message From f66bcf87f5556cf0479d0a85380b2512e33b5c50 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 26 Jun 2025 17:17:59 +0200 Subject: [PATCH 44/50] Testing new follower scripts --- .../config/dual_ekf_navsat_params.yaml | 4 +- src/nav2_tutorial/config/nav2_params.yaml | 22 +- .../launch/gps_waypoint_follower.launch.py | 34 +- src/nav2_tutorial/setup.py | 2 + .../src/logged_refill_follower.py | 359 ++++++++++ src/nav2_tutorial/src/periodic_logger.py | 22 +- src/nav2_tutorial/src/simple_follower.py | 676 ++++++++++++++++++ 7 files changed, 1089 insertions(+), 30 deletions(-) create mode 100644 src/nav2_tutorial/src/logged_refill_follower.py create mode 100644 src/nav2_tutorial/src/simple_follower.py diff --git a/src/nav2_tutorial/config/dual_ekf_navsat_params.yaml b/src/nav2_tutorial/config/dual_ekf_navsat_params.yaml index d51461c..8c9e115 100644 --- a/src/nav2_tutorial/config/dual_ekf_navsat_params.yaml +++ b/src/nav2_tutorial/config/dual_ekf_navsat_params.yaml @@ -10,7 +10,7 @@ ekf_filter_node_odom: map_frame: map odom_frame: odom - base_link_frame: base_link + base_link_frame: vrtk_link world_frame: odom odom0: /fixposition/odometry_enu @@ -52,7 +52,7 @@ ekf_filter_node_map: map_frame: map odom_frame: odom - base_link_frame: base_link + base_link_frame: vrtk_link world_frame: map odom0: /fixposition/odometry_enu diff --git a/src/nav2_tutorial/config/nav2_params.yaml b/src/nav2_tutorial/config/nav2_params.yaml index d1344d5..d3eeb69 100644 --- a/src/nav2_tutorial/config/nav2_params.yaml +++ b/src/nav2_tutorial/config/nav2_params.yaml @@ -2,11 +2,15 @@ bt_navigator: ros__parameters: global_frame: map - robot_base_frame: base_link + robot_base_frame: vrtk_link odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 - transform_tolerance: 0.3 + transform_tolerance: 0.2 + failure_recovery_enabled: true + failure_recovery_spin_behavior: true + failure_recovery_clear_costmap: true + failure_recovery_max_attempts: 2 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator::NavigateToPoseNavigator" @@ -31,19 +35,19 @@ controller_server: required_movement_radius: 0.2 movement_time_allowance: 20.0 goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" + plugin: "nav2_controller::StoppedGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 stateful: True FollowPath: plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" - desired_linear_vel: 1.3 + desired_linear_vel: 0.8 lookahead_dist: 1.0 min_lookahead_dist: 0.2 max_lookahead_dist: 2.0 lookahead_time: 1.5 rotate_to_heading_angular_vel: 1.0 - transform_tolerance: 0.5 + transform_tolerance: 0.2 use_velocity_scaled_lookahead_dist: false min_approach_linear_velocity: 0.05 approach_velocity_scaling_dist: 0.6 @@ -66,7 +70,7 @@ global_costmap: global_costmap: ros__parameters: global_frame: map - robot_base_frame: base_link + robot_base_frame: vrtk_link resolution: 0.05 rolling_window: true width: 50 @@ -81,7 +85,7 @@ local_costmap: local_costmap: ros__parameters: global_frame: odom - robot_base_frame: base_link + robot_base_frame: vrtk_link rolling_window: true width: 3 height: 3 @@ -138,8 +142,8 @@ behavior_server: plugin: "nav2_behaviors::AssistedTeleop" local_frame: odom global_frame: map - robot_base_frame: base_link - transform_tolerance: 0.5 + robot_base_frame: vrtk_link + transform_tolerance: 0.2 simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 diff --git a/src/nav2_tutorial/launch/gps_waypoint_follower.launch.py b/src/nav2_tutorial/launch/gps_waypoint_follower.launch.py index 766b5e7..b359729 100644 --- a/src/nav2_tutorial/launch/gps_waypoint_follower.launch.py +++ b/src/nav2_tutorial/launch/gps_waypoint_follower.launch.py @@ -41,9 +41,9 @@ def generate_launch_description(): PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_description.launch.py')) ) - robot_localization_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'dual_ekf_navsat.launch.py')) - ) + # robot_localization_cmd = IncludeLaunchDescription( + # PythonLaunchDescriptionSource(os.path.join(launch_dir, 'dual_ekf_navsat.launch.py')) + # ) navigation2_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, "navigation.launch.py")), @@ -74,23 +74,23 @@ def generate_launch_description(): # Add actions ld.add_action(scout_cmd) - ld.add_action(robot_localization_cmd) + # ld.add_action(robot_localization_cmd) ld.add_action(navigation2_cmd) ld.add_action(rviz_cmd) ld.add_action(mapviz_cmd) - # Add the set_datum node - datum_node = TimerAction( - period=5.0, - actions=[ - Node( - package="nav2_tutorial", - executable="set_datum", - name="set_datum", - output="screen" - ) - ] - ) - ld.add_action(datum_node) + # # Add the set_datum node + # datum_node = TimerAction( + # period=5.0, + # actions=[ + # Node( + # package="nav2_tutorial", + # executable="set_datum", + # name="set_datum", + # output="screen" + # ) + # ] + # ) + # ld.add_action(datum_node) return ld diff --git a/src/nav2_tutorial/setup.py b/src/nav2_tutorial/setup.py index 4b8630f..10c9544 100644 --- a/src/nav2_tutorial/setup.py +++ b/src/nav2_tutorial/setup.py @@ -28,6 +28,8 @@ 'console_scripts': [ 'logged_waypoint_follower = src.logged_waypoint_follower:main', 'logged_smooth_follower = src.logged_smooth_follower:main', + 'logged_refill_follower = src.logged_refill_follower:main', + 'simple_follower = src.simple_follower:main', 'interactive_waypoint_follower = src.interactive_waypoint_follower:main', 'gps_waypoint_logger = src.gps_waypoint_logger:main', 'terminal_logger = src.terminal_logger:main', diff --git a/src/nav2_tutorial/src/logged_refill_follower.py b/src/nav2_tutorial/src/logged_refill_follower.py new file mode 100644 index 0000000..dd264ea --- /dev/null +++ b/src/nav2_tutorial/src/logged_refill_follower.py @@ -0,0 +1,359 @@ +import os +import sys +import time +import math +import yaml +import glob +import argparse +from datetime import datetime + +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 + +from geometry_msgs.msg import PoseStamped +from robot_localization.srv import FromLL + +# Visualization +from nav_msgs.msg import Path +from visualization_msgs.msg import Marker, MarkerArray +from rclpy.qos import QoSProfile, DurabilityPolicy +MARKER_VIZ = True + +import tf2_ros # ← NEW +from geometry_msgs.msg import TransformStamped + + +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"] + 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 + + +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 + self.chunk_len = 60 # poses per NavigateThroughPoses goal + self.overlap = 5 # poses to keep ahead when we “refill” + self.refill_dist = 2.00 + + # FromLL client (WGS-84 → map frame) – create it on the navigator node + self._fromll_cli = self.navigator.create_client(FromLL, "/fromLL") + self.navigator.get_logger().info("Waiting for /fromLL service…") + if not self._fromll_cli.wait_for_service(timeout_sec=10.0): + raise RuntimeError("robot_localization /fromLL service is not available") + + self.tf_buffer = tf2_ros.Buffer(cache_time=rclpy.duration.Duration(seconds=2.0)) + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.navigator, spin_thread=False) + + if MARKER_VIZ: + latched_qos = QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + + # Path for visualizing waypoints + 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) + + # Convert (lat, lon, yaw) to PoseStamped in the map frame + def _latlon_to_pose(self, lat: float, lon: float, yaw: float) -> PoseStamped: + req = FromLL.Request() + req.ll_point.latitude = lat + req.ll_point.longitude = lon + req.ll_point.altitude = 0.0 + + future = self._fromll_cli.call_async(req) + rclpy.spin_until_future_complete(self.navigator, future) + map_pt = future.result().map_point + + pose = PoseStamped() + pose.header.frame_id = "map" + pose.header.stamp = rclpy.time.Time().to_msg() + pose.pose.position.x = map_pt.x + pose.pose.position.y = map_pt.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 + + def _latlon_list_to_poses(self, raw_wps) -> list[PoseStamped]: + """Convert the YAML list directly to PoseStamped list.""" + poses = [] + for wp in raw_wps: + yaw = (self.wp_parser._reverse_yaw(wp["yaw"]) + if self.reverse else wp["yaw"]) + poses.append(self._latlon_to_pose( + wp["latitude"], wp["longitude"], yaw)) + return poses + + @staticmethod + def _path_remaining(robot_xy, slice_poses) -> float: + """Return length [m] from robot's nearest pose to end of slice.""" + rx, ry = robot_xy + # 1) closest pose index + idx = min( + range(len(slice_poses)), + key=lambda i: (slice_poses[i].pose.position.x - rx)**2 + + (slice_poses[i].pose.position.y - ry)**2 + ) + # 2) accumulate segment lengths to end + rem = 0.0 + for a, b in zip(slice_poses[idx:-1], slice_poses[idx+1:]): + dx = b.pose.position.x - a.pose.position.x + dy = b.pose.position.y - a.pose.position.y + rem += math.hypot(dx, dy) + return rem + + def start_ntp(self): + self.navigator.waitUntilNav2Active(localizer="robot_localization") + + raw = (self.wp_parser.wps_dict["waypoints"][::-1] + if self.reverse else self.wp_parser.wps_dict["waypoints"]) + poses = self._latlon_list_to_poses(raw) + + if MARKER_VIZ: + self._publish_waypoints(poses) + + idx, total = 0, len(poses) + self.navigator.goThroughPoses(poses[idx: idx + self.chunk_len]) + + # -------- streaming loop using path-length remaining ------------ + avg_spacing = 0.08 # metres between recorded points + refill_len = self.overlap * avg_spacing # e.g. 10 * 0.08 = 0.8 m + + current_slice = poses[idx : idx + self.chunk_len] + + while rclpy.ok(): + rclpy.spin_once(self.navigator, timeout_sec=0.05) + + # --- if current goal is done but slices remain, queue the next one + if self.navigator.isTaskComplete() and idx < total: + idx += self.chunk_len - self.overlap + current_slice = poses[idx : idx + self.chunk_len] + self.navigator.goThroughPoses(current_slice) + continue # wait for new TF / feedback + + try: + tf = self.tf_buffer.lookup_transform( + 'map', 'base_link', rclpy.time.Time()) + except (tf2_ros.LookupException, + tf2_ros.ExtrapolationException): + continue # wait for TF to become available + + robot_xy = (tf.transform.translation.x, + tf.transform.translation.y) + + rem_len = self._path_remaining(robot_xy, current_slice) + print(f"Len remaining: {rem_len:.2f} m") + + # queue next slice when only 'overlap' distance is left + if rem_len < refill_len: + print("Idx: ", idx, "of", total) + idx += self.chunk_len - self.overlap + if idx >= total: # all slices queued + break + current_slice = poses[idx : idx + self.chunk_len] + print("Current slice: ", current_slice) + self.navigator.goThroughPoses(current_slice) + + if self.navigator.isTaskComplete() and idx >= total: + break + + while not self.navigator.isTaskComplete(): + rclpy.spin_once(self.navigator, timeout_sec=0.1) + + print("Waypoints completed successfully (streaming mode)") + + def _publish_waypoints(self, poses): + # Path message + path = Path() + path.header.frame_id = "map" + path.header.stamp = rclpy.time.Time().to_msg() + path.poses = poses + self.waypoint_path_pub.publish(path) + + # Optional coloured spheres + markers = MarkerArray() + for i, p in enumerate(poses): + m = Marker() + m.header = path.header + m.ns = "wps" + m.id = i + m.type = Marker.SPHERE + m.action = Marker.ADD + m.pose = p.pose + m.scale.x = m.scale.y = m.scale.z = 0.05 + m.color.r, m.color.g, m.color.b, m.color.a = (0.1, 0.9, 0.1, 1.0) + markers.markers.append(m) + self.waypoint_marker_pub.publish(markers) + + +def main(argv: list[str] | None = None): + rclpy.init(args=argv) + + parser = argparse.ArgumentParser(description="Follow GPS waypoints from a YAML file") + parser.add_argument("yaml_file", nargs="?", default=None, help="Path to the YAML waypoints file") + parser.add_argument("--last", action="store_true", help="Load the most recent waypoints file in the config directory") + parser.add_argument("--reverse", action="store_true", help="Follow the trajectory in reverse order (adds 180° to yaw)") + args = parser.parse_args() + + try: + yaml_file_path = _select_yaml(args) + except FileNotFoundError as e: + print(f"[WARN] {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_ntp() + except Exception as exc: + print(f"[ERROR] {exc}", file=sys.stderr) + sys.exit(1) + + +if __name__ == "__main__": + main() diff --git a/src/nav2_tutorial/src/periodic_logger.py b/src/nav2_tutorial/src/periodic_logger.py index 76f6df5..801458e 100644 --- a/src/nav2_tutorial/src/periodic_logger.py +++ b/src/nav2_tutorial/src/periodic_logger.py @@ -23,6 +23,7 @@ import sys import termios import tty +import math from datetime import datetime import rclpy @@ -47,6 +48,10 @@ def __init__(self, logging_file_path: str, interval: float): self.last_gps: NavSatFix | None = None self.last_yaw: float = 0.0 self.saved_points: int = 0 + + self._DIST_TOL_M = 0.01 # Save only if moved ≥ 1 cm + self._DEG2M_LAT = 110_574.0 # Rough conversion at mid-latitudes + self._last_saved_fix : NavSatFix | None = None # ------------------------------------------------------------------ # Topic subscriptions @@ -88,12 +93,25 @@ def yaw_callback(self, msg: Odometry) -> None: # ------------------------------------------------------------------ def _log_waypoint(self) -> None: """Append the current pose to YAML.""" - if self.last_gps is None: + fix = self.last_gps + if fix is None: return # wait for first fix - + if self.saved_points == 0: # First time we succeed in logging – announce start of collection. self.get_logger().info("Started collecting waypoints …") + + if self._last_saved_fix is not None: + dlat = (fix.latitude - self._last_saved_fix.latitude ) * self._DEG2M_LAT + # convert longitude degrees to metres at current latitude + lon_scale = 111_320.0 * math.cos(math.radians(fix.latitude)) + dlon = (fix.longitude - self._last_saved_fix.longitude) * lon_scale + dist = math.hypot(dlat, dlon) + if dist < self._DIST_TOL_M: + return # too close → skip this sample + + # Passed the distance test → remember it for next time + self._last_saved_fix = fix waypoint = { 'latitude': self.last_gps.latitude, diff --git a/src/nav2_tutorial/src/simple_follower.py b/src/nav2_tutorial/src/simple_follower.py new file mode 100644 index 0000000..3a50996 --- /dev/null +++ b/src/nav2_tutorial/src/simple_follower.py @@ -0,0 +1,676 @@ +import os +import sys +import time +import math +import yaml +import glob +import argparse +from datetime import datetime + +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 CRS, Transformer + +# 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 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 + + +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 + + +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 + + # Odometry topic with ENU pose in map frame + self._last_odom_pose: PoseStamped | None = None + self.navigator.create_subscription(Odometry, "/fixposition/odometry_enu", self._odom_cb, 10) + + # Get datum + _base_lat, _base_lon, _base_alt = self._get_datum() + + # Transform from WGS-84 (lat, lon, h) to ENU (e, n, u) in the map frame + enu_pipeline = ( + "+proj=pipeline " + "+step +proj=unitconvert +xy_in=deg +xy_out=rad " + "+step +proj=axisswap +order=2,1,3 " # lon,lat order 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.max_window = 50.0 # meters (square side length) + self.safety_margin = 1.0 # meters (keep a little inside the edge) + self.max_seg_len = 100.0 # meters of path length per sub-goal + self.odom_timeout_s = 2.0 # wait this long for first /fixposition msg + self.max_retries = 1 # re-attempt each failed segment once + self._last_goal_uuid: UUID | None = None + self._retries_left = 1 + + # Sliding-window parameters + self.seg_len_max = 6.0 # m – length of window Nav2 sees + self.advance_tol = 1.60 # m – when to roll the window forward + self.overlap_tol = 0.30 # m – keep poses this close as overlap + self._fp_client = None + + # Statistics + self._total_wps = 0 # filled in start_ntp() + self._visited_wps = 0 # will be advanced per segment + + if MARKER_VIZ: + latched_qos = QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + + # Path for visualizing waypoints + 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, "/gps_full_traj", latched_qos) + self.full_traj_mrk_pub = self.navigator.create_publisher( + MarkerArray, "/gps_full_traj_markers", latched_qos) + + def _get_datum(self, topic: str = "/fixposition/datum", + timeout_s: float = 3.0) -> tuple[float, float, float]: + """ + Block until a NavSatFix is received on *topic* (max *timeout_s*). + Returns (lat [deg], lon [deg], alt [m]). Raises RuntimeError on timeout. + """ + datum: list[float] | None = None + + def _cb(msg: NavSatFix): + nonlocal datum + datum = [msg.latitude, msg.longitude, msg.altitude] + + sub = self.navigator.create_subscription( + NavSatFix, topic, _cb, qos_profile_sensor_data) + + deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( + seconds=timeout_s) + + 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 + + def start_ntp(self): + 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"]) + ] + + # 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() + window = self._make_window(robot, remaining) + new_wps = len(window) - 1 # skip robot pose + + self._log_segment(window, + overlap_cnt=0, + seg_idx =seg_idx, + new_wps =new_wps) + self._publish_waypoints(window, seg_idx) + self.navigator.goThroughPoses(window) + + self._visited_wps += new_wps # update tally *after* queueing + remaining = remaining[new_wps:] + seg_idx += 1 + + # ------------------------------------------------------------ + # 3) Continuous streaming loop + # ------------------------------------------------------------ + while rclpy.ok(): + rclpy.spin_once(self.navigator, timeout_sec=0.05) + + 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)) + + #----------------------------------------------------------- + # 1) *Stale* result from the goal we just pre-empted + #----------------------------------------------------------- + if not result_is_current: + continue # ignore and keep streaming + + #----------------------------------------------------------- + # 2) Current goal SUCCEEDED + #----------------------------------------------------------- + if status == TaskResult.SUCCEEDED: + if not remaining: # trajectory finished + self.navigator.get_logger().info("Trajectory finished") + break + else: + continue # hand-off to next window + + #----------------------------------------------------------- + # 3) Current goal FAILED → one retry + #----------------------------------------------------------- + if status == TaskResult.FAILED: + if self._retries_left > 0: + self._retries_left -= 1 + + robot = self._get_robot_pose() + + # 1. Walk forward inside the failed window until you + # reach the first pose that is *ahead* of the robot. + remaining_in_window = [] + robot_found_tail = False + for p in window: + if robot_found_tail or self._dist(robot, p) > 1e-3: + robot_found_tail = True + remaining_in_window.append(p) + + # 2. If nothing left → give up (unlikely but safe-guard) + if not remaining_in_window: + self.navigator.get_logger().error( + "Retry requested but no poses remain in failed window") + break + + # 3. New window = [robot_pose] + the untouched tail + retry_window = [robot] + remaining_in_window + + self.navigator.get_logger().warn( + f"Segment {seg_idx+1} failed – retrying {len(remaining_in_window)} poses") + self._publish_waypoints(retry_window, seg_idx) # same seg-idx: overwrite colour + self.navigator.goThroughPoses(retry_window) + continue # <-- DO NOT modify `remaining` or `_visited_wps` here + else: + self.navigator.get_logger().error("NavigateThroughPoses failed twice – aborting") + break + + # 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 + + overlap = [] + for p in reversed(window): + if (self._dist(p, tail) <= self.overlap_tol and # close to tail + self._dist(robot, p) <= dist_rt + 1e-6): # not behind robot + overlap.append(p) + else: + break + overlap.reverse() + + if not remaining: + continue # path finished; let Nav2 coast to the end + + # -------- build the forward part of the new window --------------- + forward = self._make_window(tail, remaining)[1:] # skip dup ‘tail’ + new_window = overlap + forward + + # drop from ‘remaining’ exactly the poses we just attached + remaining = remaining[len(forward):] + + # log stats **before** we cancel / send + self._log_segment(new_window, + overlap_cnt=len(overlap), + seg_idx =seg_idx, + new_wps =len(forward)) + + # # Pre-empt the running action + # self.navigator.cancelTask() + # # ─── Wait until the cancellation is fully processed ─── + # cancel_deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( + # seconds=3.0) # plenty – normally < 0.3 s + # while not self.navigator.isTaskComplete(): + # if self.navigator.get_clock().now() > cancel_deadline: + # self.navigator.get_logger().error("Cancel timeout – aborting") + # return + # rclpy.spin_once(self.navigator, timeout_sec=0.05) + + # # ───────────────────────────────────────────────────────────── + # # Extra guard: wait until internal follow_path is cancelled + # # ───────────────────────────────────────────────────────────── + # if self._fp_client is None: + # self._fp_client = ActionClient(self.navigator, FollowPath, "follow_path") + # if not self._fp_client.wait_for_server(timeout_sec=2.0): + # self.navigator.get_logger().warn("follow_path server not available") + + # fp_deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( + # seconds=3.0) + # while True: + # goal_handles = self._fp_client._goal_handles # internal list + # if not goal_handles: + # break # nothing active + # status = goal_handles[0].status + # if status in (GoalStatus.STATUS_SUCCEEDED, GoalStatus.STATUS_CANCELED): + # break + # if self.navigator.get_clock().now() > fp_deadline: + # self.navigator.get_logger().error("follow_path cancel timeout") + # return + # rclpy.spin_once(self.navigator, timeout_sec=0.05) + + self._publish_waypoints(new_window, seg_idx) + self.navigator.goThroughPoses(new_window) + self._visited_wps += len(forward) # advance tally + seg_idx += 1 + window = new_window + + def _odom_cb(self, msg: Odometry) -> None: + """Cache the most recent odometry pose as a `PoseStamped`.""" + ps = PoseStamped() + ps.header = msg.header + ps.pose = msg.pose.pose + self._last_odom_pose = ps + + def _get_robot_pose(self) -> PoseStamped: + """ + Return the most recent pose from /fixposition/odometry_enu. + Blocks (spins) until a message arrives or the timeout elapses. + """ + deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( + seconds=self.odom_timeout_s) + + while self._last_odom_pose is None: + if self.navigator.get_clock().now() > deadline: + raise RuntimeError( + f"No Odometry on /fixposition/odometry_enu " + f"after {self.odom_timeout_s:.1f}s") + rclpy.spin_once(self.navigator, timeout_sec=0.05) + + return self._last_odom_pose + + def _publish_waypoints(self, poses, seg_idx: int): + # Path message + path = Path() + path.header.frame_id = "map" + path.header.stamp = rclpy.time.Time().to_msg() + path.poses = poses + + # RViz colours Paths per-topic, not per-message → we publish a Marker + # LINE_STRIP instead (same header) so each segment gets its own hue. + line = Marker() + line.header = path.header + line.ns = "seg_line" + line.id = seg_idx # unique id per segment + line.type = Marker.LINE_STRIP + line.action = Marker.ADD + line.pose.orientation.w = 1.0 # identity + line.scale.x = 0.03 # line width + hue = (seg_idx * 0.18) % 1.0 # golden-ratio hop around the circle + r, g, b = _hsv_to_rgb(hue, 1.0, 1.0) + line.color.r, line.color.g, line.color.b, line.color.a = r, g, b, 1.0 + # copy points + for p in poses: + line.points.append(p.pose.position) + + self.waypoint_path_pub.publish(path) # still publish for tooling + self.waypoint_marker_pub.publish(MarkerArray(markers=[line])) + + # Optional coloured spheres + markers = MarkerArray() + for i, p in enumerate(poses): + m = Marker() + m.header = path.header + m.ns = "wps" + m.id = seg_idx*1000 + i + m.type = Marker.SPHERE + m.action = Marker.ADD + m.pose = p.pose + m.scale.x = m.scale.y = m.scale.z = 0.05 + m.color.r, m.color.g, m.color.b, m.color.a = r, g, b, 1.0 + markers.markers.append(m) + self.waypoint_marker_pub.publish(markers) + + def _dist(self, a: PoseStamped, b: PoseStamped) -> float: + 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, + new_wps: int) -> None: + seg_len = self._segment_length(window) + num_wps = len(window) + progress = max(self._visited_wps - overlap_cnt, 0) + + self.navigator.get_logger().info( + f"Segment {seg_idx+1} length: {seg_len:0.2f} m, " + f"Num. waypoints: {num_wps}, " + f"Overlap: {overlap_cnt}, " + f"Progress: {progress}/{self._total_wps}") + + def _publish_full_traj(self, poses: list[PoseStamped]): + """Publish the whole trajectory as a latched Path + MarkerArray.""" + path = Path() + path.header.frame_id = "map" + path.header.stamp = rclpy.time.Time().to_msg() + path.poses = poses + self.full_traj_pub.publish(path) + + # one violet line-strip + tiny white spheres at every waypoint + line = Marker() + line.header = path.header + line.ns = "full_traj" + line.id = 0 + line.type = Marker.LINE_STRIP + line.action = Marker.ADD + line.pose.orientation.w = 1.0 + line.scale.x = 0.04 + line.color.r, line.color.g, line.color.b, line.color.a = 0.7, 0.3, 1.0, 1.0 + line.points = [p.pose.position for p in poses] + + dots = MarkerArray() + for i, p in enumerate(poses): + m = Marker() + m.header = path.header + m.ns, m.id = "full_traj_pts", i + m.type, m.action = Marker.SPHERE, Marker.ADD + m.pose = p.pose + m.scale.x = m.scale.y = m.scale.z = 0.05 + m.color.r = m.color.g = m.color.b = 1.0 + m.color.a = 1.0 + dots.markers.append(m) + + self.full_traj_mrk_pub.publish(MarkerArray( + markers=[line] + dots.markers)) + + +def main(argv: list[str] | None = None): + 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)") + args = parser.parse_args() + + try: + yaml_file_path = _select_yaml(args) + except FileNotFoundError as e: + print(f"[WARN] {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_ntp() + except Exception as exc: + print(f"[ERROR] {exc}", file=sys.stderr) + sys.exit(1) + + +if __name__ == "__main__": + main() From 578ba7ab0289f2ab9520548f2451286cc78dacc5 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 26 Jun 2025 18:21:48 +0200 Subject: [PATCH 45/50] Improved controller --- src/nav2_tutorial/src/simple_follower.py | 97 +++++++++++++++--------- 1 file changed, 62 insertions(+), 35 deletions(-) diff --git a/src/nav2_tutorial/src/simple_follower.py b/src/nav2_tutorial/src/simple_follower.py index 3a50996..b4329bd 100644 --- a/src/nav2_tutorial/src/simple_follower.py +++ b/src/nav2_tutorial/src/simple_follower.py @@ -246,9 +246,10 @@ def __init__(self, wps_file_path: str, reverse: bool = False): self._retries_left = 1 # 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 = 1.60 # m – when to roll the window forward - self.overlap_tol = 0.30 # m – keep poses this close as overlap + self.advance_tol = 2.60 # m – when to roll the window forward + self.overlap_tol = 1.00 # m – keep poses this close as overlap self._fp_client = None # Statistics @@ -343,6 +344,25 @@ def start_ntp(self): # 2) Prime the first window # ------------------------------------------------------------ robot = self._get_robot_pose() + + # Fast-forward to the first pose that is > 1 m away + start_skip_tol = 1.0 # [m] change as needed + while remaining and self._dist(robot, remaining[0]) < start_skip_tol: + remaining.pop(0) + self._visited_wps += 1 # we “virtually” visited it + + if self._visited_wps: + self.navigator.get_logger().info( + f"Pruned waypoints! Starting from {self._visited_wps} out of {self._total_wps} waypoints…") + + # if everything was within the tolerance, we’re already done! + if not remaining: + self.navigator.get_logger().info("Trajectory already completed.") + return + + window = self._make_window(robot, remaining) + + window = self._make_window(robot, remaining) new_wps = len(window) - 1 # skip robot pose @@ -369,63 +389,70 @@ def start_ntp(self): 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 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)) - #----------------------------------------------------------- - # 1) *Stale* result from the goal we just pre-empted - #----------------------------------------------------------- + # *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 + #----------------------------------------------------------- - # 2) Current goal SUCCEEDED + # 1) SUCCEEDED → accept only if really at tail *and* + # nothing remains in the global list #----------------------------------------------------------- if status == TaskResult.SUCCEEDED: - if not remaining: # trajectory finished + at_tail = (self._dist(robot, window[-1]) <= self.advance_tol) + if at_tail and not remaining: self.navigator.get_logger().info("Trajectory finished") - break + break # all done else: - continue # hand-off to next window + # 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 + 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` #----------------------------------------------------------- - # 3) Current goal FAILED → one retry + # 2) FAILED or CANCELLED → one retry #----------------------------------------------------------- - if status == TaskResult.FAILED: + if status in (TaskResult.FAILED, TaskResult.CANCELED): if self._retries_left > 0: self._retries_left -= 1 - - robot = self._get_robot_pose() - - # 1. Walk forward inside the failed window until you - # reach the first pose that is *ahead* of the robot. - remaining_in_window = [] - robot_found_tail = False - for p in window: - if robot_found_tail or self._dist(robot, p) > 1e-3: - robot_found_tail = True - remaining_in_window.append(p) - - # 2. If nothing left → give up (unlikely but safe-guard) - if not remaining_in_window: + unreached = [p for p in window + if self._dist(robot, p) > self.advance_tol] + if not unreached: self.navigator.get_logger().error( - "Retry requested but no poses remain in failed window") + "Retry requested but no poses remain in window") break - - # 3. New window = [robot_pose] + the untouched tail - retry_window = [robot] + remaining_in_window - + retry_window = [robot] + unreached self.navigator.get_logger().warn( - f"Segment {seg_idx+1} failed – retrying {len(remaining_in_window)} poses") - self._publish_waypoints(retry_window, seg_idx) # same seg-idx: overwrite colour + 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) - continue # <-- DO NOT modify `remaining` or `_visited_wps` here + window = retry_window + continue else: - self.navigator.get_logger().error("NavigateThroughPoses failed twice – aborting") + self.navigator.get_logger().error( + "Segment failed twice - aborting") break # Live pose From 10cc8c146b2e0ee8bb592435356186509180fcde Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Mon, 30 Jun 2025 16:00:58 +0200 Subject: [PATCH 46/50] Improved follower --- src/nav2_tutorial/src/simple_follower.py | 74 +++++++++++++----------- 1 file changed, 40 insertions(+), 34 deletions(-) diff --git a/src/nav2_tutorial/src/simple_follower.py b/src/nav2_tutorial/src/simple_follower.py index b4329bd..139f853 100644 --- a/src/nav2_tutorial/src/simple_follower.py +++ b/src/nav2_tutorial/src/simple_follower.py @@ -1,11 +1,9 @@ import os import sys -import time import math import yaml import glob import argparse -from datetime import datetime import rclpy from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult @@ -15,7 +13,7 @@ from rclpy.action import ActionClient from rclpy.qos import qos_profile_sensor_data from uuid import UUID -from pyproj import CRS, Transformer +from pyproj import Transformer # ROS messages from sensor_msgs.msg import NavSatFix @@ -237,19 +235,17 @@ def __init__(self, wps_file_path: str, reverse: bool = False): self._tf_llh2enu = Transformer.from_pipeline(enu_pipeline) # Parameters for the NavigateThroughPoses task - self.max_window = 50.0 # meters (square side length) - self.safety_margin = 1.0 # meters (keep a little inside the edge) - self.max_seg_len = 100.0 # meters of path length per sub-goal - self.odom_timeout_s = 2.0 # wait this long for first /fixposition msg - self.max_retries = 1 # re-attempt each failed segment once + self.odom_timeout_s = 2.0 # wait this long for first /fixposition msg + self.max_retries = 1 # re-attempt each failed segment once + self._retries_left = self.max_retries self._last_goal_uuid: UUID | None = None - self._retries_left = 1 # 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.60 # m – when to roll the window forward - self.overlap_tol = 1.00 # m – keep poses this close as overlap + self.seg_len_max = 6.0 # m – length of window Nav2 sees + self.advance_tol = 2.6 # m – when to roll the window forward + self.overlap_tol = 1.0 # m – keep poses this close as overlap + self.start_seg_tol = 3.0 # m – max distance we “snap” to the path self._fp_client = None # Statistics @@ -345,15 +341,18 @@ def start_ntp(self): # ------------------------------------------------------------ robot = self._get_robot_pose() - # Fast-forward to the first pose that is > 1 m away - start_skip_tol = 1.0 # [m] change as needed - while remaining and self._dist(robot, remaining[0]) < start_skip_tol: - remaining.pop(0) - self._visited_wps += 1 # we “virtually” visited it - - if self._visited_wps: + # Fast-forward to the first pose within start_seg_tol + closest_idx = self._closest_wp_index(robot, remaining) + if self._dist(robot, remaining[closest_idx]) > self.start_seg_tol: + closest_idx = 0 # fall back to the first wp + + if closest_idx: + self._visited_wps += closest_idx # statistics + skipped = closest_idx + remaining = remaining[closest_idx:] self.navigator.get_logger().info( - f"Pruned waypoints! Starting from {self._visited_wps} out of {self._total_wps} waypoints…") + f"Skipped {skipped} waypoint(s); " + f"starting with wp #{skipped+1}/{self._total_wps}") # if everything was within the tolerance, we’re already done! if not remaining: @@ -361,19 +360,19 @@ def start_ntp(self): return window = self._make_window(robot, remaining) - - - window = self._make_window(robot, remaining) - new_wps = len(window) - 1 # skip robot pose + 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, - new_wps =new_wps) + seg_idx =seg_idx) self._publish_waypoints(window, seg_idx) self.navigator.goThroughPoses(window) - self._visited_wps += new_wps # update tally *after* queueing remaining = remaining[new_wps:] seg_idx += 1 @@ -488,10 +487,10 @@ def start_ntp(self): 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, - new_wps =len(forward)) + seg_idx =seg_idx) # # Pre-empt the running action # self.navigator.cancelTask() @@ -528,7 +527,7 @@ def start_ntp(self): self._publish_waypoints(new_window, seg_idx) self.navigator.goThroughPoses(new_window) - self._visited_wps += len(forward) # advance tally + self._retries_left = self.max_retries seg_idx += 1 window = new_window @@ -556,7 +555,7 @@ def _get_robot_pose(self) -> PoseStamped: return self._last_odom_pose - def _publish_waypoints(self, poses, seg_idx: int): + def _publish_waypoints(self, poses: list[PoseStamped], seg_idx: int): # Path message path = Path() path.header.frame_id = "map" @@ -625,11 +624,10 @@ def _segment_length(self, poses: list[PoseStamped]) -> float: def _log_segment(self, window, *, overlap_cnt: int, - seg_idx: int, - new_wps: int) -> None: + seg_idx: int) -> None: seg_len = self._segment_length(window) num_wps = len(window) - progress = max(self._visited_wps - overlap_cnt, 0) + progress = self._visited_wps self.navigator.get_logger().info( f"Segment {seg_idx+1} length: {seg_len:0.2f} m, " @@ -671,6 +669,14 @@ 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): From d92fb7d9caeb701fcd223479062a6f70965c859e Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Mon, 30 Jun 2025 16:36:50 +0200 Subject: [PATCH 47/50] Updated loggers --- README.md | 32 +- src/nav2_tutorial/config/nav2_params.yaml | 2 +- src/nav2_tutorial/setup.py | 22 +- src/nav2_tutorial/src/gps_waypoint_logger.py | 135 ------- .../src/logged_refill_follower.py | 359 ------------------ src/nav2_tutorial/src/loggers/__init__.py | 0 .../src/{ => loggers}/periodic_logger.py | 0 .../src/{ => loggers}/terminal_logger.py | 51 ++- 8 files changed, 56 insertions(+), 545 deletions(-) delete mode 100644 src/nav2_tutorial/src/gps_waypoint_logger.py delete mode 100644 src/nav2_tutorial/src/logged_refill_follower.py create mode 100644 src/nav2_tutorial/src/loggers/__init__.py rename src/nav2_tutorial/src/{ => loggers}/periodic_logger.py (100%) rename src/nav2_tutorial/src/{ => loggers}/terminal_logger.py (75%) diff --git a/README.md b/README.md index 4c2d8e4..3e024f1 100644 --- a/README.md +++ b/README.md @@ -137,6 +137,23 @@ source /opt/ros/jazzy/setup.bash ros2 run teleop_twist_keyboard teleop_twist_keyboard ``` +## Step 5: Log GNSS waypoints +The user must populate the predefined gps_waypoints.yaml file with the waypoints the robot has to follow. The user can either provide the waypoints manually or use the provided waypoint logging tool as shown below: +``` +ros2 run nav2_tutorial gps_waypoint_logger +``` + +If your terminal does not support X11 forwarding, you can use the following script: +``` +ros2 run nav2_tutorial terminal_logger +``` + +Lastly, you can use a periodic logger to have denser points: +``` +ros2 run nav2_tutorial periodic_logger +``` + + ## Step 6: Start GPS Waypoint Following Launch the ROS nodes in the following order: ``` @@ -157,22 +174,7 @@ Finally, start the navigation. There are two types waypoint following methods. W ros2 run nav2_tutorial interactive_waypoint_follower ``` -* Logged GPS Waypoint Follower - -First, the user must populate the predefined gps_waypoints.yaml file with the waypoints the robot has to follow. The user can either provide the waypoints manually or use the provided waypoint logging tool as shown below: -``` -ros2 run nav2_tutorial gps_waypoint_logger -``` - -If your terminal does not support X11 forwarding, you can use the following script: -``` -ros2 run nav2_tutorial terminal_logger -``` -Lastly, you can use a periodic logger to have denser points: -``` -ros2 run nav2_tutorial periodic_logger -``` Then, call the logged_waypoint_follower script to make the robot follow the logged waypoints: ``` diff --git a/src/nav2_tutorial/config/nav2_params.yaml b/src/nav2_tutorial/config/nav2_params.yaml index d3eeb69..f90e791 100644 --- a/src/nav2_tutorial/config/nav2_params.yaml +++ b/src/nav2_tutorial/config/nav2_params.yaml @@ -41,7 +41,7 @@ controller_server: stateful: True FollowPath: plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" - desired_linear_vel: 0.8 + desired_linear_vel: 1.3 lookahead_dist: 1.0 min_lookahead_dist: 0.2 max_lookahead_dist: 2.0 diff --git a/src/nav2_tutorial/setup.py b/src/nav2_tutorial/setup.py index 10c9544..7a26f1f 100644 --- a/src/nav2_tutorial/setup.py +++ b/src/nav2_tutorial/setup.py @@ -1,6 +1,6 @@ -from setuptools import find_packages, setup import os from glob import glob +from setuptools import find_packages, setup package_name = 'nav2_tutorial' @@ -21,20 +21,24 @@ zip_safe=True, maintainer='Facundo Garcia', maintainer_email='facundo.garcia@fixposition.com', - description='GPS Waypoints Follower for AgileX Scout Mini', + description='GPS Waypoint Follower for Vision-RTK2', license='MIT', tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'logged_waypoint_follower = src.logged_waypoint_follower:main', - 'logged_smooth_follower = src.logged_smooth_follower:main', - 'logged_refill_follower = src.logged_refill_follower:main', + # GPS Waypoint Follower 'simple_follower = src.simple_follower:main', + 'logged_smooth_follower = src.logged_smooth_follower:main', + 'logged_waypoint_follower = src.logged_waypoint_follower:main', 'interactive_waypoint_follower = src.interactive_waypoint_follower:main', - 'gps_waypoint_logger = src.gps_waypoint_logger:main', - 'terminal_logger = src.terminal_logger:main', - 'periodic_logger = src.periodic_logger:main', - 'set_datum = src.set_datum:main' + + # GPS Waypoint Logger + 'terminal_logger = src.loggers.terminal_logger:main', + 'periodic_logger = src.loggers.periodic_logger:main', + + # Datum Setter + 'set_datum = src.set_datum:main', + 'set_datum_from_tf = src.set_datum_from_tf:main', ], }, ) diff --git a/src/nav2_tutorial/src/gps_waypoint_logger.py b/src/nav2_tutorial/src/gps_waypoint_logger.py deleted file mode 100644 index 5feef4f..0000000 --- a/src/nav2_tutorial/src/gps_waypoint_logger.py +++ /dev/null @@ -1,135 +0,0 @@ -import os -import sys -import yaml - -import rclpy -from rclpy.node import Node -from nav_msgs.msg import Odometry -from sensor_msgs.msg import NavSatFix - -import tkinter as tk -from tkinter import messagebox - -from src.utils.gps_utils import euler_from_quaternion - - -class GpsGuiLogger(tk.Tk, Node): - """ - ROS2 node to log GPS waypoints to a file - """ - - def __init__(self, logging_file_path): - tk.Tk.__init__(self) - Node.__init__(self, 'gps_waypoint_logger') - self.title("GPS Waypoint Logger") - - self.logging_file_path = logging_file_path - - self.gps_pose_label = tk.Label(self, text="Current Coordinates:") - self.gps_pose_label.pack() - self.gps_pose_textbox = tk.Label(self, text="", width=45) - self.gps_pose_textbox.pack() - - self.log_gps_wp_button = tk.Button(self, text="Log GPS Waypoint", - command=self.log_waypoint) - self.log_gps_wp_button.pack() - - self.gps_subscription = self.create_subscription( - NavSatFix, - '/fixposition/odometry_llh', - self.gps_callback, - 1 - ) - self.last_gps_position = NavSatFix() - - self.yaw_subscription = self.create_subscription( - Odometry, - '/fixposition/odometry_enu', - self.yaw_callback, - 1 - ) - self.last_heading = 0.0 - - def gps_callback(self, msg: NavSatFix): - """ - Callback to store the last GPS pose - """ - self.last_gps_position = msg - self.updateTextBox() - - def yaw_callback(self, msg: Odometry): - """ - Callback to store the last heading - """ - self.last_heading = euler_from_quaternion(msg.pose.pose.orientation)[2] - self.updateTextBox() - - def updateTextBox(self): - """ - Function to update the GUI with the last coordinates - """ - self.gps_pose_textbox.config( - text=f"Lat: {self.last_gps_position.latitude:.6f}, Lon: {self.last_gps_position.longitude:.6f}, yaw: {self.last_heading:.2f} rad") - - def log_waypoint(self): - """ - Function to save a new waypoint to a file - """ - # Read existing waypoints - try: - with open(self.logging_file_path, 'r') as yaml_file: - existing_data = yaml.safe_load(yaml_file) - # If the file does not exist, create with the new wps - except FileNotFoundError: - existing_data = {"waypoints": []} - # If other exception, raise the warning - except Exception as ex: - messagebox.showerror( - "Error", f"Error logging position: {str(ex)}") - return - - # Build new waypoint object - data = { - "latitude": self.last_gps_position.latitude, - "longitude": self.last_gps_position.longitude, - "yaw": self.last_heading - } - existing_data["waypoints"].append(data) - - # Write updated waypoints - try: - with open(self.logging_file_path, 'w') as yaml_file: - yaml.dump(existing_data, yaml_file, default_flow_style=False) - except Exception as ex: - messagebox.showerror( - "Error", f"Error logging position: {str(ex)}") - return - - messagebox.showinfo("Info", "Waypoint logged succesfully") - - -def main(args=None): - rclpy.init(args=args) - - # Allow to pass the logging path as an argument - default_yaml_file_path = os.path.expanduser("~/gps_waypoints.yaml") - if len(sys.argv) > 1: - yaml_file_path = sys.argv[1] - else: - yaml_file_path = default_yaml_file_path - - gps_gui_logger = GpsGuiLogger(yaml_file_path) - - # Spin ROS system and Tk interface - while rclpy.ok(): - # Run ROS2 callbacks - rclpy.spin_once(gps_gui_logger, timeout_sec=0.1) - # Update the Tkinter interface - gps_gui_logger.update() - - # Clean up - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/src/nav2_tutorial/src/logged_refill_follower.py b/src/nav2_tutorial/src/logged_refill_follower.py deleted file mode 100644 index dd264ea..0000000 --- a/src/nav2_tutorial/src/logged_refill_follower.py +++ /dev/null @@ -1,359 +0,0 @@ -import os -import sys -import time -import math -import yaml -import glob -import argparse -from datetime import datetime - -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 - -from geometry_msgs.msg import PoseStamped -from robot_localization.srv import FromLL - -# Visualization -from nav_msgs.msg import Path -from visualization_msgs.msg import Marker, MarkerArray -from rclpy.qos import QoSProfile, DurabilityPolicy -MARKER_VIZ = True - -import tf2_ros # ← NEW -from geometry_msgs.msg import TransformStamped - - -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"] - 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 - - -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 - self.chunk_len = 60 # poses per NavigateThroughPoses goal - self.overlap = 5 # poses to keep ahead when we “refill” - self.refill_dist = 2.00 - - # FromLL client (WGS-84 → map frame) – create it on the navigator node - self._fromll_cli = self.navigator.create_client(FromLL, "/fromLL") - self.navigator.get_logger().info("Waiting for /fromLL service…") - if not self._fromll_cli.wait_for_service(timeout_sec=10.0): - raise RuntimeError("robot_localization /fromLL service is not available") - - self.tf_buffer = tf2_ros.Buffer(cache_time=rclpy.duration.Duration(seconds=2.0)) - self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.navigator, spin_thread=False) - - if MARKER_VIZ: - latched_qos = QoSProfile( - depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) - - # Path for visualizing waypoints - 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) - - # Convert (lat, lon, yaw) to PoseStamped in the map frame - def _latlon_to_pose(self, lat: float, lon: float, yaw: float) -> PoseStamped: - req = FromLL.Request() - req.ll_point.latitude = lat - req.ll_point.longitude = lon - req.ll_point.altitude = 0.0 - - future = self._fromll_cli.call_async(req) - rclpy.spin_until_future_complete(self.navigator, future) - map_pt = future.result().map_point - - pose = PoseStamped() - pose.header.frame_id = "map" - pose.header.stamp = rclpy.time.Time().to_msg() - pose.pose.position.x = map_pt.x - pose.pose.position.y = map_pt.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 - - def _latlon_list_to_poses(self, raw_wps) -> list[PoseStamped]: - """Convert the YAML list directly to PoseStamped list.""" - poses = [] - for wp in raw_wps: - yaw = (self.wp_parser._reverse_yaw(wp["yaw"]) - if self.reverse else wp["yaw"]) - poses.append(self._latlon_to_pose( - wp["latitude"], wp["longitude"], yaw)) - return poses - - @staticmethod - def _path_remaining(robot_xy, slice_poses) -> float: - """Return length [m] from robot's nearest pose to end of slice.""" - rx, ry = robot_xy - # 1) closest pose index - idx = min( - range(len(slice_poses)), - key=lambda i: (slice_poses[i].pose.position.x - rx)**2 + - (slice_poses[i].pose.position.y - ry)**2 - ) - # 2) accumulate segment lengths to end - rem = 0.0 - for a, b in zip(slice_poses[idx:-1], slice_poses[idx+1:]): - dx = b.pose.position.x - a.pose.position.x - dy = b.pose.position.y - a.pose.position.y - rem += math.hypot(dx, dy) - return rem - - def start_ntp(self): - self.navigator.waitUntilNav2Active(localizer="robot_localization") - - raw = (self.wp_parser.wps_dict["waypoints"][::-1] - if self.reverse else self.wp_parser.wps_dict["waypoints"]) - poses = self._latlon_list_to_poses(raw) - - if MARKER_VIZ: - self._publish_waypoints(poses) - - idx, total = 0, len(poses) - self.navigator.goThroughPoses(poses[idx: idx + self.chunk_len]) - - # -------- streaming loop using path-length remaining ------------ - avg_spacing = 0.08 # metres between recorded points - refill_len = self.overlap * avg_spacing # e.g. 10 * 0.08 = 0.8 m - - current_slice = poses[idx : idx + self.chunk_len] - - while rclpy.ok(): - rclpy.spin_once(self.navigator, timeout_sec=0.05) - - # --- if current goal is done but slices remain, queue the next one - if self.navigator.isTaskComplete() and idx < total: - idx += self.chunk_len - self.overlap - current_slice = poses[idx : idx + self.chunk_len] - self.navigator.goThroughPoses(current_slice) - continue # wait for new TF / feedback - - try: - tf = self.tf_buffer.lookup_transform( - 'map', 'base_link', rclpy.time.Time()) - except (tf2_ros.LookupException, - tf2_ros.ExtrapolationException): - continue # wait for TF to become available - - robot_xy = (tf.transform.translation.x, - tf.transform.translation.y) - - rem_len = self._path_remaining(robot_xy, current_slice) - print(f"Len remaining: {rem_len:.2f} m") - - # queue next slice when only 'overlap' distance is left - if rem_len < refill_len: - print("Idx: ", idx, "of", total) - idx += self.chunk_len - self.overlap - if idx >= total: # all slices queued - break - current_slice = poses[idx : idx + self.chunk_len] - print("Current slice: ", current_slice) - self.navigator.goThroughPoses(current_slice) - - if self.navigator.isTaskComplete() and idx >= total: - break - - while not self.navigator.isTaskComplete(): - rclpy.spin_once(self.navigator, timeout_sec=0.1) - - print("Waypoints completed successfully (streaming mode)") - - def _publish_waypoints(self, poses): - # Path message - path = Path() - path.header.frame_id = "map" - path.header.stamp = rclpy.time.Time().to_msg() - path.poses = poses - self.waypoint_path_pub.publish(path) - - # Optional coloured spheres - markers = MarkerArray() - for i, p in enumerate(poses): - m = Marker() - m.header = path.header - m.ns = "wps" - m.id = i - m.type = Marker.SPHERE - m.action = Marker.ADD - m.pose = p.pose - m.scale.x = m.scale.y = m.scale.z = 0.05 - m.color.r, m.color.g, m.color.b, m.color.a = (0.1, 0.9, 0.1, 1.0) - markers.markers.append(m) - self.waypoint_marker_pub.publish(markers) - - -def main(argv: list[str] | None = None): - rclpy.init(args=argv) - - parser = argparse.ArgumentParser(description="Follow GPS waypoints from a YAML file") - parser.add_argument("yaml_file", nargs="?", default=None, help="Path to the YAML waypoints file") - parser.add_argument("--last", action="store_true", help="Load the most recent waypoints file in the config directory") - parser.add_argument("--reverse", action="store_true", help="Follow the trajectory in reverse order (adds 180° to yaw)") - args = parser.parse_args() - - try: - yaml_file_path = _select_yaml(args) - except FileNotFoundError as e: - print(f"[WARN] {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_ntp() - except Exception as exc: - print(f"[ERROR] {exc}", file=sys.stderr) - sys.exit(1) - - -if __name__ == "__main__": - main() diff --git a/src/nav2_tutorial/src/loggers/__init__.py b/src/nav2_tutorial/src/loggers/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/nav2_tutorial/src/periodic_logger.py b/src/nav2_tutorial/src/loggers/periodic_logger.py similarity index 100% rename from src/nav2_tutorial/src/periodic_logger.py rename to src/nav2_tutorial/src/loggers/periodic_logger.py diff --git a/src/nav2_tutorial/src/terminal_logger.py b/src/nav2_tutorial/src/loggers/terminal_logger.py similarity index 75% rename from src/nav2_tutorial/src/terminal_logger.py rename to src/nav2_tutorial/src/loggers/terminal_logger.py index f59bf49..3c25753 100644 --- a/src/nav2_tutorial/src/terminal_logger.py +++ b/src/nav2_tutorial/src/loggers/terminal_logger.py @@ -5,6 +5,7 @@ import select import termios import tty +import time from datetime import datetime import rclpy @@ -29,38 +30,45 @@ def __init__(self, logging_file_path): self.logging_file_path = logging_file_path self.last_gps = None self.last_yaw = 0.0 + self.last_press_time = 0.0 + + self.gps_topic = '/fixposition/odometry_llh' + self.odom_topic = '/fixposition/odometry_enu' self.create_subscription( NavSatFix, - '/fixposition/odometry_llh', + self.gps_topic, self.gps_callback, 1 ) self.create_subscription( Odometry, - '/fixposition/odometry_enu', + 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." ) - def gps_callback(self, msg: NavSatFix): + def gps_callback(self, msg: NavSatFix) -> None: self.last_gps = msg - def yaw_callback(self, msg: Odometry): - # Extract yaw from quaternion + def yaw_callback(self, msg: Odometry) -> None: _, _, self.last_yaw = euler_from_quaternion(msg.pose.pose.orientation) - def log_waypoint(self): + def log_waypoint(self) -> None: if self.last_gps is None: self.get_logger().warn("No GPS fix yet; skipping log.") return - # Load existing file or generate a new one + if not (-90.0 <= self.last_gps.latitude <= 90.0 and -180.0 <= self.last_gps.longitude <= 180.0): + self.get_logger().warn("Received invalid GPS coordinates.") + return + try: with open(self.logging_file_path, 'r') as f: data = yaml.safe_load(f) or {} @@ -75,14 +83,18 @@ def log_waypoint(self): wp = { 'latitude': self.last_gps.latitude, 'longitude': self.last_gps.longitude, - 'yaw': self.last_yaw + 'altitude': self.last_gps.altitude, + 'yaw': self.last_yaw, + 'logged_at': datetime.now().isoformat() } wps.append(wp) data['waypoints'] = wps + tmp_path = f"{self.logging_file_path}.tmp" try: - with open(self.logging_file_path, 'w') as f: + with open(tmp_path, 'w') as f: yaml.dump(data, f, default_flow_style=False) + os.replace(tmp_path, self.logging_file_path) except Exception as e: self.get_logger().error(f"Failed to write log file: {e}") return @@ -92,34 +104,20 @@ def log_waypoint(self): f"lat={wp['latitude']:.6f}, lon={wp['longitude']:.6f}, yaw={wp['yaw']:.2f}" ) - def stdin_ready(): return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) - def main(argv=None): rclpy.init(args=argv) - # If the user passed an explicit path, use it if len(sys.argv) > 1: yaml_path = sys.argv[1] - else: - # Find the *install* share directory for nav2_tutorial share_dir = get_package_share_directory('nav2_tutorial') - - # Climb up: share/nav2_tutorial -> share -> install -> workspace root ws_root = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(share_dir)))) - - # Build the path to your src/config src_config = os.path.join(ws_root, 'src', 'nav2_tutorial', 'trajectories') - if os.path.isdir(src_config): - traj_dir = src_config - else: - # Fallback to the installed share/config - traj_dir = os.path.join(share_dir, 'trajectories') - + traj_dir = src_config if os.path.isdir(src_config) else os.path.join(share_dir, 'trajectories') os.makedirs(traj_dir, exist_ok=True) stamp = datetime.now().strftime('%Y%m%d_%H%M%S') @@ -127,7 +125,6 @@ def main(argv=None): node = GpsKeyLogger(yaml_path) - # Set terminal into raw mode fd = sys.stdin.fileno() old = termios.tcgetattr(fd) tty.setcbreak(fd) @@ -136,8 +133,10 @@ def main(argv=None): while rclpy.ok(): rclpy.spin_once(node, timeout_sec=0.1) if stdin_ready(): + now = time.time() c = sys.stdin.read(1) - if c == 'f': + if c == 'f' and (now - node.last_press_time > 0.5): + node.last_press_time = now node.log_waypoint() elif c == 'q': node.get_logger().info("Quit key pressed, shutting down.") From fb8f40da592403bf2214eb1e6bab47422aa85e87 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Mon, 30 Jun 2025 17:22:01 +0200 Subject: [PATCH 48/50] Further improvements to loggers --- README.md | 88 ++++++++++- src/nav2_tutorial/setup.py | 4 +- .../{terminal_logger.py => gps_keylogger.py} | 42 +++-- ...iodic_logger.py => gps_periodic_logger.py} | 144 +++++++----------- .../src/utils/visualize_gps_yaml.py | 66 ++++++++ 5 files changed, 226 insertions(+), 118 deletions(-) rename src/nav2_tutorial/src/loggers/{terminal_logger.py => gps_keylogger.py} (88%) rename src/nav2_tutorial/src/loggers/{periodic_logger.py => gps_periodic_logger.py} (54%) create mode 100644 src/nav2_tutorial/src/utils/visualize_gps_yaml.py diff --git a/README.md b/README.md index 3e024f1..c7be21c 100644 --- a/README.md +++ b/README.md @@ -137,22 +137,94 @@ source /opt/ros/jazzy/setup.bash ros2 run teleop_twist_keyboard teleop_twist_keyboard ``` -## Step 5: Log GNSS waypoints -The user must populate the predefined gps_waypoints.yaml file with the waypoints the robot has to follow. The user can either provide the waypoints manually or use the provided waypoint logging tool as shown below: -``` -ros2 run nav2_tutorial gps_waypoint_logger +# 🧭 GPS Waypoint Loggers for ROS 2 + +This package provides two ROS 2 nodes for recording GPS waypoints to a YAML file, based on GNSS and odometry data: + +| Script | Logging Mode | Trigger | Ideal Use Case | +| ------------------------ | -------------------- | -------------- | ------------------------------- | +| `gps_keylogger.py` | Manual | Keyboard `'f'` | Sparse or event-based recording | +| `gps_periodic_logger.py` | Automated (periodic) | Time interval | Continuous route logging | + +--- + +## 🧰 Topics Subscribed + +Both scripts subscribe to: + +* `/fixposition/odometry_llh` — `sensor_msgs/NavSatFix` +* `/fixposition/odometry_enu` — `nav_msgs/Odometry` + +--- + +## 🗺 Waypoint Format + +Logged waypoints are stored in a YAML file under the `waypoints:` key. Each entry includes: + +```yaml +waypoints: + - latitude: 47.123456 + longitude: 8.654321 + altitude: 520.4 + yaw: 1.57 + logged_at: "2025-06-30T15:47:12.003918" ``` -If your terminal does not support X11 forwarding, you can use the following script: +--- + +## 🚀 How to Use + +### Manual Logger (`gps_keylogger.py`) + +```bash +ros2 run nav2_tutorial gps_keylogger.py [optional_output.yaml] ``` -ros2 run nav2_tutorial terminal_logger + +* Press `'f'` to log a waypoint +* Press `'q'` to quit and save + +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 ``` -Lastly, you can use a periodic logger to have denser points: +* Logs waypoints automatically every 0.2s (default) +* Press `'q'` to stop recording +* Uses buffered writing to reduce I/O overhead +* Filters out insignificant movement (less than 1 cm). + +--- + +## 📦 Output Location + +If no output path is specified, the log will be written to: ``` -ros2 run nav2_tutorial periodic_logger +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: + +### Example: +```bash +python3 visualize_gps_yaml.py path/to/gps_waypoints.yaml # simple 2D plot +python3 visualize_gps_yaml.py path/to/gps_waypoints.yaml --map # map overlay (if supported) +``` + +### Requirements for Map Overlay: +- `geopandas` +- `contextily` +- `shapely` + +This will display your trajectory as a line over OpenStreetMap tiles (Mapnik style) using Web Mercator projection. + + ## Step 6: Start GPS Waypoint Following Launch the ROS nodes in the following order: diff --git a/src/nav2_tutorial/setup.py b/src/nav2_tutorial/setup.py index 7a26f1f..4620ff4 100644 --- a/src/nav2_tutorial/setup.py +++ b/src/nav2_tutorial/setup.py @@ -33,8 +33,8 @@ 'interactive_waypoint_follower = src.interactive_waypoint_follower:main', # GPS Waypoint Logger - 'terminal_logger = src.loggers.terminal_logger:main', - 'periodic_logger = src.loggers.periodic_logger:main', + 'gps_keylogger = src.loggers.gps_keylogger:main', + 'gps_periodic_logger = src.loggers.gps_periodic_logger:main', # Datum Setter 'set_datum = src.set_datum:main', diff --git a/src/nav2_tutorial/src/loggers/terminal_logger.py b/src/nav2_tutorial/src/loggers/gps_keylogger.py similarity index 88% rename from src/nav2_tutorial/src/loggers/terminal_logger.py rename to src/nav2_tutorial/src/loggers/gps_keylogger.py index 3c25753..35df771 100644 --- a/src/nav2_tutorial/src/loggers/terminal_logger.py +++ b/src/nav2_tutorial/src/loggers/gps_keylogger.py @@ -31,6 +31,8 @@ def __init__(self, logging_file_path): self.last_gps = None self.last_yaw = 0.0 self.last_press_time = 0.0 + self.waypoints = [] + self.flush_threshold = 5 self.gps_topic = '/fixposition/odometry_llh' self.odom_topic = '/fixposition/odometry_enu' @@ -69,6 +71,24 @@ def log_waypoint(self) -> None: self.get_logger().warn("Received invalid GPS coordinates.") return + wp = { + 'latitude': self.last_gps.latitude, + 'longitude': self.last_gps.longitude, + 'altitude': self.last_gps.altitude, + 'yaw': self.last_yaw, + 'logged_at': datetime.now().isoformat() + } + self.waypoints.append(wp) + + 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: try: with open(self.logging_file_path, 'r') as f: data = yaml.safe_load(f) or {} @@ -79,15 +99,7 @@ def log_waypoint(self) -> None: return wps = data.get('waypoints', []) - - wp = { - 'latitude': self.last_gps.latitude, - 'longitude': self.last_gps.longitude, - 'altitude': self.last_gps.altitude, - 'yaw': self.last_yaw, - 'logged_at': datetime.now().isoformat() - } - wps.append(wp) + wps.extend(self.waypoints) data['waypoints'] = wps tmp_path = f"{self.logging_file_path}.tmp" @@ -95,14 +107,11 @@ def log_waypoint(self) -> None: with open(tmp_path, 'w') as f: yaml.dump(data, f, default_flow_style=False) os.replace(tmp_path, self.logging_file_path) + self.get_logger().info(f"Flushed {len(self.waypoints)} waypoint(s) to disk.") + self.waypoints.clear() except Exception as e: self.get_logger().error(f"Failed to write log file: {e}") - return - self.get_logger().info( - f"Logged waypoint #{len(wps)}: " - f"lat={wp['latitude']:.6f}, lon={wp['longitude']:.6f}, yaw={wp['yaw']:.2f}" - ) def stdin_ready(): return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) @@ -135,7 +144,7 @@ def main(argv=None): if stdin_ready(): now = time.time() c = sys.stdin.read(1) - if c == 'f' and (now - node.last_press_time > 0.5): + if c == 'f' and (now - node.last_press_time > 0.2): node.last_press_time = now node.log_waypoint() elif c == 'q': @@ -144,6 +153,9 @@ def main(argv=None): except KeyboardInterrupt: node.get_logger().info("Interrupted by user, shutting down.") finally: + if node.waypoints: + node.get_logger().info("Saving remaining waypoints before exit...") + node.flush_waypoints_to_disk() termios.tcsetattr(fd, termios.TCSADRAIN, old) rclpy.shutdown() diff --git a/src/nav2_tutorial/src/loggers/periodic_logger.py b/src/nav2_tutorial/src/loggers/gps_periodic_logger.py similarity index 54% rename from src/nav2_tutorial/src/loggers/periodic_logger.py rename to src/nav2_tutorial/src/loggers/gps_periodic_logger.py index 801458e..479b5ea 100644 --- a/src/nav2_tutorial/src/loggers/periodic_logger.py +++ b/src/nav2_tutorial/src/loggers/gps_periodic_logger.py @@ -1,29 +1,14 @@ #!/usr/bin/env python3 -""" -Periodic GPS waypoint logger for ROS2 -============================================================== - -* Saves a waypoint every *interval* seconds (default: 0.2s). -* Logs when logging starts (first waypoint collected). -* Prints a throttled status message containing the most recent waypoint every 2s. -* Press **q** in the console to stop recording gracefully. - -Usage examples:: - - ros2 run gps_periodic_logger.py # default - ros2 run gps_periodic_logger.py -i 0.1 # faster - ros2 run gps_periodic_logger.py logs/points.yaml -""" - from __future__ import annotations - -import argparse import os -import select import sys -import termios import tty import math +import time +import yaml +import select +import termios +import argparse from datetime import datetime import rclpy @@ -31,15 +16,12 @@ from nav_msgs.msg import Odometry from sensor_msgs.msg import NavSatFix -import yaml from ament_index_python.packages import get_package_share_directory from src.utils.gps_utils import euler_from_quaternion class GpsPeriodicLogger(Node): - """ROS2 node that periodically logs GPS waypoints to a YAML file.""" - def __init__(self, logging_file_path: str, interval: float): super().__init__('gps_waypoint_periodic_logger') @@ -48,103 +30,88 @@ def __init__(self, logging_file_path: str, interval: float): self.last_gps: NavSatFix | None = None self.last_yaw: float = 0.0 self.saved_points: int = 0 - - self._DIST_TOL_M = 0.01 # Save only if moved ≥ 1 cm - self._DEG2M_LAT = 110_574.0 # Rough conversion at mid-latitudes - self._last_saved_fix : NavSatFix | None = None - - # ------------------------------------------------------------------ - # Topic subscriptions - # ------------------------------------------------------------------ - 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.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.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) - # ------------------------------------------------------------------ - # Timers: waypoint logger + throttled status at 2 s - # ------------------------------------------------------------------ self.create_timer(self.log_interval, self._log_waypoint) self.create_timer(2.0, self._status_message) self.get_logger().info( - f"Will log a waypoint every {self.log_interval:.3f} s to '{self.logging_file_path}'." + f"Logging every {self.log_interval:.2f} s to '{self.logging_file_path}'. Press 'q' to stop." ) - # ------------------------------------------------------------------ - # Topic callbacks - # ------------------------------------------------------------------ def gps_callback(self, msg: NavSatFix) -> None: self.last_gps = msg def yaw_callback(self, msg: Odometry) -> None: _, _, self.last_yaw = euler_from_quaternion(msg.pose.pose.orientation) - # ------------------------------------------------------------------ - # Internal helpers - # ------------------------------------------------------------------ def _log_waypoint(self) -> None: - """Append the current pose to YAML.""" fix = self.last_gps if fix is None: - return # wait for first fix - + return + + if not (-90 <= fix.latitude <= 90 and -180 <= fix.longitude <= 180): + return + if self.saved_points == 0: - # First time we succeed in logging – announce start of collection. self.get_logger().info("Started collecting waypoints …") - + if self._last_saved_fix is not None: - dlat = (fix.latitude - self._last_saved_fix.latitude ) * self._DEG2M_LAT - # convert longitude degrees to metres at current latitude + 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 - dist = math.hypot(dlat, dlon) - if dist < self._DIST_TOL_M: - return # too close → skip this sample - - # Passed the distance test → remember it for next time + if math.hypot(dlat, dlon) < self._DIST_TOL_M: + return + self._last_saved_fix = fix waypoint = { - 'latitude': self.last_gps.latitude, - 'longitude': self.last_gps.longitude, + 'latitude': fix.latitude, + 'longitude': fix.longitude, + 'altitude': fix.altitude, 'yaw': self.last_yaw, + 'logged_at': datetime.now().isoformat() } - # Read existing file + self.waypoints.append(waypoint) + self.saved_points += 1 + + if len(self.waypoints) >= self.flush_threshold: + self.flush_to_disk() + + def flush_to_disk(self) -> None: try: with open(self.logging_file_path, 'r') as fh: - data: dict = yaml.safe_load(fh) or {} + data = yaml.safe_load(fh) or {} except FileNotFoundError: data = {} except Exception as exc: self.get_logger().error(f"Failed to read log file: {exc}") return - waypoints = data.get('waypoints', []) - waypoints.append(waypoint) - data['waypoints'] = waypoints + existing = data.get('waypoints', []) + existing.extend(self.waypoints) + data['waypoints'] = existing - # Write back updated YAML + tmp_path = self.logging_file_path + ".tmp" try: - with open(self.logging_file_path, 'w') as fh: + with open(tmp_path, 'w') as fh: yaml.dump(data, fh, default_flow_style=False) + os.replace(tmp_path, self.logging_file_path) + self.get_logger().info(f"Flushed {len(self.waypoints)} waypoint(s) to disk.") + self.waypoints.clear() except Exception as exc: self.get_logger().error(f"Failed to write log file: {exc}") - return - - self.saved_points += 1 def _status_message(self) -> None: - """Emit a throttled info log with the most recently recorded waypoint.""" if self.saved_points == 0: return @@ -154,50 +121,38 @@ 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') - - # share/nav2_tutorial ➜ share ➜ install ➜ ws root 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 _parse_arguments(argv: list[str] | None = None) -> tuple[str, float]: - parser = argparse.ArgumentParser(description="Periodic GPS waypoint logger") + parser = argparse.ArgumentParser(description="Periodic GPS Waypoint Logger") parser.add_argument('yaml', nargs='?', help='Path to the YAML log file.') - parser.add_argument('-i', '--interval', type=float, default=0.2, help='Save interval in seconds (default 0.2).') + parser.add_argument('-i', '--interval', type=float, default=0.2, help='Log interval in seconds (default: 0.2).') args = parser.parse_args(argv) - yaml_path = args.yaml or _default_yaml_path() - return yaml_path, args.interval - + return args.yaml or _default_yaml_path(), args.interval def _stdin_ready() -> bool: return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) - def main(argv: list[str] | None = None) -> None: yaml_path, interval = _parse_arguments(argv) rclpy.init(args=argv) node = GpsPeriodicLogger(yaml_path, interval) - # Configure terminal for non‑blocking single‑character input fd = sys.stdin.fileno() old_settings = termios.tcgetattr(fd) tty.setcbreak(fd) try: while rclpy.ok(): - # Spin (but return regularly so we can poll the keyboard) rclpy.spin_once(node, timeout_sec=0.1) - if _stdin_ready(): ch = sys.stdin.read(1) if ch == 'q': @@ -206,6 +161,9 @@ def main(argv: list[str] | None = None) -> None: except KeyboardInterrupt: node.get_logger().info("Interrupted by user, shutting down.") finally: + if node.waypoints: + node.get_logger().info("Saving remaining waypoints before exit...") + node.flush_to_disk() termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) rclpy.shutdown() diff --git a/src/nav2_tutorial/src/utils/visualize_gps_yaml.py b/src/nav2_tutorial/src/utils/visualize_gps_yaml.py new file mode 100644 index 0000000..7aaf15a --- /dev/null +++ b/src/nav2_tutorial/src/utils/visualize_gps_yaml.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +import argparse +import yaml +import matplotlib.pyplot as plt +import contextlib + +# 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, 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 + + # Convert to GeoDataFrame and plot with contextily + 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.show() + + +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) + plot_waypoints(waypoints, use_map=args.map) + + +if __name__ == "__main__": + main() From 7873189176a692c0e1661c8de564bafd2c9a0473 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Mon, 30 Jun 2025 17:26:53 +0200 Subject: [PATCH 49/50] Save visualization plots --- src/nav2_tutorial/src/utils/visualize_gps_yaml.py | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/nav2_tutorial/src/utils/visualize_gps_yaml.py b/src/nav2_tutorial/src/utils/visualize_gps_yaml.py index 7aaf15a..d301eae 100644 --- a/src/nav2_tutorial/src/utils/visualize_gps_yaml.py +++ b/src/nav2_tutorial/src/utils/visualize_gps_yaml.py @@ -3,19 +3,18 @@ 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, use_map=False): +def plot_waypoints(waypoints, output_path, use_map=False): if not waypoints: print("No waypoints found.") return @@ -37,7 +36,6 @@ def plot_waypoints(waypoints, use_map=False): import geopandas as gpd from shapely.geometry import Point - # Convert to GeoDataFrame and plot with contextily gdf = gpd.GeoDataFrame( geometry=[Point(lon, lat) for lon, lat in zip(lons, lats)], crs="EPSG:4326" @@ -49,8 +47,8 @@ def plot_waypoints(waypoints, use_map=False): print("Map overlay requires 'geopandas' and 'contextily'. Falling back to plain plot.") plt.tight_layout() - plt.show() - + plt.savefig(output_path) + print(f"Saved plot to: {output_path}") def main(): parser = argparse.ArgumentParser(description="Visualize GPS waypoints from YAML") @@ -59,8 +57,9 @@ def main(): args = parser.parse_args() waypoints = load_waypoints(args.yaml_file) - plot_waypoints(waypoints, use_map=args.map) - + base_path, _ = os.path.splitext(args.yaml_file) + output_path = base_path + ".png" + plot_waypoints(waypoints, output_path, use_map=args.map) if __name__ == "__main__": main() From ac0ff624f3d9d69d5108727f5e04f92155acbe81 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Mon, 30 Jun 2025 17:51:55 +0200 Subject: [PATCH 50/50] Updated README and improved readability --- .devcontainer/Dockerfile | 2 + README.md | 65 +-- src/nav2_tutorial/setup.py | 11 +- ...follower.py => interactive_wp_follower.py} | 0 .../src/logged_smooth_follower.py | 412 ------------------ ...int_follower.py => precise_wp_follower.py} | 0 ...mple_follower.py => smooth_wp_follower.py} | 0 .../src/{ => utils}/set_datum.py | 0 .../src/{ => utils}/set_datum_from_tf.py | 0 9 files changed, 40 insertions(+), 450 deletions(-) rename src/nav2_tutorial/src/{interactive_waypoint_follower.py => interactive_wp_follower.py} (100%) delete mode 100644 src/nav2_tutorial/src/logged_smooth_follower.py rename src/nav2_tutorial/src/{logged_waypoint_follower.py => precise_wp_follower.py} (100%) rename src/nav2_tutorial/src/{simple_follower.py => smooth_wp_follower.py} (100%) rename src/nav2_tutorial/src/{ => utils}/set_datum.py (100%) rename src/nav2_tutorial/src/{ => utils}/set_datum_from_tf.py (100%) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 23f859b..fcdc041 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -29,6 +29,8 @@ RUN apt update && \ can-utils \ net-tools \ python3-tk \ + python3-matplotlib \ + python3-geopandas \ ros-jazzy-xacro \ ros-jazzy-navigation2 \ ros-jazzy-nav2-bringup \ diff --git a/README.md b/README.md index c7be21c..7ad4352 100644 --- a/README.md +++ b/README.md @@ -15,14 +15,14 @@ The following ROS packages are required for this project: 3. Fixposition Nav2 Tutorial ([nav2_tutorial](https://github.com/fixposition/nav2_tutorial.git)) To obtain them, simply execute the following command at the root of the repository: -``` +```bash git submodule update --init --recursive ``` ### (Optional) Docker container The user can also compile the provided Docker container in the .devcontainer folder to test this tutorial. To achieve this, the following commands can be used: -``` +```bash docker compose -f .devcontainer/docker-compose.yaml build docker compose -f .devcontainer/docker-compose.yaml up -d docker compose -f ~/dev/nav2_tutorial/.devcontainer/docker-compose.yaml exec vrtk bash @@ -30,7 +30,7 @@ docker compose -f ~/dev/nav2_tutorial/.devcontainer/docker-compose.yaml exec vrt Alternatively, the user can compile it directly using the Dev Containers extension in VSCode. To check the status of the different services, you can run the following commands: -``` +```bash docker compose logs vrtk ``` @@ -70,14 +70,14 @@ converter: ## Step 3: Build ROS2 workspace Build the ROS2 workspace. -``` +```bash source /opt/ros/jazzy/setup.bash colcon build --cmake-args -DBUILD_TESTING=OFF ``` ## Step 4: Source built packages -``` +```bash source install/setup.bash ``` @@ -87,19 +87,19 @@ source install/setup.bash ### Establish connection with the Scout robot To communicate with the Scout robot, you must use the provided USB-to-CAN adapter. Then, run the following commands to start the connection: -``` +```bash sudo modprobe gs_usb can-utils 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: -``` +```bash sudo ./scripts/start_can.sh ``` Example output from the can0 port: -``` +```bash can0 311 [8] 00 00 25 C6 FF FF F0 A4 can0 251 [8] 00 00 00 00 00 00 00 00 can0 252 [8] 00 00 00 00 00 00 00 00 @@ -112,27 +112,27 @@ can0 311 [8] 00 00 25 C6 FF FF F0 A4 ``` Note: To automatically establish the connection to the Agilex, you can edit the /etc/modules file as such: -``` +```bash # /etc/modules: kernel modules to load at boot time. gs_usb can-utils ``` In addition, in case the error messsage 'RTNETLINK answers: Device or resource busy' appears, please run the following command: -``` +```bash sudo ip link set can0 down ``` ### Control the Scout robot using the keyboard In terminal 1, run these commands to start the base node for the Scout robot: -``` +```bash source /opt/ros/jazzy/setup.bash && source install/setup.bash ros2 launch scout_base scout_mini_base.launch.py ``` In terminal 2, run these commands to start the keyboard-based controller: -``` +```bash source /opt/ros/jazzy/setup.bash ros2 run teleop_twist_keyboard teleop_twist_keyboard ``` @@ -213,8 +213,8 @@ To quickly visualize GPS waypoint logs, use the `visualize_gps_yaml.py` script: ### Example: ```bash -python3 visualize_gps_yaml.py path/to/gps_waypoints.yaml # simple 2D plot -python3 visualize_gps_yaml.py path/to/gps_waypoints.yaml --map # map overlay (if supported) +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) ``` ### Requirements for Map Overlay: @@ -226,34 +226,35 @@ This will display your trajectory as a line over OpenStreetMap tiles (Mapnik sty -## Step 6: Start GPS Waypoint Following +# GPS Waypoint Follower Launch the ROS nodes in the following order: -``` +```bash ros2 launch scout_base scout_mini_base.launch.py ros2 launch nav2_tutorial fp_driver_node.launch config:=fp_driver_config.yaml 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. -For launching the graphical interface, you can run the following command (note that this only works on x86/amd64 devices): -``` -ros2 launch nav2_tutorial mapviz.launch.py +* 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. +```bash +ros2 run nav2_tutorial precise_wp_follower ``` -Finally, start the navigation. There are two types waypoint following methods. We can only choose one method each time we execute it. - -* Interactive GPS Waypoint Follower -``` -ros2 run nav2_tutorial interactive_waypoint_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. +```bash +ros2 run nav2_tutorial smooth_wp_follower ``` - - -Then, call the logged_waypoint_follower script to make the robot follow the logged waypoints: -``` -ros2 run nav2_tutorial logged_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 ``` -For a smooth version using a pre-computed trajectory (instead of point-by-point), call the logged_smooth_follower script: -``` -ros2 run nav2_tutorial logged_smooth_follower +For launching the graphical interface, you can run the following command (note that this only works on x86/amd64 devices): +```bash +ros2 launch nav2_tutorial mapviz.launch.py ``` +Or alternatively use the `--mapviz` flag on the `gps_waypoint_follower.launch.py` script. diff --git a/src/nav2_tutorial/setup.py b/src/nav2_tutorial/setup.py index 4620ff4..d46c886 100644 --- a/src/nav2_tutorial/setup.py +++ b/src/nav2_tutorial/setup.py @@ -27,18 +27,17 @@ entry_points={ 'console_scripts': [ # GPS Waypoint Follower - 'simple_follower = src.simple_follower:main', - 'logged_smooth_follower = src.logged_smooth_follower:main', - 'logged_waypoint_follower = src.logged_waypoint_follower:main', - 'interactive_waypoint_follower = src.interactive_waypoint_follower:main', + 'smooth_wp_follower = src.smooth_wp_follower:main', + 'precise_wp_follower = src.precise_wp_follower:main', + 'interactive_wp_follower = src.interactive_wp_follower:main', # GPS Waypoint Logger 'gps_keylogger = src.loggers.gps_keylogger:main', 'gps_periodic_logger = src.loggers.gps_periodic_logger:main', # Datum Setter - 'set_datum = src.set_datum:main', - 'set_datum_from_tf = src.set_datum_from_tf:main', + 'set_datum = src.utils.set_datum:main', + 'set_datum_from_tf = src.utils.set_datum_from_tf:main', ], }, ) diff --git a/src/nav2_tutorial/src/interactive_waypoint_follower.py b/src/nav2_tutorial/src/interactive_wp_follower.py similarity index 100% rename from src/nav2_tutorial/src/interactive_waypoint_follower.py rename to src/nav2_tutorial/src/interactive_wp_follower.py diff --git a/src/nav2_tutorial/src/logged_smooth_follower.py b/src/nav2_tutorial/src/logged_smooth_follower.py deleted file mode 100644 index 6e3518d..0000000 --- a/src/nav2_tutorial/src/logged_smooth_follower.py +++ /dev/null @@ -1,412 +0,0 @@ -import os -import sys -import time -import math -import yaml -import glob -import argparse -from datetime import datetime - -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 geometry_msgs.msg import PoseStamped -from robot_localization.srv import FromLL -from nav_msgs.msg import Odometry - -# Visualization -from nav_msgs.msg import Path -from visualization_msgs.msg import Marker, MarkerArray -from rclpy.qos import QoSProfile, DurabilityPolicy -MARKER_VIZ = True - - -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"] - 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 - - -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 - - # Odometry topic with ENU pose in map frame - self._last_odom_pose: PoseStamped | None = None - self.navigator.create_subscription(Odometry, "/fixposition/odometry_enu", self._odom_cb, 10) - - # Parameters for the NavigateThroughPoses task - self.max_window = 50.0 # meters (square side length) - self.safety_margin = 1.0 # meters (keep a little inside the edge) - self.max_seg_len = 100.0 # meters of path length per sub-goal - self.radius = self.max_window/2 - self.safety_margin # ≈ 24 m - self.odom_timeout_s = 2.0 # wait this long for first /fixposition msg - self.max_retries = 1 # re-attempt each failed segment once - - # FromLL client (WGS-84 → map frame) – create it on the navigator node - self._fromll_cli = self.navigator.create_client(FromLL, "/fromLL") - self.navigator.get_logger().info("Waiting for /fromLL service…") - if not self._fromll_cli.wait_for_service(timeout_sec=10.0): - raise RuntimeError("robot_localization /fromLL service is not available") - - if MARKER_VIZ: - latched_qos = QoSProfile( - depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) - - # Path for visualizing waypoints - 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) - - # Convert (lat, lon, yaw) to PoseStamped in the map frame - def _latlon_to_pose(self, lat: float, lon: float, yaw: float) -> PoseStamped: - req = FromLL.Request() - req.ll_point.latitude = lat - req.ll_point.longitude = lon - req.ll_point.altitude = 0.0 - - future = self._fromll_cli.call_async(req) - rclpy.spin_until_future_complete(self.navigator, future) - map_pt = future.result().map_point - - pose = PoseStamped() - pose.header.frame_id = "map" - pose.header.stamp = rclpy.time.Time().to_msg() - pose.pose.position.x = map_pt.x - pose.pose.position.y = map_pt.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 - - def start_ntp(self): - """Send *multiple* NavigateThroughPoses goals whenever the full list of - waypoints would not fit inside the rolling global cost-map. Each sub-goal - is guaranteed to stay inside a radius *R* around the pose that was current - when the sub-goal was dispatched.""" - self.navigator.waitUntilNav2Active(localizer="robot_localization") - - raw_wps = self.wp_parser.wps_dict["waypoints"] - if self.reverse: - raw_wps = raw_wps[::-1] - - poses = [] - for wp in raw_wps: - yaw = (self.wp_parser._reverse_yaw(wp["yaw"]) - if self.reverse else wp["yaw"]) - poses.append(self._latlon_to_pose( - wp["latitude"], wp["longitude"], yaw)) - - seg_idx = 0 - remaining = poses - retries_left = self.max_retries - - while remaining: - # Current robot pose in map frame (start-of-segment centre) - try: - cur_pose = self._get_robot_pose() - except Exception as e: - self.navigator.get_logger().error(f"Failed to get robot pose: {e}") - return - cx, cy = cur_pose.pose.position.x, cur_pose.pose.position.y - - # Build one segment whose every pose lies inside the radius R - segment = [] - cum_length = 0.0 # running distance - prev_xy = (cx, cy) # start measuring from robot - - for p in remaining: - dx = p.pose.position.x - cx - dy = p.pose.position.y - cy - - # Window test **in map axes** (square, not circle) - half_win = self.max_window/2 - self.safety_margin - inside_window = (abs(dx) <= half_win) and (abs(dy) <= half_win) - - # Distance if we *would* append this pose - step = math.hypot(p.pose.position.x - prev_xy[0], - p.pose.position.y - prev_xy[1]) - inside_length = (cum_length + step) <= self.max_seg_len - - if not (inside_window and inside_length): - # Adding this pose would violate at least one rule - if segment: # we already have a valid chunk - break # finish the segment here - else: - # Even the *first* pose is out of bounds → force-split - # so we can advance and recompute next time - self.navigator.get_logger().warn( - f"Pose {(p.pose.position.x, p.pose.position.y)} " - "is out of the current 50x50 m window - " - "sending it alone.") - segment.append(p) - break - - # Safe to add - segment.append(p) - cum_length += step - prev_xy = (p.pose.position.x, p.pose.position.y) - - # Publish & send this segment - self._publish_waypoints(segment) - self.navigator.goThroughPoses(segment) - - seg_idx += 1 - self.navigator.get_logger().info(f"Segment {seg_idx}: {len(segment)} poses") - - while not self.navigator.isTaskComplete(): - rclpy.spin_once(self.navigator, timeout_sec=0.1) - - # Assess the result of the NavigateThroughPoses task - result = self.navigator.getResult() - - # Path successfully completed - if result == TaskResult.SUCCEEDED: - remaining = remaining[len(segment):] - retries_left = self.max_retries - continue - - # Otherwise: FAILED or CANCELED - self.navigator.get_logger().warn( - f"Segment {seg_idx} did not reach the goal (status={result}).") - - if retries_left <= 0: - self.navigator.get_logger().error( - "Exceeded retry budget — aborting mission.") - return - - retries_left -= 1 - - # Re-insert the un-reached waypoints *and* a fresh “start-now” pose - try: - cur_pose = self._get_robot_pose() - except Exception as e: - self.navigator.get_logger().error(f"Cannot recover: {e}") - return - - self.navigator.get_logger().info( - "Re-planning from current position…") - - # Re-insert the current pose to start from it - remaining = [cur_pose] + segment + remaining[len(segment):] - - print("Waypoints completed successfully (segmented mode)") - - def _odom_cb(self, msg: Odometry) -> None: - """Cache the most recent odometry pose as a `PoseStamped`.""" - ps = PoseStamped() - ps.header = msg.header - ps.pose = msg.pose.pose - self._last_odom_pose = ps - - def _get_robot_pose(self) -> PoseStamped: - """ - Return the most recent pose from /fixposition/odometry_enu. - Blocks (spins) until a message arrives or the timeout elapses. - """ - deadline = self.navigator.get_clock().now() + rclpy.duration.Duration( - seconds=self.odom_timeout_s) - - while self._last_odom_pose is None: - if self.navigator.get_clock().now() > deadline: - raise RuntimeError( - f"No Odometry on /fixposition/odometry_enu " - f"after {self.odom_timeout_s:.1f}s") - rclpy.spin_once(self.navigator, timeout_sec=0.05) - - return self._last_odom_pose - - def _publish_waypoints(self, poses): - # Path message - path = Path() - path.header.frame_id = "map" - path.header.stamp = rclpy.time.Time().to_msg() - path.poses = poses - self.waypoint_path_pub.publish(path) - - # Optional coloured spheres - markers = MarkerArray() - for i, p in enumerate(poses): - m = Marker() - m.header = path.header - m.ns = "wps" - m.id = i - m.type = Marker.SPHERE - m.action = Marker.ADD - m.pose = p.pose - m.scale.x = m.scale.y = m.scale.z = 0.05 - m.color.r, m.color.g, m.color.b, m.color.a = (0.1, 0.9, 0.1, 1.0) - markers.markers.append(m) - self.waypoint_marker_pub.publish(markers) - - -def main(argv: list[str] | None = None): - rclpy.init(args=argv) - - parser = argparse.ArgumentParser(description="Follow GPS waypoints from a YAML file") - parser.add_argument("yaml_file", nargs="?", default=None, help="Path to the YAML waypoints file") - parser.add_argument("--last", action="store_true", help="Load the most recent waypoints file in the config directory") - parser.add_argument("--reverse", action="store_true", help="Follow the trajectory in reverse order (adds 180° to yaw)") - args = parser.parse_args() - - try: - yaml_file_path = _select_yaml(args) - except FileNotFoundError as e: - print(f"[WARN] {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_ntp() - except Exception as exc: - print(f"[ERROR] {exc}", file=sys.stderr) - sys.exit(1) - - -if __name__ == "__main__": - main() diff --git a/src/nav2_tutorial/src/logged_waypoint_follower.py b/src/nav2_tutorial/src/precise_wp_follower.py similarity index 100% rename from src/nav2_tutorial/src/logged_waypoint_follower.py rename to src/nav2_tutorial/src/precise_wp_follower.py diff --git a/src/nav2_tutorial/src/simple_follower.py b/src/nav2_tutorial/src/smooth_wp_follower.py similarity index 100% rename from src/nav2_tutorial/src/simple_follower.py rename to src/nav2_tutorial/src/smooth_wp_follower.py diff --git a/src/nav2_tutorial/src/set_datum.py b/src/nav2_tutorial/src/utils/set_datum.py similarity index 100% rename from src/nav2_tutorial/src/set_datum.py rename to src/nav2_tutorial/src/utils/set_datum.py diff --git a/src/nav2_tutorial/src/set_datum_from_tf.py b/src/nav2_tutorial/src/utils/set_datum_from_tf.py similarity index 100% rename from src/nav2_tutorial/src/set_datum_from_tf.py rename to src/nav2_tutorial/src/utils/set_datum_from_tf.py