diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 00000000..6774b854 Binary files /dev/null and b/.DS_Store differ diff --git a/.github/.DS_Store b/.github/.DS_Store new file mode 100644 index 00000000..5fcc68a0 Binary files /dev/null and b/.github/.DS_Store differ diff --git a/.gitignore b/.gitignore index 79d9b449..baa5bafe 100644 --- a/.gitignore +++ b/.gitignore @@ -24,6 +24,8 @@ __pycache__/ # Distribution / packaging .Python build/ +install/ +log/ develop-eggs/ dist/ downloads/ diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 00000000..e4ebb8c3 --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,155 @@ +# URC Software Agent Guide + +This file defines the engineering standards for changes to this repository. + +## Repository Context + +- This is a ROS 2 Humble workspace targeting Ubuntu 22.04. +- First-party packages use C++17, Python launch files, `ament_cmake`, and `colcon`. +- The repository root is `rover_ws/src`; run `colcon` commands from `rover_ws`. +- Treat `external/` as vendored submodule code. Do not modify it unless the task + explicitly requires a dependency change. +- Do not edit generated files or files under `build/`, `install/`, or `log/`. +- Inspect `git status` before editing and preserve unrelated user changes. + +## Working Principles + +- Understand the owning package before changing it. Read its `CMakeLists.txt`, + `package.xml`, configuration, public headers, and nearby implementation. +- Make the smallest coherent change that fully solves the requested problem. +- Apply these standards strongly to new and meaningfully modified code. +- When legacy code near the change violates these standards, improve the changed + responsibility and nearby correctness hazards without starting an unrelated + repository-wide refactor. +- Keep dependencies, install rules, launch files, configuration, interfaces, and + documentation synchronized with behavior. + +## Architecture and File Organization + +- Keep ROS nodes thin. They should primarily own parameters, publishers, + subscriptions, services, actions, timers, lifecycle behavior, and top-level + orchestration. +- Move algorithms, state machines, validation, transformations, filtering, + scoring, serialization, and hardware-independent policy into focused + components. +- Main orchestration functions should read as a short sequence of clearly named + operations. +- Prefer one principal responsibility per file and one abstraction level per + function. +- Split substantial responsibilities into domain-named header/source pairs. + Avoid using generic `helpers`, `utils`, or `common` files as dumping grounds. +- Public headers contain contracts, declarations, public types, and only the + members necessary to represent the type. Implementations and file-local + helpers belong in `.cpp` files. +- Use anonymous namespaces for helpers used by only one translation unit. +- Use the conventional package structure: + - `include//` for public headers + - `src/` for implementations and private components + - `config/` for runtime configuration + - `launch/` for launch composition + - ROS-specific asset folders such as `urdf/`, `rviz/`, `world/`, and `meshes/` + only for their respective assets +- Treat roughly 250 non-generated source lines per file and 40 lines per function + as decomposition review triggers. Exceeding them requires a cohesive reason. +- Split by responsibility rather than line count. Do not replace one monolith + with many trivial or tightly coupled files. + +## Clean Code + +- Use descriptive, domain-specific names. Code structure and naming should make + normal control flow understandable without explanatory comments. +- Give each function one clear purpose with explicit inputs and outputs. +- Prefer early returns and shallow control flow over deep nesting. +- Avoid hidden side effects, boolean control parameters, oversized parameter + lists, duplicated logic, mutable global state, and ambiguous ownership. +- Use RAII, initialized state, const-correctness, narrow interfaces, standard + library facilities, and explicit ownership. +- Use smart pointers only when pointer semantics are required. Never use a raw + pointer to express ownership. +- Include only what a file uses. Keep public headers dependency-light and use + package-scoped include paths. +- Replace unexplained literals with well-named constants or validated parameters + when the value has domain meaning. +- Do not use non-standard umbrella headers such as `bits/stdc++.h`. +- Remove unused includes, members, locals, functions, and unreachable code. +- Do not leave commented-out code, debugging statements, or unjustified warning + suppressions. +- Do not swallow exceptions broadly. Handle errors at the layer that can add + context or make a safe recovery decision. +- Follow `ament_code_style.cfg` for C++ formatting and `ament_flake8` for Python. + +## Comments and Documentation + +- Keep package documentation concise and task-oriented. Cover the package's + purpose, primary usage, public contracts, operational constraints, and links + to deeper references. +- Do not duplicate source structure, enumerate incidental launch options, or + document test-only and niche workflows unless they are operationally + important. Let code, configuration, and `ros2 launch --show-args` provide + exhaustive details. +- Comments are exceptional. Prefer clearer names, smaller functions, and better + structure. +- Add a comment only when it explains information the code cannot express well: + - design intent or a non-obvious invariant + - a safety or failure constraint + - frames, units, coordinate conventions, ownership, or concurrency contracts + - non-obvious mathematics, algorithms, or source references + - a deliberate performance tradeoff or platform constraint +- Do not narrate statements, label obvious sections, explain straightforward + loops, repeat symbol names, or preserve debugging notes. +- Remove stale or redundant comments when modifying the associated code. +- Document public behavior and contracts rather than implementation trivia. +- TODOs must identify a concrete remaining problem, not a vague improvement. + +## Runtime and Complexity + +- Be mindful of runtime and memory complexity when choosing algorithms and data + structures. Formal Big-O documentation is not required. +- Avoid quadratic work or repeated full-data scans when a practical linear or + logarithmic approach exists. +- Keep callbacks, timers, control loops, and hardware paths bounded and + non-blocking. +- Avoid unnecessary ROS message copies, heap allocations, filesystem access, + network waits, transform waits, and verbose logging in hot paths. +- Move expensive work out of latency-sensitive callbacks when needed. Define + synchronization, cancellation, ownership, and stale-result behavior clearly. +- Prefer incremental updates, bounded queues, timeouts, and configurable work + limits for inputs that can grow indefinitely. +- Cache expensive derived data only when ownership and invalidation are clear. +- Prefer observed behavior and profiling over speculative micro-optimization. + +## ROS and Robotics Standards + +- Parameterize operational topics, services, actions, frames, device paths, + thresholds, limits, and rates. Validate parameters during initialization. +- Internal values that are not operationally tunable may be local `constexpr` + constants. +- Put normal parameter overrides in package YAML. Keep launch files focused on + composition, configuration loading, and high-level mode selection. +- Choose QoS deliberately for the data's reliability, durability, and frequency. +- Preserve timestamps, frame ownership, coordinate conventions, and physical + units across interfaces. +- Use an appropriate executor model, mutex, or atomic strategy for shared callback + state. Avoid holding locks while publishing, logging, or performing slow work. +- Handle invalid input, stale data, unavailable services, missing transforms, + cancellation, partial initialization, and shutdown safely. +- Throttle repetitive diagnostics. Logging must not dominate sensor or control + callbacks. +- Prefer simulation when it can validate the behavior safely. Use physical + hardware only when the task and available environment authorize it. +- Never silently change safety limits, hardware assumptions, network addresses, + device paths, topic contracts, frames, or units. + +## Validation and Completion + +- Build affected packages and run applicable repository lint checks when the + environment supports them. +- Validate behavior in simulation or on hardware as appropriate. Qualitative + validation is acceptable for normal robotics behavior; use quantitative + evaluation when the task specifically requires measurable performance. +- New unit tests and launch tests are not required unless explicitly requested. +- Run `git diff --check` before completion. +- Report what changed, what was checked, what was observed, and anything that + could not be verified. +- Never claim that a build, lint check, simulation, hardware trial, or performance + target passed unless it was actually run and observed. diff --git a/DOCUMENTATION.md b/DOCUMENTATION.md deleted file mode 100644 index 8d35e87f..00000000 --- a/DOCUMENTATION.md +++ /dev/null @@ -1,76 +0,0 @@ -# RoboNav Package Documentation - -Welcome to the centralized documentation for the **RoboNav software stack**. This directory contains detailed documentation for each package in the system to aid development, onboarding, and debugging. - -## Installation - -Full Installation Instructions [URC Software README](../README.md). - -## Package Documentation - -### Core System & Utilities -- [README.md](README.md) – You’re currently reading this overview file. -- [bringup.md](bringup.md) – Universal launch files and system bringup instructions. -- [orchestrator.md](orchestrator.md) – System orchestration node managing overall robot behavior. -- [msgs.md](msgs.md) – Custom ROS2 messages shared across packages. -- [nanopb.md](nanopb.md) – NanoPB protocol buffers configuration and usage. -- [util.md](util.md) – Utility scripts and helper nodes for development and deployment. - -### Hardware & Simulation -- [hw.md](hw.md) – ROS2 control hardware interface implementation. -- [hw_description.md](hw_description.md) – URDF and hardware description files for the rover. -- [controllers.md](controllers.md) – ROS2 control controllers for hardware interfaces. -- [platform.md](platform.md) – Platform-specific hardware communication nodes. -- [gazebo.md](gazebo.md) – Simulation configuration and Gazebo integration. - -### Robotic Arm Control -- [arm.md](arm.md) – Nodes for controlling the robotic arm. -- [arm_moveit_config.md](arm_moveit_config.md) – MoveIt configuration package for arm planning and control. -- [walli_arm.md](walli_arm.md) – Walli-specific robotic arm MoveIt configuration. - -### Autonomy & Perception -- [bt.md](bt.md) – Behavior tree core package for task execution. -- [bt_nodes.md](bt_nodes.md) – Custom behavior tree nodes for autonomy. -- [navigation.md](navigation.md) – Navigation stack, including Nav2 configuration and nodes. -- [perception.md](perception.md) – Perception stack, including computer vision and detection nodes. - ---- - -## How to Use These Documents - -Each markdown file follows a standardized structure to provide comprehensive information about the package: - -1. **Overview** - A high-level description of the package, its purpose, and the system modules it includes. - -2. **Features** - Key functionalities and capabilities of the package. - -3. **Package Structure** - Details of the folder and file organization within the package. - -4. **Components** - Descriptions of modules and their components, including inputs, outputs, and key functionality. - -5. **Nodes** - Information about ROS 2 nodes (if applicable), including: - - Subscriptions - - Publishers - - Services - - Actions - - Parameters - -6. **Launch Instructions** - How to start nodes or modules using ROS 2 launch files. - ---- - -## Helpful Links - -- [URC Software Repository](https://github.com/RoboJackets/urc-software) -- [ROS2 Documentation](https://docs.ros.org/en/humble/index.html) -- [MoveIt2 Documentation](https://moveit.picknik.ai/main/index.html) -- [Nav2 Documentation](https://navigation.ros.org/) - -_Last updated: 2025-08-19_ - diff --git a/README.md b/README.md index 85b73019..886fd9e0 100644 --- a/README.md +++ b/README.md @@ -1,93 +1,284 @@ -# urc-software [![CI Status Badge](https://github.com/RoboJackets/urc-software/actions/workflows/ci.yml/badge.svg)](https://github.com/RoboJackets/urc-software/actions) - -Welcome to the RoboJackets/RoboNav software repo for the [University Rover Challenge](https://urc.marssociety.org/) (URC)! This document will give you a brief description of the repo's layout and an overview of the repo. - -[![Static Badge](https://img.shields.io/badge/Software_Lead-Mrinal_Jain-EAAA00)](https://github.com/mrinalTheCoder) - -## Directory Structure - -- **.github** - _CI pipeline and PR/issue templates_ -- **cmake** - _CMake files to aid with building_ -- **documents** - _Research, design, and documentation_ -- **external** - _Where all our submodules are located_ -- **urc_arm_moveit_config** - _Moveit config folder for rover arm_ -- **urc_bringup** - _Location of the universal launch file + heartbeat node_ -- **urc_controllers** - _ros2-control controllers_ -- **urc_gazebo** - _Helper nodes used for simulation purposes_ -- **urc_hw** - _ros2-control hardware interface_ -- **urc_hw_description** - _URDF description for the rover_ -- **urc_manipulation** - _Collection of nodes used for the robotic arm_ -- **urc_nanopb** - _nanopb related files and settings_ -- **urc_msgs** - _Custom ROS messages used in various packages_ -- **urc_platform** - _Manages our nanopb protocol buffers_ -- **urc_navigation** - _Collection of nodes that form our navigation stack_ -- **urc_perception** - _Collection of nodes that form our perception stack_ -- **urc_platform** - _Nodes that are platform specific and used to communicate with the hardware, ie. IMU, joystick and motor controller_ - -## Installation Instructions - -**Essential**
-You will need to be using Ubuntu 22.04 to run ROS2. This can be accomplished with any of the following methods: - -- [Ubuntu 22.04: Native Installation or WSL (Windows/Linux)](documents/installation/ubuntu_installation.md) **Strongly recommended!** -- [Docker Installation Instructions (Mac/Windows/Linux)](documents/installation/docker_installation.md) **Less viable, use for Apple Silicon** - -**Specific Features** - -- [XBox Controller Setup](documents/installation/controller_setup.md) -- [Depth Camera Setup](documents/installation/camera_setup.md) -- [ROS2 Control Gazebo Setup](documents/installation/ros2_control.md) -- [Radio Communication Between Rover and Ground Station](documents/installation/radio_setup.md) - -## Helpful Resources - -- [Useful Commands: ROS2 Commands, Git Commands](documents/helpers/useful_commands.md) -- [Design Presentation Requirements](documents/design/README.md) -- [Drone Repository](https://github.com/RoboJackets/urc-drone) -- [Firmware Repository](https://github.com/RoboJackets/urc-firmware/tree/master) - -## Team-Related Links - -- [Slack](https://robojackets.slack.com/) -- [Google Drive](https://drive.google.com/drive/folders/1qZ3fwFvTRdvCWRLjbE44AmqxUnaBq8FP?usp=drive_link) -- [Software Training](https://github.com/RoboJackets/software-training-old) - -## External Documentation and Background Reading - -- [ROS2 Humble Documentation](https://docs.ros.org/en/humble/index.html) -- [MoveIt2 Documentation](https://moveit.picknik.ai/main/index.html) -- [Nav2 Documentation](https://navigation.ros.org/) -- [ROS2 Control Documentation](https://control.ros.org/master/index.html) - -## Common Issues - -#### NanoPB Not Building - -Fix (will only build after the last time): +# URC Rover Software +[![CI](https://github.com/RoboJackets/urc-software/actions/workflows/ci.yml/badge.svg)](https://github.com/RoboJackets/urc-software/actions/workflows/ci.yml) + +This repository contains the RoboJackets University Rover Challenge ROS 2 +software stack. It targets ROS 2 Humble on Ubuntu 22.04 and uses C++17, Python +launch files, `ament_cmake`, and `colcon`. + +## Quick start + +Create a workspace and clone the repository with its submodules: + +```bash +mkdir -p rover_ws +git clone --recurse-submodules https://github.com/RoboJackets/urc-software.git rover_ws/src +cd rover_ws ``` -colcon build --symlink-install ; chmod +x build/urc_nanopb/nanopb/generator/protoc-gen-nanopb -colcon build --symlink-install ; chmod +x build/urc_nanopb/nanopb/generator/nanopb_generator.py + +Install dependencies and build from the workspace root: + +```bash +source /opt/ros/humble/setup.bash +rosdep update +rosdep install --from-paths src --ignore-src -r -y colcon build --symlink-install +source install/setup.bash +``` + +Start the rover simulation: + +```bash +ros2 launch urc_bringup sim.launch.py +``` + +Enable the autonomy stack when needed: + +```bash +ros2 launch urc_bringup sim.launch.py autonomy:=true +``` + +There is currently no single launch file for complete physical-rover bringup. + +## Repository map + +- [`urc_bringup`](urc_bringup/README.md) composes simulation, autonomy, + base-station, and rocker-control launches. +- [`urc_hw`](urc_hw/README.md), + [`urc_controllers`](urc_controllers/README.md), and + [`urc_hw_description`](urc_hw_description/README.md) own hardware access, + ROS 2 control, and the rover model. +- [`urc_localization`](urc_localization/README.md), + [`urc_perception`](urc_perception/README.md), + [`urc_path_planning`](urc_path_planning/README.md), + [`urc_state_machine`](urc_state_machine/README.md), and + [`urc_trajectory_following`](urc_trajectory_following/README.md) form the + autonomy stack. +- [`urc_platform`](urc_platform/README.md), [`urc_msgs`](urc_msgs/README.md), + [`urc_nanopb`](urc_nanopb/README.md), and + [`urc_nav_common`](urc_nav_common/README.md) provide platform adapters and + shared interfaces. +- `external` contains vendored submodules and should not be modified as normal + first-party code. + +## Core package layout + +The core packages follow the standard ROS 2 layout: public C++ headers live in +`include//`, implementations live in `src/`, runtime parameters live in +`config/`, and launch composition lives in `launch/`. The trees below omit the +`CMakeLists.txt`, `package.xml`, and `README.md` present at each package root. + +### Bringup and autonomy + +`urc_bringup` remains the entry point for composing the rover stack. It does not +own general-purpose controller, localization, or platform node implementations. + +```text +urc_bringup/ +├── config/ +│ ├── sim_config.yaml +│ └── test_controllers.yaml +└── launch/ + ├── autonomy.launch.py + ├── base_station.launch.py + ├── rocker_effort_pid.launch.py + └── sim.launch.py + +urc_perception/ +├── config/ +│ ├── pcl_grid_map_params.yaml +│ └── traversability_params.yaml +├── include/urc_perception/ +│ ├── gaussian_filter.hpp +│ └── traversability_mapping.hpp +├── launch/ +│ ├── d435i.launch.py +│ ├── mapping.launch.py +│ └── perception.launch.py +└── src/ + ├── gaussian_filter.cpp + └── traversability_mapping.cpp + +urc_nav_common/ +├── include/urc_nav_common/grid_map_utils.hpp +└── src/grid_map_utils.cpp + +urc_path_planning/ +├── include/urc_path_planning/ +│ ├── astar.hpp +│ └── planner_server.hpp +├── launch/planning.launch.py +└── src/ + ├── astar.cpp + └── planner_server.cpp + +urc_state_machine/ +├── include/urc_state_machine/nav_coordinator.hpp +└── src/nav_coordinator.cpp + +urc_trajectory_following/ +├── config/pure_pursuit_config.yaml +├── include/urc_trajectory_following/ +│ ├── follower_action_server.hpp +│ ├── geometry_util.hpp +│ ├── trajectory_controller.hpp +│ ├── trajectory_factory.hpp +│ └── pure_pursuit/pure_pursuit.hpp +├── launch/trajectory_following.launch.py +└── src/ + ├── follower_action_server.cpp + ├── geometry_util.cpp + ├── trajectory_factory.cpp + └── pure_pursuit/pure_pursuit.cpp ``` -Can also try downgrading/installing protobuf (described in error message): + +Run the complete simulation through `urc_bringup`: + +```bash +ros2 launch urc_bringup sim.launch.py +ros2 launch urc_bringup sim.launch.py autonomy:=true ``` -pip install protoc==3.20 + +Run only the autonomy nodes when localization, sensors, TF, and rover control +are already available: + +```bash +ros2 launch urc_bringup autonomy.launch.py ``` + +There is currently no single launch file for complete physical-rover bringup. + +### Hardware, localization, and platform integration + +```text +urc_controllers/ +├── include/urc_controllers/ +│ ├── bms_broadcaster.hpp +│ ├── rocker_effort_pid.hpp +│ ├── rocker_tf_broadcaster.hpp +│ ├── status_light_controller.hpp +│ └── swerve_drive_controller.hpp +└── src/ + ├── bms_broadcaster.cpp + ├── rocker_effort_pid.cpp + ├── rocker_tf_broadcaster.cpp + ├── status_light_controller.cpp + └── swerve_drive_controller.cpp + +urc_hw/ +├── include/urc_hw/ +│ ├── hardware/serial.hpp +│ ├── hardware_interface_types.hpp +│ └── hardware_interfaces/ +│ ├── arm_control.hpp +│ ├── battery_management.hpp +│ ├── rover_drivetrain.hpp +│ ├── science_module.hpp +│ ├── status_light.hpp +│ └── test_hardware.hpp +└── src/ + ├── hardware/serial.cpp + └── hardware_interfaces/ + ├── arm_control.cpp + ├── battery_management.cpp + ├── rover_drivetrain.cpp + ├── science_module.cpp + ├── status_light.cpp + └── test_hardware.cpp + +urc_localization/ +├── config/ekf_redemption.yaml +├── include/urc_localization/ +│ ├── covariances_on_gps.hpp +│ ├── covariances_on_imu.hpp +│ ├── gps_imu_localizer.hpp +│ └── ground_truth.hpp +├── launch/ekf.launch.py +└── src/ + ├── covariances_on_gps.cpp + ├── covariances_on_imu.cpp + ├── gps_imu_localizer.cpp + └── ground_truth.cpp + +urc_platform/ +├── config/ +│ ├── controller_config.yaml +│ ├── twist_mux.yaml +│ └── vectornav_imu.yaml +├── include/urc_platform/ +│ ├── heartbeat_publisher.hpp +│ ├── imu_ned2enu.hpp +│ ├── joystick_driver.hpp +│ ├── preprocessing.hpp +│ ├── sim_gps_handler.hpp +│ └── twist_mux.hpp +└── src/ + ├── heartbeat_publisher.cpp + ├── imu_ned2enu.cpp + ├── joystick_driver.cpp + ├── sim_gps_handler.cpp + └── twist_mux.cpp +``` + +### Rover description and shared interfaces + +```text +urc_hw_description/ +├── config/joint_limits.yaml +├── launch/display.launch.py +├── meshes/ +├── models/ +├── rviz/display.rviz +├── urdf/simplified_swerve/ +└── world/ + +urc_msgs/ +├── action/NavigateToWaypoint.action +├── msg/ +│ ├── BatteryInfo.msg +│ ├── GridLocation.msg +│ ├── OrientationPoses.msg +│ ├── RoverPoses.msg +│ ├── StatusLightCommand.msg +│ └── Waypoint.msg +└── srv/GeneratePlan.srv + +urc_nanopb/ +└── proto/urc.proto +``` + +### Package moves in this refactor + +| Previous location | New location | +| --- | --- | +| `urc_navigation/grid_map_utils` | `urc_nav_common` | +| `urc_navigation/path_planning` | `urc_path_planning` | +| `urc_navigation/nav_testing` | `urc_state_machine` | +| `urc_navigation/trajectory_following` | `urc_trajectory_following` | +| `urc_perception/src/GaussianFilter.cpp` | `urc_perception/src/gaussian_filter.cpp` | +| `urc_perception/include/GaussianFilter.hpp` | `urc_perception/include/urc_perception/gaussian_filter.hpp` | +| `urc_bringup/src/rocker_effort_pid.cpp` | `urc_controllers/src/rocker_effort_pid.cpp` | +| `urc_bringup/src/rocker_tf_broadcaster.cpp` | `urc_controllers/src/rocker_tf_broadcaster.cpp` | +| `urc_bringup/src/ground_truth.cpp` | `urc_localization/src/ground_truth.cpp` | +| `urc_bringup/src/heartbeat_publisher.cpp` | `urc_platform/src/heartbeat_publisher.cpp` | + +## Setup and development + +- [Native Ubuntu installation](documents/installation/ubuntu_installation.md) +- [Docker installation](documents/installation/docker_installation.md) +- [Navigation architecture](documents/navigation.md) +- [ROS 2 control integration](documents/installation/ros2_control.md) +- [Useful development commands](documents/helpers/useful_commands.md) +- [Common troubleshooting issues](documents/helpers/common_issues.md) + +Run build and test commands from `rover_ws`, not `rover_ws/src`: + +```bash +colcon build --packages-up-to --symlink-install +colcon test --packages-select +colcon test-result --verbose +``` + +Source `install/setup.bash` again after rebuilding. Package manifests are the +source of truth for dependencies; use `rosdep` rather than maintaining a manual +package list. diff --git a/documents/.DS_Store b/documents/.DS_Store new file mode 100644 index 00000000..0bc13842 Binary files /dev/null and b/documents/.DS_Store differ diff --git a/documents/installation/docker_installation.md b/documents/installation/docker_installation.md index fb641a7a..c1172d04 100644 --- a/documents/installation/docker_installation.md +++ b/documents/installation/docker_installation.md @@ -1,222 +1,79 @@ # Docker Installation -Doing a Docker installation is a great method for setting up the repository on any operating system -that isn't Ubuntu 22.04. It is a faster and more lightweight alternative to a traditional Virtual Machine. -In addition, you can still run GUI applications like Gazebo using the NoVNC desktop environment. +The repository's [`setup.sh`](../../setup.sh) script creates the supported +development container. It uses `tiryoh/ros2-desktop-vnc:humble`, names the +container `urc_container`, exposes the NoVNC desktop on port 6060, and mounts the +host project directory at `/home/ubuntu/urc_container`. -## 1. Install Docker +## Prerequisites -[Windows Instructions](https://docs.docker.com/desktop/windows/install/) +Install [Docker Desktop](https://docs.docker.com/desktop/) on macOS or Windows, +or [Docker Engine](https://docs.docker.com/engine/install/ubuntu/) on Ubuntu. +Install Git as well. On Linux, configure Docker so your user can run it without +`sudo`. -[Mac Instructions](https://docs.docker.com/desktop/mac/install/) +## Create the project directory -[Ubuntu Instructions](https://docs.docker.com/engine/install/ubuntu/) - -### NOTE - -- If you are on Linux, add yourself to the `docker` group. Being a member of the `docker` group allows you to run `docker` without `sudo`. - -```bash -sudo groupadd docker -sudo usermod -aG docker $USER -``` - -After you complete the installation, **restart your computer**! - -To check that everything installed OK, you should be able to open the command line and type: - -```bash -docker -``` - -## 2. Install VS Code (Highly Recommended) - -You do not have to use VS Code. However, VS Code has very nice extensions for using Docker containers. - -[Download VS Code here](https://code.visualstudio.com/Download) - -### 2a. Install VS Code Extensions - -Search for and install the following extensions in VS Code - -- Docker -- DevContainers -- ROS -- C/C++ -- CMake - -## 3. Install Git - -[Install Git using the instructions here](https://git-scm.com/book/en/v2/Getting-Started-Installing-Git) - -## 4. Obtain Docker Image - -You can either pull the image from Dockerhub or build the image manually using a Dockerfile. -**Pulling from Dockerhub is faster, easier, and less error-prone**. However, if you want to -edit the Dockerfile for some reason, do the manual build instructions. - -- ### Pull From Dockerhub (Recommended!) - -```bash -docker pull robojackets/urc-gui-baseimage -``` - -- ### [Manually Build Docker Image](manual_docker_image.md) - -## 5. Create directory to mount container - -The docker container is essentially a self-contained instance of Ubuntu 22.04, but can access some files on your system in a limited way. We use this so the container can see the repositories you clone locally, that way both local development and containerized testing can be done without any delays. - -**You can place this directory wherever you want**, I recommend `/home` for Mac/Linux and `C:\Users\[Username]\` for Windows - -```bash -mkdir urc_container -``` - -## 6. [Download our installation script](../../setup.sh) - -Our installation script will - -- Clone all necessary repos -- Automatically setup your desktop NoVNC environment - -## 7. Move the script into your `urc_container` directory - -## 8. Run the script - -**If on Windows, you will need to use Git Bash to run the following commands!** - -Run this command only if you are on Mac/Linux - -```bash -chmod +x setup.sh -``` - -```bash -./setup.sh -``` - -## 9. Access your new container - -For beginners to Docker: - -- Go to `localhost:6060` in your web browser of choice - -Recommended way: - -- Open up VS Code -- Click on the whale Docker icon on your left -- Right click the currently running container (Should be called tiryoh/ros2) -- Select open in browser - -![Picture of where to head in VS Code](../pictures/docker_tab.png) - -## 10. Head to your mounted directory - -Open terminator in the desktop (this is the recommended terminal for commands in your container) - -![You can find terminator in the bottom left corner here](../pictures/terminator_location.png) - -In terminator, run +The setup script manages both rover and drone workspaces. Create their parent +directories, then clone the rover repository: ```bash +mkdir -p urc_container/rover_ws urc_container/drone_ws +git clone --recurse-submodules \ + https://github.com/RoboJackets/urc-software.git \ + urc_container/rover_ws/src cd urc_container ``` -## 11. Get necessary packages +## Start the container -First, it's always a good idea to check for updates. Nothing will happen if you just created the image. However, if you decide to re-create the container a while after you made the initial image, you will need to update those packages. +Run the setup script from `urc_container` so that directory becomes the mounted +project root: ```bash -sudo apt update -sudo apt upgrade +chmod +x rover_ws/src/setup.sh +./rover_ws/src/setup.sh start ``` -```bash -cd /rover_ws -rosdep update && rosdep install --from-paths src --ignore-src -r -y -``` +On first use, the script: -## 12. Build the repo! +- clones the drone repository into `drone_ws/src` +- pulls the ROS 2 Humble NoVNC image +- creates and starts `urc_container` +- installs the container dependencies maintained by the script -Now, it's time for the moment of truth! +Open [http://localhost:6060](http://localhost:6060) for the browser desktop, or +open a shell directly: ```bash -colcon build +docker exec -it urc_container bash ``` -## 13. Developing using the Docker NoVNC container - -1. Work on your code in `urc_container` locally to avoid input lag -2. When ready to test, you can build and run your code in the container just like on native Ubuntu! - -## 14. Closing/Starting the Container - -Once you are done with the conatiner, be sure to close the Docker container. Otherwise, the -Docker container will take up a big chunk of memory on your computer. - -- ### Using VS Code - - Go to the Docker tab in VS Code - - Right click the container (it has a green arrow next to it if already running) - - Press stop/start - - ![Picture of where to head in VSCode](../pictures/docker_tab.png) - -- ### Using the install script - - Go to where you placed `urc_container` locally - - Run `./setup.sh stop` to stop - - Run `./setup.sh start` to start up again - -# Faster, but untested docker method - -The below steps are for a faster docker container. However, there are a few caveats: running graphical - -## 4. Clone the Repository +## Build inside the container -Open a terminal (or Git Bash on Windows), create a folder where you want the repository to be located, navigate to that folder, and run: +From the container shell: ```bash -mkdir rover_ws -cd rover_ws && git clone https://github.com/RoboJackets/urc-software src -``` - -## 5. Pull the docker image - -```bash -docker pull osrf/ros:humble-desktop -``` - -## 6. Run the docker container - -The first time, you will have to navigate to the `docker` folder (located inside `rover_ws/src/docker`) and run the following command: - -```bash -docker compose up +cd /home/ubuntu/urc_container/rover_ws +source /opt/ros/humble/setup.bash +rosdep update +rosdep install --from-paths src --ignore-src -r -y +colcon build --symlink-install +source install/setup.bash ``` -You can open another terminal and run `docker exec -it ros_desktop bash` to access the container. For subsequent launches, it is recommended to use VS Code to connect to the container and develop inside the container. - -## 7. Connect to container using VS Code (Highly Recommended) - -Open VS Code, press `F1`, and select `Dev Containers: Attach to Running Container...`. Then select the container named `ros_desktop`. -You should now be conneceted to the container and can open the folder `/home/rover_ws` to start working on the code. +The source tree is shared with the host, while `build/`, `install/`, and `log/` +are created in the mounted workspace. -## 8. Modify container `.bashrc` +## Stop and restart -To make sure that ROS is sourced every time you open a new terminal in the container, run the following command _inside the docker container_: +Run these commands from the host `urc_container` directory: ```bash -echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc +./rover_ws/src/setup.sh stop +./rover_ws/src/setup.sh start ``` -## 9. Install dependencies and build the code - -Run the following commands _inside the docker container_: - -``` -sudo apt update -rosdep update -cd /home/rover_ws -rosdep install --from-paths src --ignore-src -r -y -colcon build --symlink-install -``` +If the container configuration or mount location must change, update `setup.sh` +and recreate the container deliberately; restarting an existing container does +not apply new `docker run` options. diff --git a/documents/installation/required_packages.md b/documents/installation/required_packages.md index 3b6ddf8d..04a0faf2 100644 --- a/documents/installation/required_packages.md +++ b/documents/installation/required_packages.md @@ -1,14 +1,26 @@ # Required Packages -This is a general list of various packages that we know will be need at some point. Run: `sudo apt-get install` followed by the name of the package for each of these. -Alternatively, use the requirements.bash script in the helper_scripts folder. -# Navigation Packages -sudo apt-get install ros-humble-navigation2 -sudo apt-get install ros-humble-nav2-bringup -sudo apt-get install ros-humble-turtlebot3-gazebo +Package manifests are the source of truth for repository dependencies. Use +`rosdep` instead of maintaining or installing a separate manual list of ROS +packages. +After installing ROS 2 Humble, install the dependency-management and build +tools: -# Control Packages -sudo apt-get install ros-humble-ros2-control -sudo apt-get install ros-humble-ros2-controllers -sudo apt-get install ros-humble-gazebo-ros2-control \ No newline at end of file +```bash +sudo apt update +sudo apt install python3-rosdep python3-colcon-common-extensions +``` + +Initialize `rosdep` once on a new system, then install dependencies from the +workspace root: + +```bash +sudo rosdep init # Skip this line if rosdep is already initialized. +rosdep update +cd rover_ws +rosdep install --from-paths src --ignore-src -r -y +``` + +Run the `rosdep install` command again after pulling changes that modify a +`package.xml`. Docker users should run it inside the development container. diff --git a/documents/installation/ros2_control.md b/documents/installation/ros2_control.md index 14f65116..95b3dfa9 100644 --- a/documents/installation/ros2_control.md +++ b/documents/installation/ros2_control.md @@ -1,47 +1,69 @@ -## Resources -- Intro, use with Gazebo: https://www.youtube.com/watch?v=4QKsDf1c4hc -- Hardware after Simulation: https://www.youtube.com/watch?v=4VVrTCnxvSw - -## Input -- Command Velocity - -### ros2_control Component -Consists of three main components: -- Diff Drive Controller -- Controller Manager (connects the controllers and hardware-abstraction sides of the ros2_control framewor) -- Hardware Interface (are used by ROS control in conjunction with one of the available ROS controllers to send (hardware_interface::RobotHW::write) commands to the hardware and receive (hardware_interface::RobotHW::read) states from the robot's resources (joints, sensors, actuators)) -

-**Diagram:** - -![ROS2_Control_Diagram](https://control.ros.org/master/_images/components_architecture.png "ROS2 Control Diagram") - -- Diff Drive controller converts command velocities into req. motor velocities -- Hardware interface converts abstract wheel velocity into motor hardware commands -- Controller manager links these two together -- Joint state broadcaster uses encoder position state from hardware interface to publish to /joint_states to the robot state publisher (update wheel positions) -- Resource manager is just the bridge between the controller manager and the hardware interface. - -### Action Steps -- We want to use our own hardware interface (can use a common one) -- Install packages - `sudo apt install ros-humble-ros2-control ros-humble-ros2-controllers ros-humble-gazebo-ros2-control` - -### XACRO Configuration -- We need to have two different plugins, one for the real robot and another for gazebo - - -# Instructions for Starting ROS2 Control (for now, requires commenting out the arm version) -1. Start the simulation with `ros2 launch urc_gazebo simulation.launch.py` -2. (Optional) Check the available hardware interfaces with `ros2 control list_hardware_interfaces` (will allow you to see the fake hardware interfaces provided by ROS2 Control to be used on the simulation rover) -3. Start the controller manager for diff_cont: `ros2 run controller_manager spawner diff_cont` to connect between the fake simulation hardware and the controls (keyboard and controller) -4. Start the controller manager for joint_broad: `ros2 run controller_manager spawner joint_broad` (doing the same for the joint broadcaster instead of the diff drive controller) -5. Start the teleop_twist_keyboard node and remap topics: `ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/diff_cont/cmd_vel_unstamped` (should allow you to publish twist messages - angle and velocity - to diff_cont/cmd_vel_unstamped). -6. (Optional) If you want to confirm that messages are being correctly published to `/diff_cont/cmd_vel_unstamped`, then run `ros2 topic echo /diff_cont/cmd_vel_unstamped` -7. Press keys on the keyboard WITHIN the terminal that you started `teleop_twist_keyboard` in order to control the rover in Gazebo - -# Using a Joystick -- Use the same set of instructions, until and including step 4. -- Run `sudo modprobe xpad` for controller setup. -- Run `ros2 launch teleop_twist_joy teleop-launch.py joy_config:='xbox'` to get the controller listener running. -- Then, run `ros2 launch urc_platform joystick.launch.py` to convert /joy messages to /diff_cont/cmd_vel_unstamped -- Use the left joystick to move the rover \ No newline at end of file +# ROS 2 Control Integration + +The rover uses ROS 2 control to connect velocity commands to either Gazebo or +physical hardware. The URDF defines the available command and state interfaces; +controllers claim those interfaces through `controller_manager`. + +## Simulation control path + +```text +/cmd_vel (Twist) + -> urc_controllers/SwerveDriveController + -> wheel velocity and swivel position interfaces + -> gz_ros2_control/GazeboSimSystem + -> simulated rover +``` + +The swerve controller also publishes `/odom`. A joint-state broadcaster publishes +the model state, while the rocker effort controller and `RockerEffortPid` manage +the suspension effort loop. + +Start the supported simulation configuration with: + +```bash +ros2 launch urc_bringup sim.launch.py +``` + +The launch generates the rover description with `use_sim:=true`, starts Gazebo, +and loads these controllers from +[`urc_bringup/config/test_controllers.yaml`](../../urc_bringup/config/test_controllers.yaml): + +- `joint_state_broadcaster` +- `swerve_controller` +- `rocker_effort_controller` by default + +Inspect the running control system with: + +```bash +ros2 control list_controllers +ros2 control list_hardware_interfaces +ros2 topic echo /odom +``` + +The swerve geometry parameters and controller types belong in the controller +YAML. Joint names, interface types, limits, and hardware selection belong in +[`simplified_swerve_ros2_control.xacro`](../../urc_hw_description/urdf/simplified_swerve/simplified_swerve_ros2_control.xacro). + +## Simulation and physical hardware + +The rover xacro selects its hardware backend through `use_sim`: + +| Mode | Hardware plugin | +| --- | --- | +| `use_sim:=true` | `gz_ros2_control/GazeboSimSystem` | +| `use_sim:=false` | `urc_hw/RoverDrivetrain` | + +Only the simulation path currently has a complete bringup launch. The physical +plugin communicates with firmware over UDP and requires verified addresses, +ports, and matching Nanopb firmware. + +The current physical selection is not ready to activate unchanged: the swerve +xacro exposes per-corner wheel and swivel interfaces, while +`RoverDrivetrain` exports left/right wheel interfaces and requires UDP hardware +parameters that the xacro does not provide. Reconcile those contracts and add a +physical controller-manager launch before attempting hardware activation. + +See the [`urc_controllers` guide](../../urc_controllers/README.md), +[`urc_hw` guide](../../urc_hw/README.md), and +[`urc_hw_description` guide](../../urc_hw_description/README.md) for package-level +contracts. diff --git a/documents/navigation.md b/documents/navigation.md new file mode 100644 index 00000000..4ea829d2 --- /dev/null +++ b/documents/navigation.md @@ -0,0 +1,74 @@ +# URC Navigation Architecture + +The rover uses a custom ROS 2 navigation stack built around a rolling Grid Map, +an A* planning service, and a waypoint-following action. It is not a Nav2 planner +plugin. + +## System flow + +```mermaid +flowchart LR + PointCloud[Point cloud] --> Mapper[Traversability mapper] + Localization -->|global odometry| Mapper + Mapper --> Costmap["/costmap"] + Costmap --> Planner[A* planner] + Costmap --> Follower[Waypoint follower] + Waypoint --> Coordinator[NavCoordinator] --> Follower + Follower -->|GeneratePlan| Planner + Planner -->|nav_msgs/Path| Follower + Localization -->|map/base_link TF| Follower + Follower --> Command[Velocity command] +``` + +The interfaces separate route creation from rover motion: + +- `GeneratePlan` takes start and goal poses, runs A*, and returns a + `nav_msgs/Path`. It does not command the rover. +- `NavigateToWaypoint` accepts either a goal or an existing path. For a goal, the + follower requests a plan first; it then tracks the path and publishes velocity + commands. + +## Package responsibilities + +| Package | Responsibility | +| --- | --- | +| [`urc_localization`](../urc_localization/README.md) | Provides global odometry and the `map`, `odom`, and `base_link` frame relationship | +| [`urc_perception`](../urc_perception/README.md) | Converts terrain point clouds into the rolling `/costmap` Grid Map | +| [`urc_state_machine`](../urc_state_machine/README.md) | Converts pose or GPS waypoint inputs into follower action goals and reports navigation state | +| [`urc_path_planning`](../urc_path_planning/README.md) | Creates cost-weighted A* paths through `GeneratePlan` | +| [`urc_trajectory_following`](../urc_trajectory_following/README.md) | Tracks paths, monitors traversal cost, replans when needed, and publishes velocity commands | +| [`urc_nav_common`](../urc_nav_common/README.md) | Provides shared Grid Map lookup behavior for planning and following | + +## Runtime contracts + +- Localization must provide a connected `map`-to-`base_link` TF tree. Point + clouds need a stamped sensor frame that can be transformed into `map`. +- Planner poses, paths, and the costmap use `map` coordinates; request frames are + not transformed by the planner. +- `/costmap` is `grid_map_msgs/msg/GridMap`; planning and following use its + `traversability_inflated` layer. +- A goal-based navigation request requires both the `plan` service and + `navigate_to_waypoint` action server. +- The follower can replan when its tracking point exceeds the configured lethal + cost. During this collision check, missing cost data is currently treated as + traversable. +- Velocity topic and message type must match the downstream command-routing or + controller configuration before operating the rover. + +## Launching + +For simulation with localization, sensors, controllers, and autonomy: + +```bash +ros2 launch urc_bringup sim.launch.py autonomy:=true +``` + +To start only the autonomy nodes: + +```bash +ros2 launch urc_bringup autonomy.launch.py +``` + +The autonomy-only launch expects localization, point-cloud input, TF, and rover +control to already be running. There is currently no complete physical-rover +bringup launch. diff --git a/urc_navigation/navtargets.md b/documents/navtargets.md similarity index 97% rename from urc_navigation/navtargets.md rename to documents/navtargets.md index 83c01bba..2e057546 100644 --- a/urc_navigation/navtargets.md +++ b/documents/navtargets.md @@ -47,4 +47,4 @@ latitude: 38.373089792425596 longitude: 110.71408008906202 altitude: 1.9090430038049817 -difficulty: hard (5/5) \ No newline at end of file +difficulty: hard (5/5) diff --git a/external/.DS_Store b/external/.DS_Store new file mode 100644 index 00000000..0aeffcc2 Binary files /dev/null and b/external/.DS_Store differ diff --git a/setup.sh b/setup.sh old mode 100644 new mode 100755 index 363570ae..c11886ce --- a/setup.sh +++ b/setup.sh @@ -66,19 +66,78 @@ get_cwd() { container_name="urc_container" mount_dir=$(get_cwd) +ros_dependencies=( + python3-colcon-common-extensions + python3-protobuf + python3-rosdep + ros-humble-behaviortree-cpp + ros-humble-controller-interface + ros-humble-diagnostic-updater + ros-humble-effort-controllers + ros-humble-filters + ros-humble-geodesy + ros-humble-geographic-msgs + ros-humble-grid-map-core + ros-humble-grid-map-cv + ros-humble-grid-map-filters + ros-humble-grid-map-msgs + ros-humble-grid-map-pcl + ros-humble-grid-map-ros + ros-humble-grid-map-rviz-plugin + ros-humble-grid-map-visualization + ros-humble-gz-ros2-control + ros-humble-hardware-interface + ros-humble-joint-state-broadcaster + ros-humble-joint-state-publisher + ros-humble-joint-state-publisher-gui + ros-humble-joint-trajectory-controller + ros-humble-librealsense2 + ros-humble-realtime-tools + ros-humble-robot-localization + ros-humble-ros2-control + ros-humble-rosbridge-server + ros-humble-usb-cam +) + +install_ros_dependencies() { + local missing_dependencies=() + + for dependency in "${ros_dependencies[@]}"; do + if ! docker exec "$container_name" dpkg-query --status "$dependency" >/dev/null 2>&1; then + missing_dependencies+=("$dependency") + fi + done + + if [[ ${#missing_dependencies[@]} -eq 0 ]]; then + echo "ROS dependencies are already installed." + return + fi + + echo "Installing missing ROS dependencies..." + docker exec "$container_name" apt-get update + docker exec \ + --env DEBIAN_FRONTEND=noninteractive \ + "$container_name" \ + apt-get install -y "${missing_dependencies[@]}" +} + # Function to start the container start_container() { # Check if the container is already running - if docker ps -q --filter "name=$container_name" | grep -q .; then + if docker ps -q --filter "name=^/${container_name}$" | grep -q .; then echo "Container $container_name is already running." - exit 1 + install_ros_dependencies + return fi # Check if the container already exists - existing_container=$(docker ps -aq --filter name="$container_name") + existing_container=$(docker ps -aq --filter "name=^/${container_name}$") if [[ -n "$existing_container" ]]; then # Start the existing container - docker start "$container_name" >/dev/null 2>&1 + if ! docker start "$container_name" >/dev/null; then + echo "Failed to start container $container_name." + exit 1 + fi echo "Container $container_name started." else # Run the container and mount with additional options on first creation @@ -86,6 +145,8 @@ start_container() { docker run -p 6060:80 --shm-size=512m --security-opt seccomp=unconfined -d --name "$container_name" -v "$mount_dir:/home/ubuntu/urc_container" "$image_name:$image_tag" echo "Container $container_name pulled, created, and started" fi + + install_ros_dependencies } # Function to stop the container @@ -126,4 +187,3 @@ else ;; esac fi - diff --git a/tools/.DS_Store b/tools/.DS_Store new file mode 100644 index 00000000..35f419b2 Binary files /dev/null and b/tools/.DS_Store differ diff --git a/urc_bringup/.DS_Store b/urc_bringup/.DS_Store new file mode 100644 index 00000000..45d16022 Binary files /dev/null and b/urc_bringup/.DS_Store differ diff --git a/urc_bringup/CMakeLists.txt b/urc_bringup/CMakeLists.txt index 4bfd142a..e4f5f31e 100644 --- a/urc_bringup/CMakeLists.txt +++ b/urc_bringup/CMakeLists.txt @@ -3,77 +3,8 @@ project(urc_bringup) include(../cmake/default_settings.cmake) -# find dependencies find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(std_msgs REQUIRED) -find_package(rosbridge_server REQUIRED) -find_package(tf2_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(urc_msgs REQUIRED) -find_package(ros_gz REQUIRED) -find_package(effort_controllers REQUIRED) -include_directories( - include -) - -# Library creation -add_library(${PROJECT_NAME} SHARED - src/heartbeat_publisher.cpp - src/ground_truth.cpp - src/rocker_effort_pid.cpp - src/rocker_tf_broadcaster.cpp -) - -set(dependencies - rclcpp - rclcpp_components - std_msgs - rosbridge_server - sensor_msgs - nav_msgs - tf2_msgs - tf2_ros - urc_msgs - ros_gz - effort_controllers -) - -ament_target_dependencies(${PROJECT_NAME} - ${dependencies} -) - -# Node registration -rclcpp_components_register_node( - ${PROJECT_NAME} - PLUGIN "heartbeat_publisher::HeartbeatPublisher" - EXECUTABLE ${PROJECT_NAME}_HeartbeatPublisher -) - - -rclcpp_components_register_node( - ${PROJECT_NAME} - PLUGIN "urc_bringup::RockerEffortPid" - EXECUTABLE ${PROJECT_NAME}_RockerEffortPid -) - -rclcpp_components_register_node( - ${PROJECT_NAME} - PLUGIN "urc_bringup::RockerTfBroadcaster" - EXECUTABLE ${PROJECT_NAME}_RockerTfBroadcaster -) - -rclcpp_components_register_node( - ${PROJECT_NAME} - PLUGIN "ground_truth::GroundTruth" - EXECUTABLE ${PROJECT_NAME}_GroundTruth -) - -# Install launch files. install( DIRECTORY launch @@ -81,14 +12,6 @@ install( DESTINATION share/${PROJECT_NAME}/ ) -# Install library -install(TARGETS - ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the copyright linker @@ -98,11 +21,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_export_include_directories(msg) - -ament_export_include_directories(include) - -ament_export_libraries(${PROJECT_NAME}) -ament_export_dependencies(${dependencies}) - ament_package() diff --git a/urc_bringup/README.md b/urc_bringup/README.md new file mode 100644 index 00000000..ca2d60a5 --- /dev/null +++ b/urc_bringup/README.md @@ -0,0 +1,35 @@ +# URC Bringup + +`urc_bringup` composes rover subsystems into launchable ROS 2 configurations. +The nodes it starts are implemented and configured by their owning packages. + +## Launching + +| Command | Purpose | +| --- | --- | +| `ros2 launch urc_bringup sim.launch.py` | Rover simulation with localization and controllers | +| `ros2 launch urc_bringup autonomy.launch.py` | Planning, trajectory following, navigation coordination, and traversability mapping | +| `ros2 launch urc_bringup base_station.launch.py` | Joystick control and base-station GNSS | +| `ros2 launch urc_bringup rocker_effort_pid.launch.py` | Standalone rocker effort PID | + +There is currently no single launch file for complete physical-rover bringup. +The base-station launch requires `ublox_dgnss` and the configured receiver. + +## Simulation + +The default simulation uses `marsyard2020.sdf`. Enable the autonomous navigation +stack when needed: + +```bash +ros2 launch urc_bringup sim.launch.py autonomy:=true +``` + +Use `ros2 launch urc_bringup sim.launch.py --show-args` for the complete set of +world, robot, controller, and simulation options. + +## Configuration + +- `config/sim_config.yaml` defines the Gazebo-to-ROS bridges for clock, laser, + IMU, GPS, point-cloud, and ground-truth data. +- `config/test_controllers.yaml` configures the controllers loaded by the + simulation. diff --git a/urc_bringup/launch/autonomy.launch.py b/urc_bringup/launch/autonomy.launch.py index 196b98e4..3c26a5cd 100644 --- a/urc_bringup/launch/autonomy.launch.py +++ b/urc_bringup/launch/autonomy.launch.py @@ -6,28 +6,32 @@ def generate_launch_description(): pkg_urc_perception = get_package_share_directory("urc_perception") - pkg_trajectory_following = get_package_share_directory("trajectory_following") + pkg_trajectory_following = get_package_share_directory("urc_trajectory_following") - traversability_config = os.path.join(pkg_urc_perception, "config", "traversability_params.yaml") - trajectory_config = os.path.join(pkg_trajectory_following, "config", "pure_pursuit.yaml") + traversability_config = os.path.join( + pkg_urc_perception, "config", "traversability_params.yaml" + ) + trajectory_config = os.path.join( + pkg_trajectory_following, "config", "pure_pursuit_config.yaml" + ) state_machine_node = Node( - package="nav_testing", - executable="nav_testing_NavCoordinator", + package="urc_state_machine", + executable="urc_state_machine_NavCoordinator", name="nav_coordinator", output="screen", ) path_planning_node = Node( - package="path_planning", - executable="path_planning_PlannerServer", + package="urc_path_planning", + executable="urc_path_planning_PlannerServer", name="planner_server", output="screen", ) trajectory_following_node = Node( - package="trajectory_following", - executable="trajectory_following_FollowerActionServer", + package="urc_trajectory_following", + executable="urc_trajectory_following_FollowerActionServer", name="follower_action_server", parameters=[trajectory_config], output="screen", diff --git a/urc_bringup/launch/base_station.launch.py b/urc_bringup/launch/base_station.launch.py index afc5714f..b76f56dd 100644 --- a/urc_bringup/launch/base_station.launch.py +++ b/urc_bringup/launch/base_station.launch.py @@ -28,7 +28,7 @@ def generate_launch_description(): parameters=[ PathJoinSubstitution( [ - FindPackageShare("urc_bringup"), + FindPackageShare("urc_platform"), "config/", "controller_config.yaml", ] diff --git a/urc_bringup/launch/bringup.launch.py b/urc_bringup/launch/bringup.launch.py deleted file mode 100644 index e382d91a..00000000 --- a/urc_bringup/launch/bringup.launch.py +++ /dev/null @@ -1,206 +0,0 @@ -import os -from launch.descriptions import executable -from xacro import process_file -import yaml -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node, SetRemap -from launch_ros.substitutions import FindPackageShare -from ament_index_python.packages import get_package_share_directory -from launch_xml.launch_description_sources import XMLLaunchDescriptionSource - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError - return None - - -def generate_launch_description(): - pkg_urc_bringup = get_package_share_directory("urc_bringup") - pkg_urc_platform = get_package_share_directory("urc_platform") - pkg_urc_localization = get_package_share_directory("urc_localization") - pkg_ublox_dgnss = get_package_share_directory("ublox_dgnss") - - controller_config_file_dir = os.path.join( - pkg_urc_bringup, "config", "controller_config.yaml" - ) - twist_mux_config = os.path.join(pkg_urc_platform, "config", "twist_mux.yaml") - use_sim_time = LaunchConfiguration("use_sim_time", default="true") - - xacro_file = os.path.join( - get_package_share_directory("urc_hw_description"), "urdf/walli.xacro" - ) - assert os.path.exists(xacro_file), "urdf path doesnt exist in " + str(xacro_file) - robot_description_config = process_file( - xacro_file, mappings={"use_simulation": "false"} - ) - robot_desc = robot_description_config.toxml() - - autonomy_arg = DeclareLaunchArgument( - "autonomy", - default_value="false", - description="Launch autonomy nodes", - ) - - heartbeat_node = Node( - package="urc_bringup", - executable="urc_bringup_HeartbeatPublisher", - parameters=[{"heartbeatInterval": 1000}], - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[controller_config_file_dir, {"robot_description": robot_desc}], - output="both", - ) - - load_robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - parameters=[ - {"use_sim_time": use_sim_time, "robot_description": robot_desc}, - ], - output="screen", - ) - - load_joint_state_broadcaster = Node( - package="controller_manager", - executable="spawner", - arguments=["-p", controller_config_file_dir, "joint_state_broadcaster"], - ) - - load_drivetrain_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["rover_drivetrain_controller"], - ) - - load_status_light_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["-p", controller_config_file_dir, "status_light_controller"], - ) - - twist_mux_node = Node( - package="urc_platform", - executable="urc_platform_TwistMux", - name="twist_mux", - parameters=[twist_mux_config], - ) - - launch_gps = GroupAction( - actions=[ - SetRemap(src="/rover/fix", dst="/gps/data"), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - pkg_ublox_dgnss, "launch", "ublox_fb+r_rover.launch.py" - ) - ), - launch_arguments={ - "device_serial_string": "rover", - "frame_id": "gps_link", - }.items(), - ), - ] - ) - - imu_ned2enu_node = Node( - package="urc_platform", - executable="urc_platform_ImuNED2ENU", - name="imu_ned2enu", - ) - - vectornav_node = Node( - package="vectornav", - executable="vectornav", - output="screen", - parameters=[os.path.join(pkg_urc_bringup, "config", "vectornav_imu.yaml")], - remappings=[("/vectornav/imu", "/imu/data")], - ) - - vectornav_sensor_msg_node = Node( - package="vectornav", - executable="vn_sensor_msgs", - output="screen", - parameters=[os.path.join(pkg_urc_bringup, "config", "vectornav_imu.yaml")], - ) - - sick_node = Node( - package="sick_scan_xd", - executable="sick_generic_caller", - parameters=[ - { - "hostname": "192.168.1.10", - "scanner_type": "sick_multiscan", - "publish_frame_id": "lidar_link", - "tf_base_frame_id": "lidar_link2", - "publish_laserscan_segment_topic": "scan_segment", - "publish_laserscan_fullframe_topic": "scan_fullframe", - "custom_pointclouds": "cloud_unstructured_fullframe", - "verbose_level": 0, - "cloud_unstructured_fullframe": "coordinateNotation=0 updateMethod=0 echos=0,1,2 layers=1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16 reflectors=0,1 infringed=0,1 rangeFilter=0.05,999,1 topic=/cloud_unstructured_fullframe frameid=lidar_link publish=1", - } - ], - output="screen", - ) - - launch_ekf = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_urc_localization, "launch", "ekf.launch.py") - ) - ) - - gps_imu_localizer_node = Node( - package="urc_localization", - name="gps_imu_localizer", - executable="urc_localization_GpsImuLocalizer", - output="screen", - ) - - rosbridge_server_node = Node( - package="rosbridge_server", - name="rosbridge_server", - executable="rosbridge_websocket.py", - parameters=[{"port": 9090}], - ) - - launch_autonomy = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_urc_bringup, "launch", "autonomy.launch.py") - ), - condition=IfCondition(LaunchConfiguration("autonomy")), - ) - - return LaunchDescription( - [ - autonomy_arg, - control_node, - load_robot_state_publisher, - load_joint_state_broadcaster, - load_drivetrain_controller, - load_status_light_controller, - imu_ned2enu_node, - twist_mux_node, - launch_gps, - rosbridge_server_node, - # launch_ekf, - gps_imu_localizer_node, - vectornav_node, - vectornav_sensor_msg_node, - heartbeat_node, - sick_node, - launch_autonomy, - ] - ) diff --git a/urc_bringup/launch/rocker_effort_pid.launch.py b/urc_bringup/launch/rocker_effort_pid.launch.py index 4492e933..20e8f44c 100644 --- a/urc_bringup/launch/rocker_effort_pid.launch.py +++ b/urc_bringup/launch/rocker_effort_pid.launch.py @@ -5,34 +5,26 @@ def generate_launch_description(): - left_joint = DeclareLaunchArgument( - "left_joint_name", default_value="L_Rocker_Joint" - ) - right_joint = DeclareLaunchArgument( - "right_joint_name", default_value="R_Rocker_Joint" + pitch_topic = DeclareLaunchArgument( + "pitch_topic", default_value="/rocker/pitch_raw" ) cmd_topic = DeclareLaunchArgument( "command_topic", default_value="/rocker_effort_controller/commands" ) - js_topic = DeclareLaunchArgument( - "joint_state_topic", default_value="/joint_states" - ) kp = DeclareLaunchArgument("kp", default_value="200.0") ki = DeclareLaunchArgument("ki", default_value="0.0") kd = DeclareLaunchArgument("kd", default_value="5.0") effort_limit = DeclareLaunchArgument("effort_limit", default_value="1200.0") node = Node( - package="urc_bringup", - executable="urc_bringup_RockerEffortPid", + package="urc_controllers", + executable="urc_controllers_RockerEffortPid", name="rocker_effort_pid", output="screen", parameters=[ { - "left_joint_name": LaunchConfiguration("left_joint_name"), - "right_joint_name": LaunchConfiguration("right_joint_name"), + "pitch_topic": LaunchConfiguration("pitch_topic"), "command_topic": LaunchConfiguration("command_topic"), - "joint_state_topic": LaunchConfiguration("joint_state_topic"), "kp": LaunchConfiguration("kp"), "ki": LaunchConfiguration("ki"), "kd": LaunchConfiguration("kd"), @@ -43,10 +35,8 @@ def generate_launch_description(): return LaunchDescription( [ - left_joint, - right_joint, + pitch_topic, cmd_topic, - js_topic, kp, ki, kd, diff --git a/urc_bringup/launch/sim.launch.py b/urc_bringup/launch/sim.launch.py index f3dc0988..e68ba7eb 100644 --- a/urc_bringup/launch/sim.launch.py +++ b/urc_bringup/launch/sim.launch.py @@ -1,10 +1,20 @@ import os from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler, TimerAction +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + RegisterEventHandler, + TimerAction, +) from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution, PythonExpression +from launch.substitutions import ( + LaunchConfiguration, + Command, + PathJoinSubstitution, + PythonExpression, +) from launch_ros.descriptions import ParameterValue from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node @@ -16,7 +26,9 @@ def generate_launch_description(): path_urc_bringup = get_package_share_directory("urc_bringup") path_urc_localization = get_package_share_directory("urc_localization") - controller_config_file_dir = os.path.join(path_urc_bringup, "config", "test_controllers.yaml") + controller_config_file_dir = os.path.join( + path_urc_bringup, "config", "test_controllers.yaml" + ) sim_world_arg = DeclareLaunchArgument( "world", @@ -34,8 +46,7 @@ def generate_launch_description(): description="Path to xacro file", ) - - bridge_yaml = DeclareLaunchArgument( + bridge_yaml = DeclareLaunchArgument( "bridge_yaml", default_value=os.path.join(path_urc_bringup, "config", "sim_config.yaml"), description="bridge YAML config", @@ -102,8 +113,12 @@ def generate_launch_description(): incline_x = LaunchConfiguration("incline_x") incline_y = LaunchConfiguration("incline_y") incline_slope = LaunchConfiguration("incline_slope") - cube_sdf_path = os.path.join(path_urc_hw_description, "world", "obstacles", "large_cube.sdf") - incline_sdf_path = os.path.join(path_urc_hw_description, "world", "obstacles", "incline_plane.sdf") + cube_sdf_path = os.path.join( + path_urc_hw_description, "world", "obstacles", "large_cube.sdf" + ) + incline_sdf_path = os.path.join( + path_urc_hw_description, "world", "obstacles", "incline_plane.sdf" + ) incline_pitch_rad = PythonExpression( ["-float(", incline_slope, ") * 3.141592653589793 / 180.0"] ) @@ -117,7 +132,9 @@ def generate_launch_description(): # Start Gazebo and immediately run the simulation (-r) gz_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(path_ros_gazebo_sim, "launch", "gz_sim.launch.py")), + PythonLaunchDescriptionSource( + os.path.join(path_ros_gazebo_sim, "launch", "gz_sim.launch.py") + ), launch_arguments={"gz_args": ["-r ", world_path]}.items(), ) @@ -151,9 +168,9 @@ def generate_launch_description(): output="screen", ) - ground_truth = Node( - package="urc_bringup", - executable="urc_bringup_GroundTruth", + ground_truth = Node( + package="urc_localization", + executable="urc_localization_GroundTruth", name="ground_truth", parameters=[ { @@ -193,12 +210,9 @@ def generate_launch_description(): condition=IfCondition(LaunchConfiguration("autonomy")), ) - - - - rocker_tf_broadcaster = Node( - package="urc_bringup", - executable="urc_bringup_RockerTfBroadcaster", + rocker_tf_broadcaster = Node( + package="urc_controllers", + executable="urc_controllers_RockerTfBroadcaster", name="rocker_tf_broadcaster", parameters=[ { @@ -297,9 +311,9 @@ def generate_launch_description(): output="screen", ) - rocker_effort_pid_node = Node( - package="urc_bringup", - executable="urc_bringup_RockerEffortPid", + rocker_effort_pid_node = Node( + package="urc_controllers", + executable="urc_controllers_RockerEffortPid", name="rocker_effort_pid", parameters=[ { @@ -353,7 +367,12 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=spawn, - on_exit=[delayed_load_jsb, delayed_load_swerve, delayed_load_rocker, ground_truth], + on_exit=[ + delayed_load_jsb, + delayed_load_swerve, + delayed_load_rocker, + ground_truth, + ], ) ), ] diff --git a/urc_bringup/launch/sim_leo.py b/urc_bringup/launch/sim_leo.py deleted file mode 100644 index 478b8920..00000000 --- a/urc_bringup/launch/sim_leo.py +++ /dev/null @@ -1,201 +0,0 @@ -import os -from tempfile import NamedTemporaryFile -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, Command -from launch_ros.descriptions import ParameterValue -from ament_index_python.packages import get_package_share_directory -from launch_ros.actions import Node -from xacro import process_file - - -def generate_launch_description(): - path_ros_gazebo_sim = get_package_share_directory("ros_gz_sim") - path_urc_hw_description = get_package_share_directory("urc_hw_description") - path_urc_bringup = get_package_share_directory("urc_bringup") - - # <-- ADDED paths for the Leo Rover packages - path_leo_description = get_package_share_directory("leo_description") - path_leo_gz_bringup = get_package_share_directory("leo_gz_bringup") - - # <-- CHANGED to use the Leo Rover's controller config - controller_config_file_dir = os.path.join( - path_leo_gz_bringup, "config", "controllers.yaml" - ) - - # controller_config_file_dir = os.path.join( - # path_urc_bringup, "config", "test_controllers.yaml" - # ) - - sim_world_arg = DeclareLaunchArgument( - "world", - # default_value=os.path.join(path_urc_hw_description, "world", "leo_world.sdf"), - default_value=os.path.join(path_urc_hw_description, "world", "marsyard2020.sdf"), - description="Path to gz world file", - ) - - # <-- CHANGED default_value to point to the Leo Rover's xacro file - # (I kept your variable name 'walli_xacro' for minimal changes, - # but you could rename it to 'robot_xacro') - walli_xacro = DeclareLaunchArgument( - "walli_xacro", - default_value=os.path.join( - path_leo_description, - "urdf", - "leo.urdf.xacro", - ), - description="Path to xacro file", - ) - # walli_xacro = DeclareLaunchArgument( - # "walli_xacro", - # default_value=os.path.join( - # path_urc_hw_description, - # "urdf/simplified_swerve", - # "simplified_swerve.urdf.xacro", - # ), - # description="Path to xacro file", - # ) - - bridge_yaml = DeclareLaunchArgument( - "bridge_yaml", - default_value=os.path.join(path_urc_bringup, "config", "sim_config.yaml"), - description="bridge YAML config", - ) - - world = LaunchConfiguration("world") - walli_xacro_config = LaunchConfiguration("walli_xacro") - - """ - robot_urdf_file = process_file( - ParameterValue(walli_xacro_config, value_type = str), - mappings = {"use_sim": "true"} - ).toxml() - """ - - robot_urdf_file = ParameterValue( - Command( - [ - "xacro ", - walli_xacro_config, - " use_sim:=", - "true", - ] - ), - value_type=str, - ) - - gz_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(path_ros_gazebo_sim, "launch", "gz_sim.launch.py") - ), - launch_arguments={"gz_args": world}.items(), - ) - - bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="ros_gz_bridge", - output="screen", - parameters=[{"config_file": LaunchConfiguration("bridge_yaml")}], - ) - - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - parameters=[{"robot_description": robot_urdf_file}], - output="screen", - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[controller_config_file_dir], - output="screen", - remappings=[("/controller_manager/robot_description", "/robot_description")], - ) - -# <-- ADDED spawner for the Leo Rover's differential drive controller - load_diff_drive_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["-p", controller_config_file_dir, "diff_drive_controller"], - ) - load_joint_state_broadcaster = Node( - package="controller_manager", - executable="spawner", - arguments=["-p", controller_config_file_dir, "joint_state_broadcaster"], - ) - - # load_position_controller = Node( - # package="controller_manager", - # executable="spawner", - # arguments=["-p", controller_config_file_dir, "position_controller"], - # ) - - # load_velocity_controller = Node( - # package="controller_manager", - # executable="spawner", - # arguments=["-p", controller_config_file_dir, "velocity_controller"], - # ) - - spawn = Node( - package="ros_gz_sim", - executable="create", - output="screen", - arguments=[ - "-name", - "leo", # "walli", - "-x", - "0", - "-y", - "0", - "-z", - "10.5", - "-R", - "0", - "-P", - "0", - "-Y", - "0", - "-topic", - "robot_description", - ], - ) - - return LaunchDescription( - [ - sim_world_arg, - walli_xacro, - gz_sim, - spawn, - bridge_yaml, - bridge, - control_node, - robot_state_publisher_node, - load_joint_state_broadcaster, - load_diff_drive_controller, # <-- ADDED - # <-- REMOVED old controllers - ] - ) - - # return LaunchDescription( - # [ - # sim_world_arg, - # walli_xacro, - # gz_sim, - # spawn, - # bridge_yaml, - # bridge, - # control_node, - # robot_state_publisher_node, - # load_joint_state_broadcaster, - # load_position_controller, - # load_velocity_controller, - # ] - # ) - - - - diff --git a/urc_bringup/package.xml b/urc_bringup/package.xml index 860545c4..35bb2416 100644 --- a/urc_bringup/package.xml +++ b/urc_bringup/package.xml @@ -9,13 +9,15 @@ ament_cmake - rclcpp - rclcpp_components - std_msgs rosbridge_server - sensor_msgs - tf2_msgs urc_msgs + urc_controllers + urc_localization + urc_path_planning + urc_perception + urc_platform + urc_state_machine + urc_trajectory_following ros_gz effort_controllers grid_map_rviz_plugin diff --git a/urc_controllers/.DS_Store b/urc_controllers/.DS_Store new file mode 100644 index 00000000..59cbeabd Binary files /dev/null and b/urc_controllers/.DS_Store differ diff --git a/urc_controllers/CMakeLists.txt b/urc_controllers/CMakeLists.txt index 53e5284a..2d2373dd 100644 --- a/urc_controllers/CMakeLists.txt +++ b/urc_controllers/CMakeLists.txt @@ -12,7 +12,10 @@ find_package(hardware_interface REQUIRED) find_package(realtime_tools REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(urc_hw REQUIRED) find_package(urc_msgs REQUIRED) find_package(urc_nanopb REQUIRED) @@ -30,6 +33,8 @@ add_library( src/status_light_controller.cpp src/bms_broadcaster.cpp src/swerve_drive_controller.cpp + src/rocker_effort_pid.cpp + src/rocker_tf_broadcaster.cpp include/urc_controllers/status_light_controller.hpp include/urc_controllers/bms_broadcaster.hpp include/urc_controllers/swerve_drive_controller.hpp @@ -47,7 +52,10 @@ ament_target_dependencies( realtime_tools pluginlib rclcpp + rclcpp_components rclcpp_lifecycle + sensor_msgs + std_msgs urc_hw urc_nanopb urc_msgs @@ -62,6 +70,18 @@ ament_target_dependencies( target_compile_definitions(urc_controllers PRIVATE "urc_hw_BUILDING_LIBRARY") pluginlib_export_plugin_description_file(controller_interface urc_controllers.xml) +rclcpp_components_register_node( + ${PROJECT_NAME} + PLUGIN "urc_controllers::RockerEffortPid" + EXECUTABLE ${PROJECT_NAME}_RockerEffortPid +) + +rclcpp_components_register_node( + ${PROJECT_NAME} + PLUGIN "urc_controllers::RockerTfBroadcaster" + EXECUTABLE ${PROJECT_NAME}_RockerTfBroadcaster +) + install( DIRECTORY include/ @@ -87,7 +107,10 @@ ament_export_dependencies( hardware_interface pluginlib rclcpp + rclcpp_components rclcpp_lifecycle + sensor_msgs + std_msgs urc_hw urc_nanopb urc_msgs diff --git a/urc_controllers/README.md b/urc_controllers/README.md new file mode 100644 index 00000000..bfe8e461 --- /dev/null +++ b/urc_controllers/README.md @@ -0,0 +1,37 @@ +# URC Controllers + +`urc_controllers` provides rover-specific `ros2_control` plugins and standalone +nodes for the rocker suspension. + +## Components + +| Component | Type | Public behavior | +| --- | --- | --- | +| `urc_controllers/SwerveDriveController` | Controller plugin | Converts `/cmd_vel` (`Twist`) into wheel velocity and swivel position commands; publishes `/odom` | +| `urc_controllers/BMSBroadcaster` | Controller plugin | Reads battery hardware state interfaces and publishes `state_battery` (`BatteryInfo`) | +| `urc_controllers/StatusLightController` | Controller plugin | Converts `/command/status_light` into status-light hardware commands | +| `RockerTfBroadcaster` | ROS component | Averages the rocker joint positions and publishes `/rocker/pitch_raw` | +| `RockerEffortPid` | ROS component | Converts rocker pitch error into paired effort commands | + +The controller plugins are loaded by `controller_manager`; they are not run as +standalone executables. Their plugin names are defined in +`urc_controllers.xml`. + +## Configuration + +- `urc_platform/config/controller_config.yaml` configures the hardware BMS and + status-light controllers. +- `urc_bringup/config/test_controllers.yaml` configures the simulated swerve and + rocker-effort controllers. +- The swerve controller requires `module_x`, `module_y`, and `wheel_radius`, plus + velocity and position interfaces for the `FL`, `FR`, `BL`, and `BR` modules. + +Start the standalone rocker PID through bringup: + +```bash +ros2 launch urc_bringup rocker_effort_pid.launch.py +``` + +Its gains, pitch target, integral clamp, minimum effort, and output effort limit +are ROS parameters. The effort limit is the final saturation applied to both +rocker commands. diff --git a/urc_controllers/controllers.md b/urc_controllers/controllers.md deleted file mode 100644 index e69de29b..00000000 diff --git a/urc_bringup/include/rocker_effort_pid.hpp b/urc_controllers/include/urc_controllers/rocker_effort_pid.hpp similarity index 94% rename from urc_bringup/include/rocker_effort_pid.hpp rename to urc_controllers/include/urc_controllers/rocker_effort_pid.hpp index f2f43509..deaee787 100644 --- a/urc_bringup/include/rocker_effort_pid.hpp +++ b/urc_controllers/include/urc_controllers/rocker_effort_pid.hpp @@ -6,7 +6,7 @@ #include #include -namespace urc_bringup +namespace urc_controllers { class RockerEffortPid : public rclcpp::Node @@ -42,4 +42,4 @@ class RockerEffortPid : public rclcpp::Node rclcpp::Publisher::SharedPtr cmd_pub_; }; -} // namespace urc_bringup +} // namespace urc_controllers diff --git a/urc_bringup/include/rocker_tf_broadcaster.hpp b/urc_controllers/include/urc_controllers/rocker_tf_broadcaster.hpp similarity index 95% rename from urc_bringup/include/rocker_tf_broadcaster.hpp rename to urc_controllers/include/urc_controllers/rocker_tf_broadcaster.hpp index 02a150bf..7cca5db2 100644 --- a/urc_bringup/include/rocker_tf_broadcaster.hpp +++ b/urc_controllers/include/urc_controllers/rocker_tf_broadcaster.hpp @@ -13,7 +13,7 @@ #include #include -namespace urc_bringup +namespace urc_controllers { class RockerTfBroadcaster : public rclcpp::Node @@ -50,4 +50,4 @@ class RockerTfBroadcaster : public rclcpp::Node int right_index_{-1}; }; -} // namespace urc_bringup \ No newline at end of file +} // namespace urc_controllers diff --git a/urc_controllers/package.xml b/urc_controllers/package.xml index 5fe9578c..4044ccc6 100644 --- a/urc_controllers/package.xml +++ b/urc_controllers/package.xml @@ -13,12 +13,18 @@ hardware_interface realtime_tools rclcpp + rclcpp_components rclcpp_lifecycle + sensor_msgs + std_msgs urc_hw urc_nanopb urc_msgs joint_trajectory_controller geometry_msgs + tf2 + tf2_geometry_msgs + tf2_ros ros2_control pluginlib diff --git a/urc_bringup/src/rocker_effort_pid.cpp b/urc_controllers/src/rocker_effort_pid.cpp similarity index 93% rename from urc_bringup/src/rocker_effort_pid.cpp rename to urc_controllers/src/rocker_effort_pid.cpp index f27f0a85..88c1f5e1 100644 --- a/urc_bringup/src/rocker_effort_pid.cpp +++ b/urc_controllers/src/rocker_effort_pid.cpp @@ -1,8 +1,8 @@ -#include "rocker_effort_pid.hpp" +#include "urc_controllers/rocker_effort_pid.hpp" #include -namespace urc_bringup +namespace urc_controllers { RockerEffortPid::RockerEffortPid(const rclcpp::NodeOptions & options) @@ -86,7 +86,7 @@ void RockerEffortPid::onPitch(const std_msgs::msg::Float64::SharedPtr msg) cmd_pub_->publish(cmd); } -} // namespace urc_bringup +} // namespace urc_controllers #include -RCLCPP_COMPONENTS_REGISTER_NODE(urc_bringup::RockerEffortPid) +RCLCPP_COMPONENTS_REGISTER_NODE(urc_controllers::RockerEffortPid) diff --git a/urc_bringup/src/rocker_tf_broadcaster.cpp b/urc_controllers/src/rocker_tf_broadcaster.cpp similarity index 92% rename from urc_bringup/src/rocker_tf_broadcaster.cpp rename to urc_controllers/src/rocker_tf_broadcaster.cpp index 817dc3c4..75bbd8c0 100644 --- a/urc_bringup/src/rocker_tf_broadcaster.cpp +++ b/urc_controllers/src/rocker_tf_broadcaster.cpp @@ -1,9 +1,9 @@ -#include "rocker_tf_broadcaster.hpp" +#include "urc_controllers/rocker_tf_broadcaster.hpp" #include #include -namespace urc_bringup +namespace urc_controllers { RockerTfBroadcaster::RockerTfBroadcaster(const rclcpp::NodeOptions & options) @@ -60,7 +60,7 @@ void RockerTfBroadcaster::onJointState(const sensor_msgs::msg::JointState::Share pitch_pub_->publish(out); } -} // namespace urc_bringup +} // namespace urc_controllers #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(urc_bringup::RockerTfBroadcaster) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(urc_controllers::RockerTfBroadcaster) diff --git a/urc_hw/.DS_Store b/urc_hw/.DS_Store new file mode 100644 index 00000000..64bbc798 Binary files /dev/null and b/urc_hw/.DS_Store differ diff --git a/urc_hw/README.md b/urc_hw/README.md new file mode 100644 index 00000000..6b8e6a65 --- /dev/null +++ b/urc_hw/README.md @@ -0,0 +1,34 @@ +# URC Hardware Interfaces + +`urc_hw` connects `ros2_control` to rover hardware over UDP using nanopb +messages. These plugins are loaded through a robot's `` +description rather than launched as standalone nodes. + +## Plugins + +| Plugin | Type | Hardware contract | +| --- | --- | --- | +| `urc_hw/RoverDrivetrain` | System | Sends left and right wheel velocity commands and receives wheel-speed feedback | +| `urc_hw/StatusLight` | System | Exposes `color` and `state` command interfaces for the rover status light | +| `urc_hw/BatteryManagement` | Sensor | Receives battery telemetry and exposes voltage, charge, current, and temperature state interfaces | + +Only plugins listed in `urc_hw.xml` are exported for runtime use. The current +physical-rover xacro selects `urc_hw/RoverDrivetrain`; simulation uses +`gz_ros2_control/GazeboSimSystem` instead. + +## Configuration + +Hardware parameters belong in the `` section of the robot +description: + +- `RoverDrivetrain` requires `udp_address`, `udp_self_address`, and `udp_port`. +- `StatusLight` requires `udp_address` and `udp_port`. +- `BatteryManagement` requires `udp_self_address` and `udp_self_port`. + +The active drivetrain integration is defined in +`urc_hw_description/urdf/simplified_swerve/simplified_swerve_ros2_control.xacro`. +Its command and state interfaces must match the controllers loaded by +`controller_manager`. + +Do not activate these plugins against physical hardware until the target +address, port, firmware protocol, and controller limits have been verified. diff --git a/urc_hw/hw.md b/urc_hw/hw.md deleted file mode 100644 index e69de29b..00000000 diff --git a/urc_hw_description/.DS_Store b/urc_hw_description/.DS_Store new file mode 100644 index 00000000..9193e989 Binary files /dev/null and b/urc_hw_description/.DS_Store differ diff --git a/urc_hw_description/README.md b/urc_hw_description/README.md new file mode 100644 index 00000000..ecd41590 --- /dev/null +++ b/urc_hw_description/README.md @@ -0,0 +1,38 @@ +# URC Hardware Description + +`urc_hw_description` owns the rover model and its visualization and simulation +assets. The primary model is the simplified swerve-drive xacro at +`urdf/simplified_swerve/simplified_swerve.urdf.xacro`. + +## Visualizing the Rover + +After building and sourcing the workspace, open the model with the joint-state +GUI and RViz: + +```bash +ros2 launch urc_hw_description display.launch.py +``` + +The launch file accepts `urdf_file`, `use_sim`, and `rviz_config_file` +overrides. Use `ros2 launch urc_hw_description display.launch.py --show-args` +for their current defaults. + +## Model Contracts + +The xacro defines the rover links, rocker and swerve joints, sensor frames, and +`ros2_control` interfaces. With `use_sim:=true`, it selects +`gz_ros2_control/GazeboSimSystem`; otherwise it selects +`urc_hw/RoverDrivetrain`. + +Joint names, frame names, limits, dimensions, and units are shared contracts +with controllers, localization, and launch files. Update those consumers with +any model change. + +## Simulation Assets + +- `world/` contains the empty and Mars Yard Gazebo worlds. +- `models/` contains the installed rover and terrain meshes. +- `rviz/display.rviz` is the default visualization configuration. +- Sensor xacros define the simulated lidar, IMU, and GPS. + +Use `ros2 launch urc_bringup sim.launch.py` for the composed rover simulation. diff --git a/urc_hw_description/hw_description.md b/urc_hw_description/hw_description.md deleted file mode 100644 index e69de29b..00000000 diff --git a/urc_localization/.DS_Store b/urc_localization/.DS_Store new file mode 100644 index 00000000..1f2fc0f4 Binary files /dev/null and b/urc_localization/.DS_Store differ diff --git a/urc_localization/CMakeLists.txt b/urc_localization/CMakeLists.txt index 2cd28bbb..28dfd069 100644 --- a/urc_localization/CMakeLists.txt +++ b/urc_localization/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(geographic_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) +find_package(tf2_msgs REQUIRED) find_package(laser_geometry REQUIRED) include_directories( @@ -27,6 +28,7 @@ add_library(${PROJECT_NAME} SHARED src/gps_imu_localizer.cpp src/covariances_on_imu.cpp src/covariances_on_gps.cpp + src/ground_truth.cpp ) set(dependencies @@ -40,8 +42,9 @@ set(dependencies geometry_msgs tf2 tf2_ros + tf2_msgs laser_geometry - + ) ament_target_dependencies(${PROJECT_NAME} @@ -67,6 +70,12 @@ rclcpp_components_register_node( EXECUTABLE ${PROJECT_NAME}_CovariancesOnGps ) +rclcpp_components_register_node( + ${PROJECT_NAME} + PLUGIN "urc_localization::GroundTruth" + EXECUTABLE ${PROJECT_NAME}_GroundTruth +) + # Install launch files. install( DIRECTORY @@ -75,6 +84,11 @@ install( DESTINATION share/${PROJECT_NAME}/ ) +install( + DIRECTORY include/ + DESTINATION include +) + install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION lib diff --git a/urc_localization/README.md b/urc_localization/README.md new file mode 100644 index 00000000..c57f789f --- /dev/null +++ b/urc_localization/README.md @@ -0,0 +1,51 @@ +# URC Localization + +`urc_localization` prepares sensor data and estimates rover pose. It combines +package-specific ROS 2 components with the `robot_localization` EKF and NavSat +nodes. + +## Localization pipeline + +[`launch/ekf.launch.py`](launch/ekf.launch.py) starts three nodes: + +1. A local EKF fuses `/odom` and `/imu/fused`, publishes + `/odometry/filtered`, and owns `odom -> base_link`. +2. `navsat_transform_node` combines `/gps/covariances`, `/imu/fused`, and the + global estimate to publish `/odometry/gps`. +3. A global EKF fuses GPS position with the local estimate, publishes + `/odometry/filtered_global`, and owns `map -> odom`. + +The filter inputs, frames, fused state variables, and noise settings are defined +in [`config/ekf_redemption.yaml`](config/ekf_redemption.yaml). + +## Package components + +| Executable | Responsibility | +| --- | --- | +| `urc_localization_CovariancesOnImu` | Restamps IMU data and assigns the covariance and `imu_link` frame expected by the filters | +| `urc_localization_CovariancesOnGps` | Restamps GPS fixes and assigns the covariance and `gps_link` frame expected by NavSat | +| `urc_localization_GroundTruth` | Converts a simulation transform into planar ground-truth odometry | +| `urc_localization_GpsImuLocalizer` | Provides a simpler GPS/IMU map-pose estimator outside the EKF pipeline | + +These executables are also registered as composable ROS 2 components. Topic +names are parameters; inspect the component source or use `ros2 param describe` +for the complete interface. + +## Usage + +The full simulation launch starts the covariance adapters, ground-truth adapter, +and EKF pipeline: + +```bash +ros2 launch urc_bringup sim.launch.py +``` + +To start only the filter pipeline: + +```bash +ros2 launch urc_localization ekf.launch.py +``` + +The standalone EKF launch expects its sensor topics to already be available. +Its current configuration enables simulation time, planar estimation, and fixed +sensor covariance assumptions; review those settings before physical-rover use. diff --git a/urc_localization/config/ekf.yaml b/urc_localization/config/ekf.yaml deleted file mode 100644 index ff541dc3..00000000 --- a/urc_localization/config/ekf.yaml +++ /dev/null @@ -1,183 +0,0 @@ -ekf_filter_node_odom: - ros__parameters: - frequency: 30.0 - sensor_timeout: 5.0 - two_d_mode: true - transform_time_offset: 0.0 - transform_timeout: 0.0 - print_diagnostics: true - debug: false - - map_frame: map - odom_frame: odom - base_link_frame: base_link - world_frame: odom - - imu0: /imu/data - imu0_config: [false, false, false, - false, false, true, - false, false, false, - false, false, true, - true, false, false] - imu0_differential: false - imu0_relative: false - imu0_queue_size: 10 - imu_remove_gravitational_acceleration: false - - odom0: /rover_drivetrain_controller/odom - odom0_config: [true, true, false, - false, false, true, - true, true, false, - false, false, true, - false, false, false] - odom0_differential: true - odom0_relative: false - odom0_queue_size: 10 - - use_control: false - # process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] - # - # - # initial_estimate_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] - dynamic_process_noise_covariance: true - print_diagnostics: true - - -ekf_filter_node_map: - ros__parameters: - #Frequency the filter outputs a position estimate - frequency: 30.0 - #Period in seconds after which we consider a sensor to have timed out - sensor_timeout: 5.0 - - # 2D vs 3D data - two_d_mode: true - - #Transform time offset - transform_time_offset: 0.0 - - #How long tf-listener waits for transform to become available - transform_timeout: 0.0 - - publish_acceleration: true - - #Publish transformation over tf topic - publish_tf: true - - - map_frame: map - odom_frame: odom - base_link_frame: base_link - world_frame: map - - - odom0: /odometry/gps - odom0_config: [true, true, false, - false, false, false, - false, false, false, - false, false, false, - false, false, false] - odom0_queue_size: 10 - odom0_differential: false - odom0_relative: false - - odom1: /rover_drivetrain_controller/odom - odom1_config: [true, true, false, - false, false, true, - true, true, false, - false, false, false, - false, false, false] - odom1_differential: false - odom1_relative: true - odom1_queue_size: 10 - - imu0: /imu/data - imu0_config: [false, false, false, - false, false, true, - false, false, false, - false, false, true, - true, false, false] - imu0_differential: false - imu0_relative: false - imu0_queue_size: 10 - imu_remove_gravitational_acceleration: false - - use_control: false - # process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] - # initial_estimate_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, - # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] - # - dynamic_process_noise_covariance: true - print_diagnostics: true - - - -navsat_transform: - ros__parameters: - # Frequency of the main run loop - frequency: 30.0 - # Delay time, in seconds, before we calculate the transform from the UTM frame to your world frame. - delay: 2.0 - magnetic_declination_radians: 0.06736971 - yaw_offset: 0.0 - zero_altitude: true - broadcast_utm_transform: true - publish_filtered_gps: true - use_odometry_yaw: false - wait_for_datum: false - diff --git a/urc_localization/include/covariances_on_gps.hpp b/urc_localization/include/urc_localization/covariances_on_gps.hpp similarity index 89% rename from urc_localization/include/covariances_on_gps.hpp rename to urc_localization/include/urc_localization/covariances_on_gps.hpp index b7ab3c56..d14ed599 100644 --- a/urc_localization/include/covariances_on_gps.hpp +++ b/urc_localization/include/urc_localization/covariances_on_gps.hpp @@ -10,7 +10,7 @@ namespace covariances_on_gps class CovariancesOnGps : public rclcpp::Node { public: - explicit CovariancesOnGps(const rclcpp::NodeOptions &options); + explicit CovariancesOnGps(const rclcpp::NodeOptions & options); private: void handleGps(const sensor_msgs::msg::NavSatFix::SharedPtr msg); @@ -21,4 +21,3 @@ class CovariancesOnGps : public rclcpp::Node } // namespace covariances_on_gps #endif // COVARIANCES_ON_GPS_HPP - diff --git a/urc_localization/include/covariances_on_imu.hpp b/urc_localization/include/urc_localization/covariances_on_imu.hpp similarity index 88% rename from urc_localization/include/covariances_on_imu.hpp rename to urc_localization/include/urc_localization/covariances_on_imu.hpp index 57cc97fb..76cbd59a 100644 --- a/urc_localization/include/covariances_on_imu.hpp +++ b/urc_localization/include/urc_localization/covariances_on_imu.hpp @@ -10,7 +10,7 @@ namespace covariances_on_imu class CovariancesOnImu : public rclcpp::Node { public: - explicit CovariancesOnImu(const rclcpp::NodeOptions &options); + explicit CovariancesOnImu(const rclcpp::NodeOptions & options); private: void handleImu(const sensor_msgs::msg::Imu::SharedPtr msg); @@ -21,4 +21,3 @@ class CovariancesOnImu : public rclcpp::Node } // namespace covariances_on_imu #endif // COVARIANCES_ON_IMU_HPP - diff --git a/urc_localization/include/gps_imu_localizer.hpp b/urc_localization/include/urc_localization/gps_imu_localizer.hpp similarity index 100% rename from urc_localization/include/gps_imu_localizer.hpp rename to urc_localization/include/urc_localization/gps_imu_localizer.hpp diff --git a/urc_bringup/include/ground_truth.hpp b/urc_localization/include/urc_localization/ground_truth.hpp similarity index 91% rename from urc_bringup/include/ground_truth.hpp rename to urc_localization/include/urc_localization/ground_truth.hpp index 04607261..93c4b325 100644 --- a/urc_bringup/include/ground_truth.hpp +++ b/urc_localization/include/urc_localization/ground_truth.hpp @@ -7,7 +7,7 @@ #include -namespace ground_truth +namespace urc_localization { class GroundTruth : public rclcpp::Node @@ -26,6 +26,6 @@ class GroundTruth : public rclcpp::Node std::string base_frame_id_; }; -} // namespace ground_truth +} // namespace urc_localization #endif // GROUND_TRUTH_HPP_ diff --git a/urc_localization/launch/ekf.launch.py b/urc_localization/launch/ekf.launch.py index 47d7797d..67a8e199 100644 --- a/urc_localization/launch/ekf.launch.py +++ b/urc_localization/launch/ekf.launch.py @@ -3,6 +3,7 @@ import launch_ros.actions import os + def generate_launch_description(): params = os.path.join( get_package_share_directory("urc_localization"), diff --git a/urc_localization/localization.md b/urc_localization/localization.md deleted file mode 100644 index 8b1ae882..00000000 --- a/urc_localization/localization.md +++ /dev/null @@ -1,86 +0,0 @@ -# urc_localization - -## Overview - -The `urc_localization` package provides configuration and launch files for rover state estimation using the **Extended Kalman Filter (EKF)** from the `robot_localization` package. - -It fuses IMU, odometry, and (optionally) GPS inputs to estimate the rover’s position, orientation, and velocity in a consistent world frame. - -This system includes the following modules: - -1. **EKF Configuration** - - YAML configuration defining sensor inputs, noise models, and state estimation parameters. - - **Key features**: Flexible tuning for rover-specific sensors. - -2. **EKF Launch** - - Launch file for starting the `ekf_node` with the provided configuration. - - **Key features**: Quick deployment of localization with minimal setup. - ---- - -## Features - -- **Sensor Fusion**: Combines IMU, odometry, and GPS into a unified state estimate. -- **EKF Tuning**: Parameters can be adjusted to account for rover-specific dynamics. -- **ROS 2 Integration**: Built on top of `robot_localization`’s `ekf_node`. -- **Launch Support**: Simple launch file to start the EKF with provided config. - - -## Package Structure - -``` -├── CMakeLists.txt -├── config -│   └── ekf.yaml -├── launch -│   └── ekf.launch.py -└── package.xml -``` - - -## Components - -### EKF Configuration - -- **ekf.yaml** - - Defines state estimation parameters for the rover. - - **Inputs**: IMU, odometry, and optional GPS topics. - - **Outputs**: Filtered state estimate (`/odometry/filtered`, `/tf`). - -### EKF Launch - -- **ekf.launch.py** - - Starts the `robot_localization/ekf_node` with the parameters in `ekf.yaml`. - - Provides a ready-to-use entry point for rover localization. - ---- - -## ekf_node | Node - -*(from `robot_localization`)* - -### Subscriptions -- `/imu/data` (`sensor_msgs/Imu`) – Raw IMU orientation, angular velocity, and acceleration. -- `/odom/wheel` (`nav_msgs/Odometry`) – Odometry from wheel encoders. -- `/gps/fix` (`sensor_msgs/NavSatFix`) – (Optional) GPS position fix. - -### Publishers -- `/odometry/filtered` (`nav_msgs/Odometry`) – EKF-fused odometry. -- `/tf` (`tf2_msgs/TFMessage`) – Transform tree updates for rover pose. - -### Parameters (from `ekf.yaml`) -- `frequency` (`double`) – Filter update rate in Hz. -- `sensor_timeout` (`double`) – Timeout for dropping sensor data. -- `two_d_mode` (`bool`) – Restrict estimation to planar (x, y, yaw) if true. -- `odom0`, `imu0`, `gps0` (`string`) – Input topics for each sensor. -- `odom0_config`, `imu0_config`, `gps0_config` (`array`) – Which variables from each sensor to fuse. - ---- - -## Launch - -Start the localization system with: - -```bash -ros2 launch urc_localization ekf.launch.py -``` diff --git a/urc_localization/package.xml b/urc_localization/package.xml index 76f06098..6650d039 100644 --- a/urc_localization/package.xml +++ b/urc_localization/package.xml @@ -17,6 +17,7 @@ geodesy geographic_msgs tf2_ros + tf2_msgs geometry_msgs tf2 laser_geometry diff --git a/urc_localization/src/covariances_on_gps.cpp b/urc_localization/src/covariances_on_gps.cpp index d143b6d3..b0a57651 100644 --- a/urc_localization/src/covariances_on_gps.cpp +++ b/urc_localization/src/covariances_on_gps.cpp @@ -1,4 +1,4 @@ -#include "covariances_on_gps.hpp" +#include "urc_localization/covariances_on_gps.hpp" #include #include @@ -8,26 +8,27 @@ namespace covariances_on_gps { -CovariancesOnGps::CovariancesOnGps(const rclcpp::NodeOptions &options) - : rclcpp::Node("covariances_on_gps", options) +CovariancesOnGps::CovariancesOnGps(const rclcpp::NodeOptions & options) +: rclcpp::Node("covariances_on_gps", options) { const auto gps_input_topic = declare_parameter("gps_input_topic", "/gps"); - const auto gps_output_topic = declare_parameter("gps_output_topic", "/gps/covariances"); + const auto gps_output_topic = declare_parameter( + "gps_output_topic", + "/gps/covariances"); gps_publisher_ = create_publisher( - gps_output_topic, - rclcpp::SystemDefaultsQoS()); + gps_output_topic, + rclcpp::SystemDefaultsQoS()); gps_subscription_ = create_subscription( - gps_input_topic, - rclcpp::SystemDefaultsQoS(), - std::bind(&CovariancesOnGps::handleGps, this, std::placeholders::_1)); + gps_input_topic, + rclcpp::SystemDefaultsQoS(), + std::bind(&CovariancesOnGps::handleGps, this, std::placeholders::_1)); } void CovariancesOnGps::handleGps(const sensor_msgs::msg::NavSatFix::SharedPtr msg) { - if (!gps_publisher_) - { + if (!gps_publisher_) { return; } @@ -36,7 +37,7 @@ void CovariancesOnGps::handleGps(const sensor_msgs::msg::NavSatFix::SharedPtr ms output.header.stamp = this->get_clock()->now(); output.header.frame_id = "gps_link"; - output.position_covariance.fill(0.0); + output.position_covariance.fill(0.0); output.position_covariance[0] = 0.15 * 0.15; // x variance (m^2) output.position_covariance[4] = 0.3 * 0.3; // y variance (m^2) diff --git a/urc_localization/src/covariances_on_imu.cpp b/urc_localization/src/covariances_on_imu.cpp index fc7df3d3..c6d7b239 100644 --- a/urc_localization/src/covariances_on_imu.cpp +++ b/urc_localization/src/covariances_on_imu.cpp @@ -1,4 +1,4 @@ -#include "covariances_on_imu.hpp" +#include "urc_localization/covariances_on_imu.hpp" #include #include @@ -8,26 +8,25 @@ namespace covariances_on_imu { -CovariancesOnImu::CovariancesOnImu(const rclcpp::NodeOptions &options) - : rclcpp::Node("covariances_on_imu", options) +CovariancesOnImu::CovariancesOnImu(const rclcpp::NodeOptions & options) +: rclcpp::Node("covariances_on_imu", options) { const auto imu_input_topic = declare_parameter("imu_input_topic", "/imu/data_raw"); const auto imu_output_topic = declare_parameter("imu_output_topic", "/imu/fused"); imu_publisher_ = create_publisher( - imu_output_topic, - rclcpp::SystemDefaultsQoS()); + imu_output_topic, + rclcpp::SystemDefaultsQoS()); imu_subscription_ = create_subscription( - imu_input_topic, - rclcpp::SystemDefaultsQoS(), - std::bind(&CovariancesOnImu::handleImu, this, std::placeholders::_1)); + imu_input_topic, + rclcpp::SystemDefaultsQoS(), + std::bind(&CovariancesOnImu::handleImu, this, std::placeholders::_1)); } void CovariancesOnImu::handleImu(const sensor_msgs::msg::Imu::SharedPtr msg) { - if (!imu_publisher_) - { + if (!imu_publisher_) { return; } @@ -51,7 +50,7 @@ void CovariancesOnImu::handleImu(const sensor_msgs::msg::Imu::SharedPtr msg) output.linear_acceleration_covariance[0] = 1e6; // x variance output.linear_acceleration_covariance[4] = 1e6; // y output.linear_acceleration_covariance[8] = 1e6; // z - + imu_publisher_->publish(output); } diff --git a/urc_localization/src/gps_imu_localizer.cpp b/urc_localization/src/gps_imu_localizer.cpp index 2275978b..c579821f 100644 --- a/urc_localization/src/gps_imu_localizer.cpp +++ b/urc_localization/src/gps_imu_localizer.cpp @@ -1,4 +1,4 @@ -#include "gps_imu_localizer.hpp" +#include "urc_localization/gps_imu_localizer.hpp" namespace gps_imu_localizer { @@ -24,7 +24,7 @@ GpsImuLocalizer::GpsImuLocalizer(const rclcpp::NodeOptions & options) set_base_subscriber_ = create_subscription( get_parameter("set_base_topic").as_string(), rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::Empty::SharedPtr msg) { + [this](const std_msgs::msg::Empty::SharedPtr) { base.first = odometry_msg_.pose.pose.position.x; base.second = odometry_msg_.pose.pose.position.y; } diff --git a/urc_bringup/src/ground_truth.cpp b/urc_localization/src/ground_truth.cpp similarity index 89% rename from urc_bringup/src/ground_truth.cpp rename to urc_localization/src/ground_truth.cpp index ac0aef8e..87398642 100644 --- a/urc_bringup/src/ground_truth.cpp +++ b/urc_localization/src/ground_truth.cpp @@ -1,4 +1,4 @@ -#include "ground_truth.hpp" +#include "urc_localization/ground_truth.hpp" #include #include @@ -6,11 +6,11 @@ #include #include -namespace ground_truth +namespace urc_localization { GroundTruth::GroundTruth(const rclcpp::NodeOptions & options) - : rclcpp::Node("ground_truth", options) +: rclcpp::Node("ground_truth", options) { const auto tf_topic = declare_parameter("tf_topic", "/ground_truth_pose"); const auto odom_topic = @@ -31,8 +31,7 @@ GroundTruth::GroundTruth(const rclcpp::NodeOptions & options) void GroundTruth::handleTransforms(const tf2_msgs::msg::TFMessage::SharedPtr msg) const { - if (!odom_publisher_ || msg->transforms.empty()) - { + if (!odom_publisher_ || msg->transforms.empty()) { return; } @@ -77,7 +76,7 @@ void GroundTruth::handleTransforms(const tf2_msgs::msg::TFMessage::SharedPtr msg odom_publisher_->publish(std::move(odom_msg)); } -} // namespace ground_truth +} // namespace urc_localization #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(ground_truth::GroundTruth) +RCLCPP_COMPONENTS_REGISTER_NODE(urc_localization::GroundTruth) diff --git a/urc_msgs/.DS_Store b/urc_msgs/.DS_Store new file mode 100644 index 00000000..0dc197e3 Binary files /dev/null and b/urc_msgs/.DS_Store differ diff --git a/urc_msgs/README.md b/urc_msgs/README.md index 33d657ae..7b5ad6e0 100644 --- a/urc_msgs/README.md +++ b/urc_msgs/README.md @@ -1,6 +1,32 @@ -## URC Messages +# URC Interfaces -A collection of custom ROS messages. Generated using ROS IDL, and exported with this package for use throughout the software stack. +`urc_msgs` defines the custom ROS 2 interfaces shared across rover packages. It +contains no runtime nodes. -These include... -- Velocity pairs \ No newline at end of file +## Messages + +| Interface | Purpose | +| --- | --- | +| `BatteryInfo` | Battery voltage, charge, current, temperature, and per-cell telemetry | +| `GridLocation` | Unsigned grid-cell coordinates | +| `OrientationPoses` | Header and named pose collection | +| `RoverPoses` | Header and named rover-pose collection | +| `StatusLightCommand` | Status-light color and off/on/blink state | +| `Waypoint` | Latitude and longitude for a navigation target | + +## Service and Action + +| Interface | Purpose | +| --- | --- | +| `GeneratePlan` service | Requests a path between start and goal poses and returns a `nav_msgs/Path` with a success or failure code | +| `NavigateToWaypoint` action | Follows a supplied path or plans to a goal, with optional final-heading enforcement and progress feedback | + +The definitions under `msg/`, `srv/`, and `action/` are authoritative for field +types, constants, and result codes. Inspect an installed interface with: + +```bash +ros2 interface show urc_msgs/action/NavigateToWaypoint +``` + +Changes to these files affect every producer and consumer. Rebuild the workspace +and update all dependent packages when an interface changes. diff --git a/urc_msgs/msgs.md b/urc_msgs/msgs.md deleted file mode 100644 index 2a6108ab..00000000 --- a/urc_msgs/msgs.md +++ /dev/null @@ -1,131 +0,0 @@ -# urc_msgs - -## Overview - -The `urc_msgs` package provides ROS 2 message (`msg`) and service (`srv`) definitions for rover perception, navigation, battery monitoring, and subsystem control. - -This system includes the following modules: - -### Aruco & Navigation Messages -- Defines messages for ArUco marker detection, localization, and navigation status. -- **Key features:** Marker IDs, positions, angles, and navigation messages for rover perception and guidance. - -### Battery & Status Messages -- Defines messages for battery monitoring and status lights. -- **Key features:** Reports cell voltages, charge, current, temperature, and status light states. - -### Motion & Waypoint Messages -- Defines messages for wheel velocities, waypoints, and grid locations. -- **Key features:** Supports motion commands and mapping for autonomous navigation. - -### Services -- Provides service definitions for path planning and behavior tree updates. -- **Key features:** Request/response patterns for planning and behavior management. - ---- - -## Features - -- **Aruco Marker Detection:** Detect and locate markers in rover environment with camera identification. -- **Battery Monitoring:** Access detailed battery status including voltages, charge, and temperature. -- **Navigation Support:** Provides waypoints, grid, and landing location messages for navigation. -- **Status Control:** Supports RGB LED control for rover status feedback. -- **Path Planning Services:** Generate navigation plans between start and goal poses. -- **Behavior Tree Updates:** Dynamically update behavior trees for rover autonomy. - ---- - -## Package Structure - -``` -├── action -├── CMakeLists.txt -├── msg -│   ├── ArucoDetection.msg -│   ├── ArucoLocation.msg -│   ├── BatteryInfo.msg -│   ├── GridLocation.msg -│   ├── LandingLocations.msg -│   ├── NavigationStatus.msg -│   ├── StatusLightCommand.msg -│   ├── VelocityPair.msg -│   └── Waypoint.msg -├── package.xml -├── README.md -└── srv - ├── GeneratePlan.srv - └── UpdateBehaviorTree.srv -``` - -## Components - -### Aruco & Navigation Messages - -- **ArucoDetection (`.msg`)** - - Provides detected marker ID, x/y angles, distance, and camera source. - - **Inputs:** Camera detection - - **Outputs:** Serialized ROS 2 message - -- **ArucoLocation (`.msg`)** - - Provides marker longitude, latitude, ID, and camera source. - - **Inputs:** Marker detection - - **Outputs:** Localization data - -- **NavigationStatus (`.msg`)** - - Publishes status messages related to navigation. - - **Outputs:** Human-readable string message - -- **LandingLocation (`.msg`)** - - Provides multiple possible landing site coordinates. - - **Outputs:** Up to 100 latitude/longitude pairs - -- **GridLocation (`.msg`)** - - Provides X/Y location on a grid map. - - **Outputs:** Grid coordinates - ---- - -### Battery & Status Messages - -- **BatteryInfo (`.msg`)** - - Reports voltage, charge, current, temperature, and all cell voltages. - - **Outputs:** Battery monitoring data - -- **StatusLightCommand (`.msg`)** - - Controls RGB LED lights on the rover with OFF, ON, or BLINK states. - - **Inputs:** Desired color and state - - **Outputs:** Light control command - ---- - -### Motion & Waypoint Messages - -- **VelocityPair (`.msg`)** - - Provides left/right wheel velocities and duration for motion commands. - - **Inputs:** Desired wheel velocities - - **Outputs:** Serialized ROS 2 message - -- **Waypoint (`.msg`)** - - Defines a GPS waypoint with latitude and longitude. - - **Outputs:** Navigation waypoint - ---- - -### Services - -- **GeneratePlan (`.srv`)** - - **Request:** Start and goal poses (`geometry_msgs/PoseStamped`) - - **Response:** Navigation path (`nav_msgs/Path`) and error code (`SUCCESS`/`FAILURE`) - -- **UpdateBehaviorTree (`.srv`)** - - **Request:** Behavior tree content, directory, and `use_dir` flag - - **Response:** Success boolean - ---- - -## Notes - -This package provides **message and service definitions only**; it does not include runtime nodes. - -Generated headers (`.hpp`) and source files (`.cpp`) from these messages/services are used by ROS 2 nodes for communication. - diff --git a/urc_nanopb/README.md b/urc_nanopb/README.md index 26d183dc..a9a7c646 100644 --- a/urc_nanopb/README.md +++ b/urc_nanopb/README.md @@ -1,11 +1,37 @@ -## URC Nanopb +# URC Nanopb -[Nanopb](https://github.com/nanopb/nanopb) is a Protocol Buffers implementation designed to efficiently package/encapsulate messages for communication with the [urc_firmware stack](https://github.com/RoboJackets/urc-firmware). It is a submodule of this repo, and the .proto messages it will use are defined in the /proto directory of this package. +`urc_nanopb` provides the lightweight Protocol Buffers types used to communicate +between rover software and microcontroller firmware. It exports a library for +other ROS packages and has no runtime nodes. +## Build contract -This package... -1. Uses nanopb to convert the .proto files into cpp/hpp files -2. Creates a library with those files and any other necessary nanopb files -3. Makes all those files accessible in the `urc_nanopb` package +[`proto/urc.proto`](proto/urc.proto) is the authoritative rover protocol schema. +During a workspace build, CMake uses the vendored Nanopb generator to create +`urc.pb.h` and `urc.pb.c`, compiles them into a shared library, and installs the +generated header as `urc_nanopb/urc.pb.h`. +Consumers declare a dependency on `urc_nanopb`, link its exported library, and +include the generated header: + +```cpp +#include +``` + +## Protocol areas + +The schema includes messages for drivetrain commands and feedback, status-light +commands, battery telemetry, arm control, IMU data, and science-module control. +The drivetrain, status-light, and battery interfaces are used by `urc_hw`. + +## Changing the protocol + +- Update `urc.proto` and the corresponding firmware together. +- Preserve field numbers and wire compatibility; do not reuse removed field + numbers for different data. +- Rebuild the workspace after schema changes. Generated Nanopb files are build + outputs and should not be edited or committed. + +The matching microcontroller implementation lives in the +[URC firmware repository](https://github.com/RoboJackets/urc-firmware). diff --git a/urc_nanopb/nanopob.md b/urc_nanopb/nanopob.md deleted file mode 100644 index 4cf9df9b..00000000 --- a/urc_nanopb/nanopob.md +++ /dev/null @@ -1,86 +0,0 @@ -# urc_nanopb - -## Overview - -The `urc_nanopb` package provides **Protocol Buffers (Nanopb) message definitions** for rover communication between ROS 2 nodes and microcontrollers. - -This system includes the following modules: - -1. **Arm & Drivetrain Messages** - - Defines encoder, setpoint, and feedback messages for rover arms and drivetrain. - - Supports structured, serialized communication with microcontrollers. - -2. **Sensor & Status Messages** - - Encodes IMU, battery, and status light messages. - - Facilitates efficient monitoring and control of rover subsystems. - ---- - -## Features - -- **Nanopb Serialization**: Lightweight C structs for microcontroller-friendly messaging. -- **Cross-Module Communication**: Standardized messages for drivetrain, arm, sensors, and lights. -- **Timestamped Data**: Most messages include timestamps for synchronization. -- **Flexible Message Types**: Supports optional and required fields, as well as `oneof` payloads for multiplexed messages. - ---- - -## Package Structure - -``` -├── CMakeLists.txt -├── package.xml -├── proto -│   └── urc.proto -└── README.md -``` - ---- - -## Components - -### Arm & Drivetrain Messages - -- **ArmEncodersMessage (`urc.proto`)** - Reports encoder ticks for shoulder, elbow, and wrist joints. - **Inputs:** Hardware encoders - **Outputs:** Serialized Nanopb message - -- **DriveEncodersMessage (`urc.proto`)** - Reports left/right drivetrain speeds and timestamp. - **Inputs:** Motor controllers - **Outputs:** Serialized Nanopb message - -- **DrivetrainRequest / DrivetrainResponse (`urc.proto`)** - Send motor setpoints and receive speed, current, and position feedback. - **Inputs:** Desired motor commands - **Outputs:** Feedback from motors - -- **ArmClawRequest / ArmEffortRequest / ArmPositionFeedback (`urc.proto`)** - Send effort or claw velocity commands and receive joint position feedback. - -### Sensor & Status Messages - -- **IMUMessage (`urc.proto`)** - Provides orientation (quaternion), linear acceleration, and angular velocity. - Used for rover heading and motion tracking. - -- **BatteryMessage (`urc.proto`)** - Reports main voltage, individual cell voltages, charge percentage, and discharge current. - -- **StatusLightMessage / NewStatusLightCommand (`urc.proto`)** - Control RGB LEDs on the rover with optional blinking. - -- **SetpointMessage (`urc.proto`)** - Drive setpoints for left and right wheels; alternative to full drivetrain messages. - -- **ScienceModuleCommand / ScienceMotorRequest (`urc.proto`)** - Commands to rotate turntables, move leadscrews, or drill. - -- **TeensyMessage (`urc.proto`)** - Multiplexed message type combining setpoints and status light commands using `oneof`. - -### Nodes - -This package does not include runtime ROS 2 nodes. -It is a message definition library. Nodes using this package will include the generated `.h` and `.c` files for communication with microcontrollers. diff --git a/urc_navigation/grid_map_utils/CMakeLists.txt b/urc_nav_common/CMakeLists.txt similarity index 92% rename from urc_navigation/grid_map_utils/CMakeLists.txt rename to urc_nav_common/CMakeLists.txt index 9e70ebac..ae3bab5e 100644 --- a/urc_navigation/grid_map_utils/CMakeLists.txt +++ b/urc_nav_common/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.5) -project(grid_map_utils) +project(urc_nav_common) -include(../../cmake/default_settings.cmake) +include(../cmake/default_settings.cmake) find_package(ament_cmake REQUIRED) find_package(grid_map_msgs REQUIRED) diff --git a/urc_nav_common/README.md b/urc_nav_common/README.md new file mode 100644 index 00000000..4081768a --- /dev/null +++ b/urc_nav_common/README.md @@ -0,0 +1,39 @@ +# URC Navigation Common + +`urc_nav_common` exports shared C++ accessors for +`grid_map_msgs/msg/GridMap`. It has no ROS nodes or launch files. + +`grid_map_utils::GridMapUtils` is used by path planning and trajectory following +to select a map layer, convert world coordinates to cell indices, and read a +cell cost. + +## Consumer setup + +Declare `urc_nav_common` as a package dependency, link it with +`ament_target_dependencies`, and include: + +```cpp +#include +``` + +A consumer must set both the current map and layer before querying it: + +```cpp +grid_map_utils::GridMapUtils grid_map; +grid_map.setLayer("traversability_inflated"); +grid_map.setMap(message); + +float cost = 0.0F; +if (grid_map.tryGetCellCost(x, y, cost)) { + // Use the selected layer's cost at the world position. +} +``` + +## Contract + +- Query methods return `false` for a missing layer, invalid dimensions or + resolution, out-of-bounds coordinates, or an invalid data index. +- `setMap` stores a copy of the message. +- Coordinate conversion assumes an axis-aligned map whose pose is its center and + whose layer data is row-major. It does not apply map-pose rotation or Grid Map + circular-buffer offsets. diff --git a/urc_navigation/grid_map_utils/include/grid_map_utils/grid_map_utils.hpp b/urc_nav_common/include/urc_nav_common/grid_map_utils.hpp similarity index 100% rename from urc_navigation/grid_map_utils/include/grid_map_utils/grid_map_utils.hpp rename to urc_nav_common/include/urc_nav_common/grid_map_utils.hpp diff --git a/urc_navigation/grid_map_utils/package.xml b/urc_nav_common/package.xml similarity index 95% rename from urc_navigation/grid_map_utils/package.xml rename to urc_nav_common/package.xml index cd4f8166..387d4744 100644 --- a/urc_navigation/grid_map_utils/package.xml +++ b/urc_nav_common/package.xml @@ -1,7 +1,7 @@ - grid_map_utils + urc_nav_common 0.0.0 Shared GridMap access utilities Shaya Farahmand diff --git a/urc_navigation/grid_map_utils/src/grid_map_utils.cpp b/urc_nav_common/src/grid_map_utils.cpp similarity index 98% rename from urc_navigation/grid_map_utils/src/grid_map_utils.cpp rename to urc_nav_common/src/grid_map_utils.cpp index 9512e4d6..57cd8512 100644 --- a/urc_navigation/grid_map_utils/src/grid_map_utils.cpp +++ b/urc_nav_common/src/grid_map_utils.cpp @@ -1,4 +1,4 @@ -#include "grid_map_utils/grid_map_utils.hpp" +#include "urc_nav_common/grid_map_utils.hpp" #include diff --git a/urc_navigation/.DS_Store b/urc_navigation/.DS_Store new file mode 100644 index 00000000..aeb55ab9 Binary files /dev/null and b/urc_navigation/.DS_Store differ diff --git a/urc_navigation/nav_testing/include/nav_coordinator.hpp b/urc_navigation/nav_testing/include/nav_coordinator.hpp deleted file mode 100644 index 03b2af15..00000000 --- a/urc_navigation/nav_testing/include/nav_coordinator.hpp +++ /dev/null @@ -1,92 +0,0 @@ -#ifndef NAV_COORDINATOR_HPP_ -#define NAV_COORDINATOR_HPP_ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace nav_coordinator -{ -class NavCoordinator : public rclcpp::Node -{ -public: - explicit NavCoordinator(const rclcpp::NodeOptions & options); - ~NavCoordinator() override = default; - -private: - using NavigateToWaypoint = urc_msgs::action::NavigateToWaypoint; - using GoalHandleNavigate = rclcpp_action::ClientGoalHandle; - - enum class State - { - IDLE, - WAITING_FOR_SERVER, - SENDING_GOAL, - TRACKING_GOAL, - SUCCEEDED, - FAILED, - CANCELED - }; - - enum class ErrorType - { - NONE, - PLANNER_FAILURE, - OBSTACLE_DETECTED, - PLANNING_FAILED_IN_FOLLOWER, - FOLLOWER_FAILURE, - SERVER_UNAVAILABLE, - UNKNOWN_ERROR - }; - - void handleWaypoint(const geometry_msgs::msg::PoseStamped::SharedPtr msg); - void handleGpsWaypoint(const urc_msgs::msg::Waypoint::SharedPtr msg); - void sendFollowerGoal(const geometry_msgs::msg::PoseStamped & waypoint); - geometry_msgs::msg::PoseStamped convertGpsToMapWaypoint( - const urc_msgs::msg::Waypoint & waypoint); - - void handleGoalResponse(const GoalHandleNavigate::SharedPtr & goal_handle); - void handleFeedback( - GoalHandleNavigate::SharedPtr, - const std::shared_ptr feedback); - void handleResult(const GoalHandleNavigate::WrappedResult & result); - - void transitionTo(State new_state, const std::string & reason); - void handleError(ErrorType error_type, const std::string & details); - void publishState(); - std::string errorTypeToString(ErrorType error_type) const; - std::string stateToString(State state) const; - - State state_; - std::string follower_action_name_; - bool cancel_on_new_waypoint_; - std::string map_frame_id_; - std::string utm_frame_id_; - - geometry_msgs::msg::PoseStamped active_waypoint_; - GoalHandleNavigate::SharedPtr active_goal_handle_; - - rclcpp::Subscription::SharedPtr waypoint_subscriber_; - rclcpp::Subscription::SharedPtr gps_waypoint_subscriber_; - rclcpp_action::Client::SharedPtr follower_client_; - rclcpp::Publisher::SharedPtr state_publisher_; - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - ErrorType last_error_; - std::string last_error_details_; -}; - -} - -#endif \ No newline at end of file diff --git a/urc_navigation/navigation.md b/urc_navigation/navigation.md deleted file mode 100644 index 21068b54..00000000 --- a/urc_navigation/navigation.md +++ /dev/null @@ -1,152 +0,0 @@ -# urc_navigation - -## Overview - -The urc_navigation package provides autonomous rover navigation capabilities by combining two custom modules: - -1. **Path Planning (`path_planning`)** - - Implements a global planner using the A* algorithm. - - Provides plans as a sequence of poses (`nav_msgs/Path`). - - Integrated with Nav2 as an external planner service. - -2. **Trajectory Following (`trajectory_follower`)** - - Consumes planned paths and generates smooth velocity commands. - - Ensures accurate and safe tracking of the planned trajectory. - - Bridges the gap between high-level plans and rover motion control. - -This system forms a lightweight navigation stack designed for URC (University Rover Challenge) and similar robotic platforms. - ---- - -## Features - -- **Custom Path Planning (A\*)**: Grid-based A* planner that generates collision-free paths on occupancy grid costmaps. -- **Trajectory Following**: Executes smooth and precise motion along planned paths. -- **ROS 2 Integration**: Implements standard ROS 2 publishers, subscribers, and services. -- **Simulation & Hardware Support**: Works in both Gazebo simulation and real rover hardware environments. - - -## Package Structure - -``` -├── path_planning -│   ├── CMakeLists.txt -│   ├── include -│   │   ├── astar.hpp -│   │   └── planner_server.hpp -│   ├── launch -│   │   └── planning.launch.py -│   ├── package.xml -│   └── src -│   ├── astar.cpp -│   └── planner_server.cpp -└── trajectory_following - ├── CMakeLists.txt - ├── config - │   └── pure_pursuit_config.yaml - ├── include - │   ├── follower_action_server.hpp - │   ├── geometry_util.hpp - │   └── pure_pursuit.hpp - ├── launch - │   └── trajectory_following.launch.py - ├── package.xml - ├── README.md - ├── src - │   ├── follower_action_server.cpp - │   ├── geometry_util.cpp - │   └── pure_pursuit.cpp - └── test - └── geometry_util_test.cpp - -``` - -### Components - -#### Path Planning (`path_planning`) - -- **AStar (`astar.hpp/cpp`)** - - Implements the grid-based A* search algorithm - - Operates directly on `OccupancyGrid` costmaps - - Produces a vector of poses (waypoints) forming the global path - -- **PlannerServer (`planner_server.hpp/cpp`)** - - Wraps the A* planner in a ROS 2 node - - Provides a service interface for plan generation - - Subscribes to a costmap and publishes computed paths - -#### Trajectory Following (`trajectory_following`) - -- **FollowerActionServer (`follower_action_server.hpp/cpp`)** - - Implements a ROS 2 Action Server for trajectory following - - Consumes planned paths (`nav_msgs/Path`) and generates velocity commands (`geometry_msgs/Twist`) - - Interfaces with rover control to execute trajectories reliably - -- **Pure Pursuit (`pure_pursuit.hpp/cpp`)** - - Implements the Pure Pursuit path tracking algorithm - - Selects lookahead points along the path for smooth velocity generation - - Configurable via `pure_pursuit_config.yaml` - -- **Geometry Util (`geometry_util.hpp/cpp`)** - - Provides reusable geometric helper functions - - Supports Pure Pursuit calculations and general path-following logic - - Includes unit tests (`geometry_util_test.cpp`) to validate correctness - ---- -## Planner_Server | Node - -### Subscriptions -- `/costmap` (`nav_msgs/OccupancyGrid`) - Costmap input used for planning. - -### Publishers -- `/path` (`nav_msgs/Path`) - Planned path published for visualization and debugging. - -### Services -- `/plan` (`urc_msgs/srv/GeneratePlan`) - - **Request**: Start pose and goal pose (`geometry_msgs/PoseStamped`) - - **Response**: Path (`nav_msgs/Path`) and success/error code - ---- - -## Trajectory_Follower | Node - -### Subscriptions -- `/trajectory` (`nav_msgs/Path`) - Planned trajectory to follow. - -- `/odom` (`nav_msgs/Odometry`) - Current robot pose and velocity feedback. - -### Publishers -- `/cmd_vel` (`geometry_msgs/Twist`) - Velocity commands for trajectory execution. - -### Actions -- `follow_trajectory` (`urc_msgs/action/FollowTrajectory`) - - **Goal**: Trajectory to follow (`nav_msgs/Path`) - - **Feedback**: Progress along trajectory, current target pose - - **Result**: Success/failure and error code - -### Parameters -- `lookahead_distance` (double) – distance ahead on trajectory for control. -- `max_linear_speed` (double) -- `max_angular_speed` (double) - ---- - -## Launch - -Start the planner server with: - -```bash -ros2 launch path_planning planning.launch.py -``` - - -Start the trajectory_follower with: - -```bash -ros2 launch trajectory_follower trajectory_following.launch.py -``` diff --git a/urc_navigation/trajectory_following/README.md b/urc_navigation/trajectory_following/README.md deleted file mode 100644 index ce20f1df..00000000 --- a/urc_navigation/trajectory_following/README.md +++ /dev/null @@ -1,41 +0,0 @@ -## Trajectory Following - -This package exposes the `/navigate_to_waypoint` action server that, when called, commands the robot along the desired path. The request, response, and feedback definitions can be found [here](/urc_msgs/action/NavigateToWaypoint.action). - -> **Note:** Currently, the server is set up to call an implementation of the pure pursuit algorithm to achieve this. However, it is also set up so that we can (somewhat) easily drop in another -> path tracking implementation in the future. However, to do this correctly, this would require the creation of a `PathTrackingAlgorithm` interface and a refactor of how we set parameters. - -### Swerve Drive Support - -The trajectory follower now takes advantage of swerve drive capabilities for improved navigation: - -* **In-place turning**: When the robot needs to change direction significantly, it will turn in place before moving forward, reducing path deviation. -* **Holonomic motion**: The controller can command both forward/backward and lateral velocities simultaneously for more direct path following. -* **Final heading alignment**: When goal heading enforcement is enabled, the robot will perform an in-place turn at the goal to achieve the desired final orientation. - -To enable swerve drive features, set `enable_holonomic_motion: true` in the configuration. The controller will fall back to diff drive behavior when this is disabled. - -The action server is set up to be configurable. See `config/pure_pursuit_config.yaml` for all of the available parameters. The default values are shown below. - -| Parameter | Default Value | Description | -| --------- | ------------- | ----------- | -| lookahead_distance | 1.0 | Distance ahead on path to track (meters) | -| desired_linear_velocity | 0.5 | Desired forward velocity (m/s) | -| max_angular_velocity | 1.0 | Maximum turning rate (rad/s) | -| heading_alignment_tolerance | 0.2 | Angular threshold for in-place turns (radians) | -| enable_holonomic_motion | true | Enable swerve drive features | -| cmd_vel_topic | "/cmd_vel" | Topic for velocity commands | -| odom_topic | "/odom" | Topic for odometry | -| map_frame | "map" | Global coordinate frame | -| goal_tolerance | 0.1 | Position tolerance for goal (meters) | -| enforce_goal_heading | false | Require specific heading at goal | -| goal_heading_tolerance | 0.1 | Heading tolerance at goal (radians) | - -These default values will be used if the parameter is not defined in the specified config file. - -There are also a couple debug outputs. - -* The pure pursuit lookahead point is published to the `/carrot` topic. -* A circle representing the lookahead distance is published to the `/lookahead_circle` topic. - -Both of these outputs can be visualized in rviz. There is also a rviz configuration file (`/rviz/navigation.rviz`) in the repo that will configure your rviz to visualize all of the outputs from the navigation stack (including path planning). This is useful for debugging purposes. diff --git a/urc_navigation/path_planning/CMakeLists.txt b/urc_path_planning/CMakeLists.txt similarity index 93% rename from urc_navigation/path_planning/CMakeLists.txt rename to urc_path_planning/CMakeLists.txt index 6e3338d0..7c5b48e1 100644 --- a/urc_navigation/path_planning/CMakeLists.txt +++ b/urc_path_planning/CMakeLists.txt @@ -1,11 +1,11 @@ cmake_minimum_required(VERSION 3.5) -project(path_planning) +project(urc_path_planning) -include(../../cmake/default_settings.cmake) +include(../cmake/default_settings.cmake) # find dependencies find_package(ament_cmake REQUIRED) -find_package(grid_map_utils REQUIRED) +find_package(urc_nav_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(urc_msgs REQUIRED) @@ -34,7 +34,7 @@ add_library(${PROJECT_NAME} SHARED set(dependencies rclcpp rclcpp_components - grid_map_utils + urc_nav_common urc_msgs std_msgs grid_map_msgs diff --git a/urc_path_planning/README.md b/urc_path_planning/README.md new file mode 100644 index 00000000..c23b0627 --- /dev/null +++ b/urc_path_planning/README.md @@ -0,0 +1,62 @@ +# URC Path Planning + +`urc_path_planning` provides the rover's global A* planner and exposes it through +the `urc_msgs/srv/GeneratePlan` service. + +## Planning versus following + +These interfaces serve different parts of navigation: + +| Interface | Responsibility | +| --- | --- | +| `GeneratePlan` service | Runs A* once and returns a waypoint path; it does not move the rover | +| `NavigateToWaypoint` action | Follows a supplied path, or requests an A* path for a goal and then follows it | + +`NavigateToWaypoint` is implemented by `urc_trajectory_following`. The navigation +coordinator sends goal-based action requests, so the follower calls this +package's planning service before commanding motion. + +## Planner server + +`urc_path_planning_PlannerServer` is also registered as a composable ROS 2 +component. It uses these fixed interfaces: + +| Interface | Type | Purpose | +| --- | --- | --- | +| `/costmap` subscription | `grid_map_msgs/msg/GridMap` | Latest traversability map | +| `plan` service | `urc_msgs/srv/GeneratePlan` | Start and goal poses in; path and result code out | +| `/path` publisher | `nav_msgs/msg/Path` | Successful plans for visualization | + +The planner reads the `traversability_inflated` costmap layer and searches an +eight-connected grid. Each move is weighted by its distance and destination-cell +cost, so higher-cost terrain is discouraged but is not treated as an impassable +obstacle. + +## Usage + +The normal entry point is the full autonomy stack: + +```bash +ros2 launch urc_bringup autonomy.launch.py +``` + +To start only the planner server: + +```bash +ros2 launch urc_path_planning planning.launch.py +``` + +The standalone launch requires a compatible `/costmap` publisher before plan +requests can succeed. + +## Planning contract + +- Request poses and the costmap must use `map` coordinates. The server does not + transform request frames and publishes paths in `map`. +- The start must lie inside a valid costmap containing the + `traversability_inflated` layer. +- When a goal lies outside the rolling costmap, A* plans to the map boundary and + appends a straight segment to the requested goal. Terrain beyond the costmap + is not validated. +- Invalid maps, out-of-bounds starts, inaccessible cells, and failed searches + return `GeneratePlan::FAILURE`. diff --git a/urc_navigation/path_planning/include/astar.hpp b/urc_path_planning/include/urc_path_planning/astar.hpp similarity index 95% rename from urc_navigation/path_planning/include/astar.hpp rename to urc_path_planning/include/urc_path_planning/astar.hpp index 88a99b05..d53e9601 100644 --- a/urc_navigation/path_planning/include/astar.hpp +++ b/urc_path_planning/include/urc_path_planning/astar.hpp @@ -6,7 +6,7 @@ #include #include #include -#include "grid_map_utils/grid_map_utils.hpp" +#include "urc_nav_common/grid_map_utils.hpp" #include #include #include @@ -120,9 +120,9 @@ class AStar int getLayerIndex() const; - bool getMapDimensions(int &width, int &height) const; + bool getMapDimensions(int & width, int & height) const; - bool worldToGrid(double x, double y, int &map_x, int &map_y) const; + bool worldToGrid(double x, double y, int & map_x, int & map_y) const; double getCellCost(double x, double y) const; diff --git a/urc_navigation/path_planning/include/planner_server.hpp b/urc_path_planning/include/urc_path_planning/planner_server.hpp similarity index 100% rename from urc_navigation/path_planning/include/planner_server.hpp rename to urc_path_planning/include/urc_path_planning/planner_server.hpp diff --git a/urc_navigation/path_planning/launch/planning.launch.py b/urc_path_planning/launch/planning.launch.py similarity index 71% rename from urc_navigation/path_planning/launch/planning.launch.py rename to urc_path_planning/launch/planning.launch.py index 69446098..4853dda7 100644 --- a/urc_navigation/path_planning/launch/planning.launch.py +++ b/urc_path_planning/launch/planning.launch.py @@ -5,8 +5,8 @@ def generate_launch_description(): path_planner_server = Node( - package='path_planning', - executable='path_planning_PlannerServer', + package='urc_path_planning', + executable='urc_path_planning_PlannerServer', output='screen' ) diff --git a/urc_navigation/path_planning/package.xml b/urc_path_planning/package.xml similarity index 94% rename from urc_navigation/path_planning/package.xml rename to urc_path_planning/package.xml index db02a459..b7030883 100644 --- a/urc_navigation/path_planning/package.xml +++ b/urc_path_planning/package.xml @@ -1,7 +1,7 @@ - path_planning + urc_path_planning 0.0.0 Package for path planning Shaya Farahmand @@ -9,7 +9,7 @@ ament_cmake - grid_map_utils + urc_nav_common rclcpp rclcpp_components urc_msgs diff --git a/urc_navigation/path_planning/src/astar.cpp b/urc_path_planning/src/astar.cpp similarity index 97% rename from urc_navigation/path_planning/src/astar.cpp rename to urc_path_planning/src/astar.cpp index becd7173..09e9d857 100644 --- a/urc_navigation/path_planning/src/astar.cpp +++ b/urc_path_planning/src/astar.cpp @@ -1,4 +1,4 @@ -#include "astar.hpp" +#include "urc_path_planning/astar.hpp" namespace astar { @@ -139,7 +139,8 @@ void AStar::createPlan( const geometry_msgs::msg::Pose requested_goal = goal_pose; geometry_msgs::msg::Pose planning_goal = goal_pose; - bool goal_inside_costmap = worldToGrid(goal_pose.position.x, goal_pose.position.y, goal_x, goal_y); + bool goal_inside_costmap = + worldToGrid(goal_pose.position.x, goal_pose.position.y, goal_x, goal_y); if (!goal_inside_costmap && !clipGoalToCostmapBoundary(start_pose, goal_pose, planning_goal)) { diff --git a/urc_navigation/path_planning/src/planner_server.cpp b/urc_path_planning/src/planner_server.cpp similarity index 96% rename from urc_navigation/path_planning/src/planner_server.cpp rename to urc_path_planning/src/planner_server.cpp index fcba0fee..8da66c29 100644 --- a/urc_navigation/path_planning/src/planner_server.cpp +++ b/urc_path_planning/src/planner_server.cpp @@ -1,8 +1,8 @@ #include #include -#include "planner_server.hpp" -#include "astar.hpp" +#include "urc_path_planning/planner_server.hpp" +#include "urc_path_planning/astar.hpp" namespace planner_server { diff --git a/urc_perception/.DS_Store b/urc_perception/.DS_Store new file mode 100644 index 00000000..8367d9cf Binary files /dev/null and b/urc_perception/.DS_Store differ diff --git a/urc_perception/CMakeLists.txt b/urc_perception/CMakeLists.txt index df4fadd0..9c3ef242 100644 --- a/urc_perception/CMakeLists.txt +++ b/urc_perception/CMakeLists.txt @@ -40,12 +40,11 @@ add_definitions(${PCL_DEFINITIONS}) # Library creation add_library(${PROJECT_NAME} SHARED - src/elevation_mapping.cpp src/traversability_mapping.cpp ) add_library(gaussian_filter SHARED - src/GaussianFilter.cpp + src/gaussian_filter.cpp ) target_link_libraries(${PROJECT_NAME} @@ -81,12 +80,6 @@ ament_target_dependencies(gaussian_filter ${dependencies} ) -rclcpp_components_register_node( - ${PROJECT_NAME} - PLUGIN "urc_perception::ElevationMapping" - EXECUTABLE ${PROJECT_NAME}_ElevationMapping -) - rclcpp_components_register_node( ${PROJECT_NAME} PLUGIN "urc_perception::TraversabilityMapping" @@ -106,6 +99,11 @@ install( DESTINATION share/${PROJECT_NAME}/ ) +install( + DIRECTORY include/ + DESTINATION include +) + install( FILES filter_plugins.xml DESTINATION share/${PROJECT_NAME} @@ -131,6 +129,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +ament_export_include_directories(include) ament_export_libraries(gaussian_filter) ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(filters filter_plugins.xml) diff --git a/urc_perception/README.md b/urc_perception/README.md new file mode 100644 index 00000000..c0c824a6 --- /dev/null +++ b/urc_perception/README.md @@ -0,0 +1,49 @@ +# URC Perception + +`urc_perception` converts terrain point clouds into the rolling traversability +grid map used by path planning and trajectory following. + +## Mapping pipeline + +`TraversabilityMapping`: + +1. Filters non-finite points and points outside the configured sensor radius. +2. Transforms the cloud into the `map` frame. +3. Extracts an elevation grid and applies the configured Grid Map filter chain. +4. Computes slope and roughness, combines them into traversability cost, and + inflates that cost with the package's Gaussian filter plugin. +5. Merges the result into a rolling cache centered on the latest odometry pose. + +With the default configuration, the node consumes `/scan/points` and +`/odometry/filtered_global`, then publishes `/costmap` as +`grid_map_msgs/msg/GridMap`. Navigation consumes its +`traversability_inflated` layer. + +## Configuration + +- [`config/traversability_params.yaml`](config/traversability_params.yaml) + defines topics, frames, cache geometry, point filtering, and the Grid Map + filter chain. +- [`config/pcl_grid_map_params.yaml`](config/pcl_grid_map_params.yaml) controls + point-cloud preprocessing and elevation-grid extraction. +- [`filter_plugins.xml`](filter_plugins.xml) exports + `urcPerception/GaussianFilter` for use in Grid Map filter chains. + +## Usage + +Start the complete autonomy stack, including mapping and Grid Map visualization: + +```bash +ros2 launch urc_bringup autonomy.launch.py +``` + +Start only the traversability mapper: + +```bash +ros2 launch urc_perception mapping.launch.py +``` + +The standalone mapping launch does not start a point-cloud source, localization, +or the required TF tree. Incoming clouds are skipped when their sensor frame +cannot be transformed into `map`. Tune the map resolution, cache size, filter +radii, and cost calculation together with their navigation consumers. diff --git a/urc_perception/config/mapping_params.yaml b/urc_perception/config/mapping_params.yaml deleted file mode 100644 index 15c806e3..00000000 --- a/urc_perception/config/mapping_params.yaml +++ /dev/null @@ -1,11 +0,0 @@ -elevation_mapping: - ros__parameters: - map_frame: "map" - camera_frame: "camera_depth_frame" - resolution: 0.1 - width: 60 - depth_topic: "/depth_camera/points" - min_z: 0.1 - max_z: 1.0 - inflation_radius: 0.7 - inflate_obstacles: true diff --git a/urc_perception/include/elevation_mapping.hpp b/urc_perception/include/elevation_mapping.hpp deleted file mode 100644 index 383ac3ee..00000000 --- a/urc_perception/include/elevation_mapping.hpp +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef ELEVATION_MAPPING_HPP_ -#define ELEVATION_MAPPING_HPP_ - -#include -#include -#include -#include -#include -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/buffer.h" - -namespace urc_perception -{ - -class ElevationMapping : public rclcpp::Node -{ -public: - explicit ElevationMapping(const rclcpp::NodeOptions & options); - ~ElevationMapping(); - -private: - void handlePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - - geometry_msgs::msg::TransformStamped lookup_transform( - std::string target_frame, - std::string source_frame, - rclcpp::Time time); - - bool worldToMap(double x, double y, nav_msgs::msg::MapMetaData info, std::pair & out); - - int cellDistance(double world_dist) - { - return std::ceil(world_dist / resolution_); - } - - void inflate(int cell_x, int cell_y, double cell_cost, int radius); - - double gaussian(double x); - - rclcpp::Subscription::SharedPtr depth_subscriber_; - rclcpp::Publisher::SharedPtr map_publisher_; - - std::unique_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - nav_msgs::msg::OccupancyGrid map_; - - int cell_inflation_radius_; - bool inflate_obstacles_; - - int8_t max_cost_ = 100; - - double resolution_; - double min_z_; - double max_z_; - unsigned int width_; - std::string map_frame_; - std::string camera_frame_; -}; - -} // namespace urc_perception - -#endif // ELEVATION_MAPPING_HPP_ diff --git a/urc_perception/include/GaussianFilter.hpp b/urc_perception/include/urc_perception/gaussian_filter.hpp similarity index 100% rename from urc_perception/include/GaussianFilter.hpp rename to urc_perception/include/urc_perception/gaussian_filter.hpp diff --git a/urc_perception/include/traversability_mapping.hpp b/urc_perception/include/urc_perception/traversability_mapping.hpp similarity index 100% rename from urc_perception/include/traversability_mapping.hpp rename to urc_perception/include/urc_perception/traversability_mapping.hpp diff --git a/urc_perception/launch/test_sick.py b/urc_perception/launch/test_sick.py index 7e6b0144..3eaea640 100644 --- a/urc_perception/launch/test_sick.py +++ b/urc_perception/launch/test_sick.py @@ -1,9 +1,7 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node + def generate_launch_description(): map_to_base_link_tf = Node( package="tf2_ros", @@ -12,11 +10,10 @@ def generate_launch_description(): output="screen", ) - sick_node = Node( - package = "sick_scan_xd", - executable = "sick_generic_caller", - parameters = [ + package="sick_scan_xd", + executable="sick_generic_caller", + parameters=[ { "hostname": "192.168.1.10", "udp_receiver_ip": "192.168.1.3", @@ -36,11 +33,10 @@ def generate_launch_description(): ) } ], - output = "screen", + output="screen", ) return LaunchDescription([ map_to_base_link_tf, sick_node, ]) - diff --git a/urc_perception/package.xml b/urc_perception/package.xml index 1d203fa9..0f7e13a5 100644 --- a/urc_perception/package.xml +++ b/urc_perception/package.xml @@ -3,7 +3,7 @@ urc_perception 0.0.0 - Convert point cloud from depth camera to elevation map + Generate traversability maps for rover navigation Shaya Farahmand MIT diff --git a/urc_perception/perception.md b/urc_perception/perception.md deleted file mode 100644 index d830952f..00000000 --- a/urc_perception/perception.md +++ /dev/null @@ -1,139 +0,0 @@ -# urc_perception - -## Overview - -The `urc_perception` package provides the rover’s perception stack for mapping, environment filtering, and traversability analysis. - -This system includes the following modules: - -1. **Gaussian Filter** -- Applies smoothing on elevation maps to reduce sensor noise. -- Outputs filtered elevation maps suitable for mapping and traversability analysis. - -2. **Elevation Mapping** -- Builds a 2.5D elevation map from sensor data (e.g., depth camera, LiDAR). -- Produces elevation grid layers for further processing. - -3. **Traversability Mapping** -- Analyzes elevation maps for terrain hazards and drivability. -- Outputs traversability costmaps for motion planning. - ---- - -## Features - -- **Gaussian Filtering**: Smooths noisy elevation data using configurable kernels. -- **Elevation Mapping**: Maintains a rolling elevation grid map aligned to the robot’s frame. -- **Traversability Analysis**: Computes cost layers (flat, rough, steep) for safe navigation. -- **ROS 2 Integration**: Implements publishers, subscribers, and parameters compatible with Nav2 and the rover’s navigation system. - ---- - -## Package Structure - -``` -├── CMakeLists.txt -├── config -│   ├── mapping_params.yaml -│   ├── pcl_grid_map_params.yaml -│   └── traversability_params.yaml -├── filter_plugins.xml -├── include -│   ├── elevation_mapping.hpp -│   ├── GaussianFilter.hpp -│   └── traversability_mapping.hpp -├── launch -│   ├── d435i.launch.py -│   ├── mapping.launch.py -│   └── perception.launch.py -├── package.xml -└── src - ├── elevation_mapping.cpp - ├── GaussianFilter.cpp - └── traversability_mapping.cpp - -``` - ---- - -## Components - -### Gaussian Filter -- **GaussianFilter** (`GaussianFilter.hpp/cpp`) - - Applies a Gaussian kernel to smooth elevation data. - - **Input**: Raw elevation grid layers. - - **Output**: Smoothed elevation maps. - - Configurable kernel size and variance parameters. - -### Elevation Mapping -- **ElevationMapping** (`ElevationMapping.hpp/cpp`) - - Maintains a 2.5D elevation map using depth or LiDAR sensors. - - **Input**: Depth images, LiDAR point clouds, and odometry transforms. - - **Output**: Elevation grid layers with height and variance data. - -### Traversability Mapping -- **TraversabilityMapping** (`TraversabilityMapping.hpp/cpp`) - - Computes terrain traversability from elevation maps. - - **Input**: Filtered elevation map layers. - - **Output**: Traversability grid or costmap (safe, rough, hazardous). - ---- - -## ElevationMapping | Node - -### Subscriptions -- `/camera/depth/points` (`sensor_msgs/msg/PointCloud2`) - Depth point cloud input from RGB-D camera. - -- `/odom` (`nav_msgs/msg/Odometry`) - Robot pose for map alignment. - -### Publishers -- `/elevation_map` (`grid_map_msgs/msg/GridMap`) - Published elevation grid layers. - -### Parameters -- `map_resolution` (`double`) – Resolution of the elevation map (m/cell). -- `map_length` (`double`) – Size of the local map window. -- `robot_base_frame` (`string`) – Frame used to align the map to the robot. - ---- - -## TraversabilityMapping | Node - -### Subscriptions -- `/elevation_map` (`grid_map_msgs/msg/GridMap`) - Filtered elevation map input. - -### Publishers -- `/traversability_map` (`grid_map_msgs/msg/GridMap`) - Traversability costmap output. - -### Parameters -- `slope_threshold` (`double`) – Maximum slope allowed for traversability. -- `roughness_threshold` (`double`) – Threshold for terrain roughness. - ---- - -## Launching the Package - -Start the perception stack with: - -```bash -ros2 launch urc_perception perception.launch.py -``` - -Run elevation and traversability mapping together - -```bash -ros2 launch urc_perception mapping.launch.py -``` - -Launch the depth camera driver (Intel D435i example): - -```bash -ros2 launch urc_perception d435i.launch.py -``` - - - diff --git a/urc_perception/src/elevation_mapping.cpp b/urc_perception/src/elevation_mapping.cpp deleted file mode 100644 index 2bdbdaa6..00000000 --- a/urc_perception/src/elevation_mapping.cpp +++ /dev/null @@ -1,199 +0,0 @@ -#include "elevation_mapping.hpp" - -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace urc_perception -{ - -ElevationMapping::ElevationMapping(const rclcpp::NodeOptions & options) -: Node("elevation_mapping", options) -{ - RCLCPP_INFO(this->get_logger(), "Mapping node has been started."); - - declare_parameter("map_frame", "odom"); - declare_parameter("camera_frame", "camera_depth_frame"); - declare_parameter("depth_topic", "/depth_camera/points"); - - declare_parameter("resolution", 0.1); - declare_parameter("width", 60); - declare_parameter("min_z", 0.1); - declare_parameter("max_z", 2.0); - - declare_parameter("inflation_radius", 0.1); - declare_parameter("inflate_obstacles", true); - - width_ = get_parameter("width").as_int(); - resolution_ = get_parameter("resolution").as_double(); - map_frame_ = get_parameter("map_frame").as_string(); - camera_frame_ = get_parameter("camera_frame").as_string(); - min_z_ = get_parameter("min_z").as_double(); - max_z_ = get_parameter("max_z").as_double(); - - cell_inflation_radius_ = cellDistance(get_parameter("inflation_radius").as_double()); - inflate_obstacles_ = get_parameter("inflate_obstacles").as_bool(); - - RCLCPP_INFO(this->get_logger(), "Cell inflation radius set to %d", cell_inflation_radius_); - RCLCPP_INFO( - this->get_logger(), "Inflate obstacles set to %s.", inflate_obstacles_ ? "true" : "false"); - - map_.header.frame_id = map_frame_; - map_.info.resolution = resolution_; - map_.info.width = map_.info.height = width_; - map_.data.resize(map_.info.width * map_.info.height); - - depth_subscriber_ = create_subscription( - get_parameter("depth_topic").as_string(), 10, - std::bind(&ElevationMapping::handlePointcloud, this, std::placeholders::_1)); - - map_publisher_ = create_publisher("/costmap", 10); - - tf_buffer_ = std::make_unique(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); -} - -ElevationMapping::~ElevationMapping() -{ -} - -geometry_msgs::msg::TransformStamped ElevationMapping::lookup_transform( - std::string target_frame, - std::string source_frame, - rclcpp::Time time) -{ - geometry_msgs::msg::TransformStamped transform; - - try { - transform = tf_buffer_->lookupTransform(target_frame, source_frame, time); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(this->get_logger(), "Could not lookup transform: %s", ex.what()); - } - - return transform; -} - -bool ElevationMapping::worldToMap( - double x, double y, nav_msgs::msg::MapMetaData info, - std::pair & out) -{ - if (x < info.origin.position.x || x >= info.origin.position.x + info.width * info.resolution || - y < info.origin.position.y || y >= info.origin.position.y + info.height * info.resolution) - { - return false; - } - - out.first = (x - info.origin.position.x) / info.resolution; - out.second = (y - info.origin.position.y) / info.resolution; - - return true; -} - -void ElevationMapping::handlePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) -{ - // Transform the point cloud to the map frame - auto camera_to_map = lookup_transform(map_frame_, msg->header.frame_id, msg->header.stamp); - sensor_msgs::msg::PointCloud2 cloud_global_; - tf2::doTransform(*msg, cloud_global_, camera_to_map); - - // Convert the transformed point cloud to a PCL point cloud - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - pcl::fromROSMsg(cloud_global_, *cloud); - - // Remove NaN values from the point cloud - std::vector indices; - pcl::removeNaNFromPointCloud(*cloud, *cloud, indices); - - // Set the origin of the costmap to the current position of the robot - tf2::Stamped trans; - tf2::fromMsg(lookup_transform(map_frame_, "base_link", msg->header.stamp), trans); - - auto pos = trans.getOrigin(); - double x = static_cast(pos.x() / map_.info.resolution) * map_.info.resolution; - double y = static_cast(pos.y() / map_.info.resolution) * map_.info.resolution; - - map_.info.origin.position.x = x - map_.info.width * map_.info.resolution * 0.5; - map_.info.origin.position.y = y - map_.info.height * map_.info.resolution * 0.5; - map_.info.origin.position.z = 0.0; - map_.info.origin.orientation.w = 1.0; - - // Reset the costmap - std::fill(map_.data.begin(), map_.data.end(), 0); - - // Update the costmap with the point cloud - for (unsigned int i = 0; i < cloud->size(); i++) { - auto & point = cloud->points[i]; - - std::pair map_coord; - if (!worldToMap(point.x, point.y, map_.info, map_coord)) { - continue; - } - - double z = point.z - pos.z(); - double y = point.y - pos.y(); - double x = point.x - pos.x(); - - if (z < min_z_ || std::sqrt(x * x + y * y) > 2.8) { - continue; - } - - int costmap_index = map_coord.first + map_coord.second * map_.info.width; - - double cost = 0.0; - - if (z > max_z_) { - cost = max_cost_; - } else { - cost = (z - min_z_) / (max_z_ - min_z_) * max_cost_; - } - - if (cost > map_.data[costmap_index]) { - map_.data[costmap_index] = cost; - - if (inflate_obstacles_) { - inflate(map_coord.first, map_coord.second, cost, cell_inflation_radius_); - } - } - } - - map_.header.stamp = get_clock()->now(); - map_publisher_->publish(map_); -} - -void ElevationMapping::inflate(int cell_x, int cell_y, double cell_cost, int radius) -{ - for (int x = cell_x - radius; x <= cell_x + radius; x++) { - for (int y = cell_y - radius; y <= cell_y + radius; y++) { - if (x < 0 || x >= map_.info.width || y < 0 || y >= map_.info.height) { - continue; - } - - int dist = std::sqrt(std::pow(x - cell_x, 2) + std::pow(y - cell_y, 2)); - - if (dist <= radius) { - int index = x + y * map_.info.width; - double inflated_cost = gaussian(dist) * cell_cost; - - if (inflated_cost > map_.data[index]) { - map_.data[index] = inflated_cost; - } - } - } - } -} - -double ElevationMapping::gaussian(double x) -{ - return std::exp(-0.5 * x * x / cell_inflation_radius_); -} - -} // namespace urc_perception - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(urc_perception::ElevationMapping) diff --git a/urc_perception/src/GaussianFilter.cpp b/urc_perception/src/gaussian_filter.cpp similarity index 98% rename from urc_perception/src/GaussianFilter.cpp rename to urc_perception/src/gaussian_filter.cpp index 05b10bb6..70d69270 100644 --- a/urc_perception/src/GaussianFilter.cpp +++ b/urc_perception/src/gaussian_filter.cpp @@ -1,4 +1,4 @@ -#include "GaussianFilter.hpp" +#include "urc_perception/gaussian_filter.hpp" #include #include diff --git a/urc_perception/src/traversability_mapping.cpp b/urc_perception/src/traversability_mapping.cpp index 741d1351..1af9f518 100644 --- a/urc_perception/src/traversability_mapping.cpp +++ b/urc_perception/src/traversability_mapping.cpp @@ -1,4 +1,4 @@ -#include "traversability_mapping.hpp" +#include "urc_perception/traversability_mapping.hpp" #include @@ -261,7 +261,7 @@ void TraversabilityMapping::handlePointcloud(const sensor_msgs::msg::PointCloud2 const grid_map::Index src_index = *it; if (!filtered_local_map.isValid(src_index, "elevation") || - !filtered_local_map.isValid(src_index, "traversability_inflated")) + !filtered_local_map.isValid(src_index, "traversability_inflated")) { continue; } diff --git a/urc_platform/.DS_Store b/urc_platform/.DS_Store new file mode 100644 index 00000000..c9a5f90a Binary files /dev/null and b/urc_platform/.DS_Store differ diff --git a/urc_platform/CMakeLists.txt b/urc_platform/CMakeLists.txt index 4d53b7a4..6ac7080f 100644 --- a/urc_platform/CMakeLists.txt +++ b/urc_platform/CMakeLists.txt @@ -27,6 +27,7 @@ add_library(${PROJECT_NAME} SHARED src/twist_mux.cpp src/sim_gps_handler.cpp src/imu_ned2enu.cpp + src/heartbeat_publisher.cpp ) set(dependencies @@ -72,6 +73,12 @@ rclcpp_components_register_node( EXECUTABLE ${PROJECT_NAME}_ImuNED2ENU ) +rclcpp_components_register_node( + ${PROJECT_NAME} + PLUGIN "urc_platform::HeartbeatPublisher" + EXECUTABLE ${PROJECT_NAME}_HeartbeatPublisher +) + # Install launch files. install( DIRECTORY @@ -79,6 +86,11 @@ install( DESTINATION share/${PROJECT_NAME}/ ) +install( + DIRECTORY include/ + DESTINATION include +) + # Install library install(TARGETS ${PROJECT_NAME} diff --git a/urc_platform/README.md b/urc_platform/README.md index 2f202796..5e68c259 100644 --- a/urc_platform/README.md +++ b/urc_platform/README.md @@ -1,14 +1,47 @@ # URC Platform -This package is a collection of lower-level nodes focused on hardware interaction. +`urc_platform` provides the rover's input and platform-adapter nodes. Its ROS 2 +components translate joystick, velocity, GPS, and IMU data and publish the +software heartbeat. -## Joystick Driver +## Components -Deals with the manual control of the rover. Takes in joystick messages (sensor_msgs::msg::Joy) and publishes velocity messages (urc_msgs::msg::VelocityPair) telling the motors what to do. +| Executable | Responsibility | +| --- | --- | +| `urc_platform_JoystickDriver` | Converts `sensor_msgs/msg/Joy` input to scaled `TwistStamped` teleoperation commands | +| `urc_platform_TwistMux` | Selects teleoperation or autonomous `TwistStamped` commands and forwards them to the drivetrain controller | +| `urc_platform_SimGpsHandler` | Republishes simulated GPS fixes with position covariance | +| `urc_platform_ImuNED2ENU` | Converts IMU orientation, angular velocity, and acceleration from NED to ENU coordinates | +| `urc_platform_HeartbeatPublisher` | Publishes timestamped heartbeat messages at a configured interval | -## Motor Controller +All five executables are also registered as composable ROS 2 components. -Directs the operation of the motors. Takes in velocity messages (urc_msgs::msg::VelocityPair) and publishes what they mean to the motor encoders over the rover's on-board LAN. +## Usage -- [More info on how the info is encapsulated](../urc_nanopb/) -- [Actual hardware implementation of messages can be seen in the firmware repo](https://github.com/RoboJackets/urc-firmware) +The base-station launch starts the joystick driver together with the ROS joystick +node: + +```bash +ros2 launch urc_bringup base_station.launch.py +``` + +Components can also be run individually, for example: + +```bash +ros2 run urc_platform urc_platform_JoystickDriver +``` + +This package does not install its own launch files. System composition belongs in +`urc_bringup`. + +## Operational contracts + +- Joystick axes, velocity limits, and input/output topics are parameters. +- `TwistMux` starts enabled in teleoperation mode. Disabling it publishes one + zero-velocity command; mode values must be `teleop` or `autonomous`. +- `HeartbeatPublisher` requires `heartbeatInterval` in milliseconds. +- Topic defaults are defined by each component. Use `ros2 param describe` for + the complete parameter interface. + +The `config/` directory contains controller, upstream twist-mux, and VectorNav +settings. Higher-level system compositions load the relevant configuration. diff --git a/urc_bringup/config/controller_config.yaml b/urc_platform/config/controller_config.yaml similarity index 100% rename from urc_bringup/config/controller_config.yaml rename to urc_platform/config/controller_config.yaml diff --git a/urc_bringup/config/vectornav_imu.yaml b/urc_platform/config/vectornav_imu.yaml similarity index 100% rename from urc_bringup/config/vectornav_imu.yaml rename to urc_platform/config/vectornav_imu.yaml diff --git a/urc_bringup/include/heartbeat_publisher.hpp b/urc_platform/include/urc_platform/heartbeat_publisher.hpp similarity index 90% rename from urc_bringup/include/heartbeat_publisher.hpp rename to urc_platform/include/urc_platform/heartbeat_publisher.hpp index a8e95155..b29e2ca3 100644 --- a/urc_bringup/include/heartbeat_publisher.hpp +++ b/urc_platform/include/urc_platform/heartbeat_publisher.hpp @@ -7,7 +7,7 @@ #include #include -namespace heartbeat_publisher +namespace urc_platform { class HeartbeatPublisher : public rclcpp::Node @@ -23,6 +23,6 @@ class HeartbeatPublisher : public rclcpp::Node void timerCallback(); }; -} +} // namespace urc_platform #endif diff --git a/urc_platform/include/imu_ned2enu.hpp b/urc_platform/include/urc_platform/imu_ned2enu.hpp similarity index 100% rename from urc_platform/include/imu_ned2enu.hpp rename to urc_platform/include/urc_platform/imu_ned2enu.hpp diff --git a/urc_platform/include/joystick_driver.hpp b/urc_platform/include/urc_platform/joystick_driver.hpp similarity index 100% rename from urc_platform/include/joystick_driver.hpp rename to urc_platform/include/urc_platform/joystick_driver.hpp diff --git a/urc_platform/include/preprocessing.hpp b/urc_platform/include/urc_platform/preprocessing.hpp similarity index 100% rename from urc_platform/include/preprocessing.hpp rename to urc_platform/include/urc_platform/preprocessing.hpp diff --git a/urc_platform/include/sim_gps_handler.hpp b/urc_platform/include/urc_platform/sim_gps_handler.hpp similarity index 100% rename from urc_platform/include/sim_gps_handler.hpp rename to urc_platform/include/urc_platform/sim_gps_handler.hpp diff --git a/urc_platform/include/twist_mux.hpp b/urc_platform/include/urc_platform/twist_mux.hpp similarity index 100% rename from urc_platform/include/twist_mux.hpp rename to urc_platform/include/urc_platform/twist_mux.hpp diff --git a/urc_platform/platform.md b/urc_platform/platform.md deleted file mode 100644 index 4b0bc3fa..00000000 --- a/urc_platform/platform.md +++ /dev/null @@ -1,157 +0,0 @@ -# urc_platform - -## Overview - -The `urc_platform` package provides software nodes for interfacing with vehicle control and simulation systems. It includes joystick input handling, simulated GPS data processing, and twist command multiplexing for teleoperation. - -This system includes the following modules: - -1. **Joystick Driver** - - Interfaces with a joystick and converts input into velocity commands. - - Publishes `TwistStamped` messages for drivetrain control. - -2. **Simulated GPS Handler** - - Processes GPS data in simulation. - - Adjusts covariance values and republishes GPS messages. - -3. **Twist Multiplexer** - - Combines multiple velocity command inputs into a single output command. - - Ensures safe teleoperation by prioritizing sources. - ---- - -## Features - -- **Joystick Teleoperation**: Convert joystick input to linear and angular velocity commands. -- **Twist Multiplexing**: Switch between autonomous and teleop control safely. -- **Simulated GPS Handling**: Provides reliable GPS messages for simulation or testing. -- **Parameter Configurable**: Velocity limits, axis selection, topic names, and inversion can be adjusted via launch files or YAML. - ---- - -## Package Structure - -```bash -├── CMakeLists.txt -├── config -│   └── twist_mux.yaml -├── include -│   ├── joystick_driver.hpp -│   ├── preprocessing.hpp -│   ├── sim_gps_handler.hpp -│   └── twist_mux.hpp -├── launch -│   ├── joy_drive.launch.py -│   ├── joystick.launch.py -│   └── src.code-workspace -├── package.xml -├── README.md -├── src -│   ├── joystick_driver.cpp -│   ├── sim_gps_handler.cpp -│   └── twist_mux.cpp -└── test - ├── CMakeLists.txt - └── launch_tests - ├── CMakeLists.txt - ├── joystick_driver_test.py - └── motor_controller_test.py -``` - -## Components - -### Joystick Driver - -**Files:** `joystick_driver.hpp` / `joystick_driver.cpp` - -- Converts joystick input to `geometry_msgs::msg::TwistStamped`. -- Scales linear and angular velocities according to parameters. -- Supports axis inversion and custom mapping. -- Publishes commands to `/cmd_vel_teleop` by default. - -### Simulated GPS Handler - -**Files:** `sim_gps_handler.hpp` / `sim_gps_handler.cpp` - -- Receives raw GPS data (`sensor_msgs::msg::NavSatFix`). -- Updates covariance to minimal values for simulation/testing. -- Republishes messages to a configurable output topic. - -### Twist Multiplexer - -**Files:** `twist_mux.hpp` / `twist_mux.cpp` - -- Subscribes to multiple `TwistStamped` sources (autonomous and teleop). -- Publishes selected command based on mode and enabled topics. -- Ensures safe operation by zeroing commands when disabled. - -### Preprocessing - -**Files:** `preprocessing.hpp` - -- Helper functions for scaling, clamping, and inverting joystick input values. - ---- - -## Nodes - -### JoystickDriver | Node - -**Subscriptions:** - -- `/driver/joy` (`sensor_msgs/msg/Joy`) — joystick input. - -**Publishers:** - -- `/cmd_vel_teleop` (`geometry_msgs/msg/TwistStamped`) — teleop velocity commands. - -**Parameters:** - -- `max_linear_velocity` (double) – Maximum linear speed. -- `max_angular_velocity` (double) – Maximum angular speed. -- `driver_velocity_x_axis` (int) – Joystick axis for linear velocity. -- `driver_velocity_z_axis` (int) – Joystick axis for angular velocity. -- `driver_left_invert` (bool) – Invert linear axis. -- `driver_right_invert` (bool) – Invert angular axis. -- `drivetrain_topic` (string) – Output topic for velocity commands. - -### SimGpsHandler | Node - -**Subscriptions:** - -- `/gps/data_raw` (`sensor_msgs/msg/NavSatFix`) — raw GPS messages. - -**Publishers:** - -- `/gps/data` (`sensor_msgs/msg/NavSatFix`) — processed GPS messages. - -### TwistMux | Node - -**Subscriptions:** - -- `/cmd_vel_autonomous` (`geometry_msgs/msg/TwistStamped`) — autonomous commands. -- `/cmd_vel_teleop` (`geometry_msgs/msg/TwistStamped`) — teleop commands. -- `/cmd_vel_enabled` (`std_msgs/msg/Bool`) — enable/disable control. -- `/cmd_vel_mode` (`std_msgs/msg/String`) — selects autonomous or teleop. - -**Publishers:** - -- `/rover_drivetrain_controller/cmd_vel` (`geometry_msgs/msg/TwistStamped`) — selected velocity commands. - ---- - -## Launching the Package - -Start Joystick Teleop - -```bash -ros2 launch urc_platform joystick.launch.py -``` - - -Start JoyDrtive Node - -```bash -ros2 launch urc_platform joy_drive.launch.py -``` - diff --git a/urc_bringup/src/heartbeat_publisher.cpp b/urc_platform/src/heartbeat_publisher.cpp similarity index 78% rename from urc_bringup/src/heartbeat_publisher.cpp rename to urc_platform/src/heartbeat_publisher.cpp index 24631856..c428fd59 100644 --- a/urc_bringup/src/heartbeat_publisher.cpp +++ b/urc_platform/src/heartbeat_publisher.cpp @@ -1,6 +1,6 @@ -#include "heartbeat_publisher.hpp" +#include "urc_platform/heartbeat_publisher.hpp" -namespace heartbeat_publisher +namespace urc_platform { HeartbeatPublisher::HeartbeatPublisher(const rclcpp::NodeOptions & options) : rclcpp::Node("heartbeat_publisher", options) @@ -22,6 +22,6 @@ void HeartbeatPublisher::timerCallback() heartbeat_publisher->publish(message); } -} +} // namespace urc_platform -RCLCPP_COMPONENTS_REGISTER_NODE(heartbeat_publisher::HeartbeatPublisher) +RCLCPP_COMPONENTS_REGISTER_NODE(urc_platform::HeartbeatPublisher) diff --git a/urc_platform/src/imu_ned2enu.cpp b/urc_platform/src/imu_ned2enu.cpp index 5649c0af..97e31750 100644 --- a/urc_platform/src/imu_ned2enu.cpp +++ b/urc_platform/src/imu_ned2enu.cpp @@ -1,4 +1,4 @@ -#include "imu_ned2enu.hpp" +#include "urc_platform/imu_ned2enu.hpp" #include namespace imu_ned2enu diff --git a/urc_platform/src/joystick_driver.cpp b/urc_platform/src/joystick_driver.cpp index 1433a3df..afe35b8a 100644 --- a/urc_platform/src/joystick_driver.cpp +++ b/urc_platform/src/joystick_driver.cpp @@ -1,5 +1,5 @@ -#include "joystick_driver.hpp" -#include "preprocessing.hpp" +#include "urc_platform/joystick_driver.hpp" +#include "urc_platform/preprocessing.hpp" #include #include diff --git a/urc_platform/src/sim_gps_handler.cpp b/urc_platform/src/sim_gps_handler.cpp index c49fa32f..94343e5b 100644 --- a/urc_platform/src/sim_gps_handler.cpp +++ b/urc_platform/src/sim_gps_handler.cpp @@ -1,4 +1,4 @@ -#include "sim_gps_handler.hpp" +#include "urc_platform/sim_gps_handler.hpp" #include namespace sim_gps_handler diff --git a/urc_platform/src/twist_mux.cpp b/urc_platform/src/twist_mux.cpp index eb26107e..5736c0dd 100644 --- a/urc_platform/src/twist_mux.cpp +++ b/urc_platform/src/twist_mux.cpp @@ -1,4 +1,4 @@ -#include "twist_mux.hpp" +#include "urc_platform/twist_mux.hpp" #include namespace twist_mux diff --git a/urc_navigation/nav_testing/CMakeLists.txt b/urc_state_machine/CMakeLists.txt similarity index 92% rename from urc_navigation/nav_testing/CMakeLists.txt rename to urc_state_machine/CMakeLists.txt index 5f4ec46c..7dce48ca 100644 --- a/urc_navigation/nav_testing/CMakeLists.txt +++ b/urc_state_machine/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.5) -project(nav_testing) +project(urc_state_machine) -include(../../cmake/default_settings.cmake) +include(../cmake/default_settings.cmake) # find dependencies find_package(ament_cmake REQUIRED) @@ -49,6 +49,11 @@ rclcpp_components_register_node( EXECUTABLE ${PROJECT_NAME}_NavCoordinator ) +install( + DIRECTORY include/ + DESTINATION include +) + # Install library install(TARGETS ${PROJECT_NAME} diff --git a/urc_state_machine/README.md b/urc_state_machine/README.md new file mode 100644 index 00000000..f4b546e6 --- /dev/null +++ b/urc_state_machine/README.md @@ -0,0 +1,47 @@ +# URC State Machine + +`urc_state_machine` coordinates high-level waypoint requests with the trajectory +follower. It does not generate paths or command drivetrain velocity directly. + +## Navigation flow + +`urc_state_machine_NavCoordinator` accepts either: + +- a map-frame `geometry_msgs/msg/PoseStamped` on `/nav/waypoint`, or +- a latitude/longitude `urc_msgs/msg/Waypoint` on `/waypoint`. + +GPS waypoints are converted to UTM and transformed from `utm` into `map`. The +coordinator then sends a goal-based `NavigateToWaypoint` action request. The +trajectory follower calls the A* planning service, follows the returned path, +and reports feedback and completion to the coordinator. + +```text +waypoint -> NavCoordinator -> NavigateToWaypoint -> GeneratePlan -> path following +``` + +## State and failure behavior + +The coordinator publishes `IDLE`, `WAITING_FOR_SERVER`, `SENDING_GOAL`, +`TRACKING_GOAL`, `SUCCEEDED`, `FAILED`, or `CANCELED` on +`nav_coordinator_state`, together with its latest error classification. + +By default, a new waypoint cancels the active follower goal before being sent. +Missing UTM-to-map transforms, an unavailable follower action server, rejected +goals, planning failures, and follower failures transition the coordinator to +`FAILED`. + +## Usage + +The coordinator is started as part of the autonomy stack: + +```bash +ros2 launch urc_bringup autonomy.launch.py +``` + +It is also available as the `urc_state_machine_NavCoordinator` executable and a +composable ROS 2 component. This package has no standalone launch file. + +The input topics, follower action name, replacement-goal behavior, and map/UTM +frame names are parameters. Pose waypoints must already use coordinates accepted +by the follower and planner; only GPS waypoints are transformed by the +coordinator. diff --git a/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp b/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp new file mode 100644 index 00000000..349d1648 --- /dev/null +++ b/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp @@ -0,0 +1,92 @@ +#ifndef NAV_COORDINATOR_HPP_ +#define NAV_COORDINATOR_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nav_coordinator +{ +class NavCoordinator : public rclcpp::Node +{ +public: + explicit NavCoordinator(const rclcpp::NodeOptions & options); + ~NavCoordinator() override = default; + +private: + using NavigateToWaypoint = urc_msgs::action::NavigateToWaypoint; + using GoalHandleNavigate = rclcpp_action::ClientGoalHandle; + + enum class State + { + IDLE, + WAITING_FOR_SERVER, + SENDING_GOAL, + TRACKING_GOAL, + SUCCEEDED, + FAILED, + CANCELED + }; + + enum class ErrorType + { + NONE, + PLANNER_FAILURE, + OBSTACLE_DETECTED, + PLANNING_FAILED_IN_FOLLOWER, + FOLLOWER_FAILURE, + SERVER_UNAVAILABLE, + UNKNOWN_ERROR + }; + + void handleWaypoint(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void handleGpsWaypoint(const urc_msgs::msg::Waypoint::SharedPtr msg); + void sendFollowerGoal(const geometry_msgs::msg::PoseStamped & waypoint); + geometry_msgs::msg::PoseStamped convertGpsToMapWaypoint( + const urc_msgs::msg::Waypoint & waypoint); + + void handleGoalResponse(const GoalHandleNavigate::SharedPtr & goal_handle); + void handleFeedback( + GoalHandleNavigate::SharedPtr, + const std::shared_ptr feedback); + void handleResult(const GoalHandleNavigate::WrappedResult & result); + + void transitionTo(State new_state, const std::string & reason); + void handleError(ErrorType error_type, const std::string & details); + void publishState(); + std::string errorTypeToString(ErrorType error_type) const; + std::string stateToString(State state) const; + + State state_; + std::string follower_action_name_; + bool cancel_on_new_waypoint_; + std::string map_frame_id_; + std::string utm_frame_id_; + + geometry_msgs::msg::PoseStamped active_waypoint_; + GoalHandleNavigate::SharedPtr active_goal_handle_; + + rclcpp::Subscription::SharedPtr waypoint_subscriber_; + rclcpp::Subscription::SharedPtr gps_waypoint_subscriber_; + rclcpp_action::Client::SharedPtr follower_client_; + rclcpp::Publisher::SharedPtr state_publisher_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + ErrorType last_error_; + std::string last_error_details_; +}; + +} + +#endif diff --git a/urc_navigation/nav_testing/package.xml b/urc_state_machine/package.xml similarity index 77% rename from urc_navigation/nav_testing/package.xml rename to urc_state_machine/package.xml index 6455f3f9..4846dda4 100644 --- a/urc_navigation/nav_testing/package.xml +++ b/urc_state_machine/package.xml @@ -1,19 +1,22 @@ - nav_testing + urc_state_machine 0.0.0 - Package for testing navigation + Navigation coordination state machine mavren22 MIT ament_cmake rclcpp rclcpp_action + rclcpp_components geometry_msgs geodesy geographic_msgs std_msgs + tf2_geometry_msgs + tf2_ros urc_msgs ament_lint_auto diff --git a/urc_navigation/nav_testing/src/nav_coordinator.cpp b/urc_state_machine/src/nav_coordinator.cpp similarity index 91% rename from urc_navigation/nav_testing/src/nav_coordinator.cpp rename to urc_state_machine/src/nav_coordinator.cpp index 8e43cec7..338272dd 100644 --- a/urc_navigation/nav_testing/src/nav_coordinator.cpp +++ b/urc_state_machine/src/nav_coordinator.cpp @@ -1,4 +1,4 @@ -#include "nav_coordinator.hpp" +#include "urc_state_machine/nav_coordinator.hpp" #include #include @@ -46,7 +46,7 @@ NavCoordinator::NavCoordinator(const rclcpp::NodeOptions & options) get_parameter("waypoint_topic").as_string().c_str(), get_parameter("gps_waypoint_topic").as_string().c_str(), follower_action_name_.c_str()); - + if (state_publisher_) { publishState(); } @@ -126,8 +126,8 @@ geometry_msgs::msg::PoseStamped NavCoordinator::convertGpsToMapWaypoint( tf_buffer_->transform(utm_point, map_point, map_frame_id_); } catch (const tf2::TransformException & ex) { throw std::runtime_error( - "Failed to transform waypoint from '" + utm_frame_id_ + "' to '" + map_frame_id_ + "': " + - ex.what()); + "Failed to transform waypoint from '" + utm_frame_id_ + "' to '" + map_frame_id_ + "': " + + ex.what()); } geometry_msgs::msg::PoseStamped pose; @@ -214,7 +214,9 @@ void NavCoordinator::handleResult(const GoalHandleNavigate::WrappedResult & resu handleError(ErrorType::FOLLOWER_FAILURE, "Follower reported generic failure."); break; default: - handleError(ErrorType::UNKNOWN_ERROR, "Follower finished with error_code=" + std::to_string(result.result->error_code)); + handleError( + ErrorType::UNKNOWN_ERROR, + "Follower finished with error_code=" + std::to_string(result.result->error_code)); break; } transitionTo(State::FAILED, "follower finished with error"); @@ -232,7 +234,9 @@ void NavCoordinator::handleResult(const GoalHandleNavigate::WrappedResult & resu return; } - handleError(ErrorType::UNKNOWN_ERROR, "Unknown follower result code: " + std::to_string(static_cast(result.code))); + handleError( + ErrorType::UNKNOWN_ERROR, + "Unknown follower result code: " + std::to_string(static_cast(result.code))); transitionTo(State::FAILED, "unknown follower result code"); } @@ -242,26 +246,26 @@ void NavCoordinator::transitionTo(State new_state, const std::string & reason) return; } - const auto state_name = [](State state) -> const char* { - switch (state) { - case State::IDLE: - return "IDLE"; - case State::WAITING_FOR_SERVER: - return "WAITING_FOR_SERVER"; - case State::SENDING_GOAL: - return "SENDING_GOAL"; - case State::TRACKING_GOAL: - return "TRACKING_GOAL"; - case State::SUCCEEDED: - return "SUCCEEDED"; - case State::FAILED: - return "FAILED"; - case State::CANCELED: - return "CANCELED"; - default: - return "UNKNOWN"; - } - }; + const auto state_name = [](State state) -> const char * { + switch (state) { + case State::IDLE: + return "IDLE"; + case State::WAITING_FOR_SERVER: + return "WAITING_FOR_SERVER"; + case State::SENDING_GOAL: + return "SENDING_GOAL"; + case State::TRACKING_GOAL: + return "TRACKING_GOAL"; + case State::SUCCEEDED: + return "SUCCEEDED"; + case State::FAILED: + return "FAILED"; + case State::CANCELED: + return "CANCELED"; + default: + return "UNKNOWN"; + } + }; RCLCPP_INFO( get_logger(), "State transition: %s -> %s (%s)", state_name(state_), diff --git a/urc_navigation/trajectory_following/CMakeLists.txt b/urc_trajectory_following/CMakeLists.txt similarity index 86% rename from urc_navigation/trajectory_following/CMakeLists.txt rename to urc_trajectory_following/CMakeLists.txt index 55701c88..30712fad 100644 --- a/urc_navigation/trajectory_following/CMakeLists.txt +++ b/urc_trajectory_following/CMakeLists.txt @@ -1,11 +1,11 @@ cmake_minimum_required(VERSION 3.5) -project(trajectory_following) +project(urc_trajectory_following) -include(../../cmake/default_settings.cmake) +include(../cmake/default_settings.cmake) # find dependencies find_package(ament_cmake REQUIRED) -find_package(grid_map_utils REQUIRED) +find_package(urc_nav_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) @@ -18,7 +18,7 @@ find_package(geometry_msgs REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -find_package(visualization_msgs REQUIRED) +find_package(tf2_ros REQUIRED) include_directories( include @@ -26,7 +26,8 @@ include_directories( # Library creation add_library(${PROJECT_NAME} SHARED - src/pure_pursuit.cpp + src/pure_pursuit/pure_pursuit.cpp + src/trajectory_factory.cpp src/geometry_util.cpp src/follower_action_server.cpp ) @@ -35,7 +36,7 @@ set(dependencies rclcpp rclcpp_action rclcpp_components - grid_map_utils + urc_nav_common urc_msgs std_msgs grid_map_msgs @@ -44,8 +45,8 @@ set(dependencies diagnostic_updater tf2 tf2_geometry_msgs + tf2_ros nav_msgs - visualization_msgs ) ament_target_dependencies(${PROJECT_NAME} @@ -66,6 +67,11 @@ install( DESTINATION share/${PROJECT_NAME}/ ) +install( + DIRECTORY include/ + DESTINATION include +) + # Install library install(TARGETS ${PROJECT_NAME} diff --git a/urc_trajectory_following/README.md b/urc_trajectory_following/README.md new file mode 100644 index 00000000..c5e355ef --- /dev/null +++ b/urc_trajectory_following/README.md @@ -0,0 +1,56 @@ +# URC Trajectory Following + +`urc_trajectory_following` owns the `NavigateToWaypoint` action that turns a +planned path into rover velocity commands. The current controller implementation +is [Pure Pursuit](src/pure_pursuit/README.md). + +## Action behavior + +`urc_trajectory_following_FollowerActionServer` accepts two request forms: + +| Goal fields | Behavior | +| --- | --- | +| `has_goal: true` | Gets the current rover pose from TF, calls the `GeneratePlan` A* service, and follows the returned path | +| `has_path: true` | Follows the supplied non-empty `nav_msgs/Path` without initial planning | + +If both flags are set, the goal-based planning path takes precedence. A request +with neither flag is rejected. + +During execution, the server publishes distance, planning state, and replan count +as action feedback. It checks the controller's tracking point against the +configured costmap layer and requests a new A* path when the cost exceeds the +lethal threshold. + +## Runtime contracts + +- The action name is `navigate_to_waypoint`; the planning service is `plan`. +- The server requires the configured map-to-base TF throughout execution. +- `/costmap` must contain the configured layer, normally + `traversability_inflated`. A missing or unreadable cost is currently treated as + zero and therefore does not trigger replanning. +- Velocity output may be `Twist` or `TwistStamped`, selected by + `cmd_vel_stamped`; its topic is configured by `cmd_vel_topic`. +- Cancellation, completion, and handled planning failures publish a final + zero-velocity command. +- Goal heading is enforced when requested by the action or enabled in the node + configuration. + +## Usage + +The normal entry point is the complete autonomy stack: + +```bash +ros2 launch urc_bringup autonomy.launch.py +``` + +To start only the follower with its installed configuration: + +```bash +ros2 launch urc_trajectory_following trajectory_following.launch.py +``` + +The standalone launch still requires TF and, for goal-based requests, the +planner and traversability map. Runtime settings live in +[`config/pure_pursuit_config.yaml`](config/pure_pursuit_config.yaml). Although +the controller is selected through `TrajectoryFactory`, `pure_pursuit` is the +only supported selection today. diff --git a/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml b/urc_trajectory_following/config/pure_pursuit_config.yaml similarity index 85% rename from urc_navigation/trajectory_following/config/pure_pursuit_config.yaml rename to urc_trajectory_following/config/pure_pursuit_config.yaml index 9f058ce6..f750194a 100644 --- a/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml +++ b/urc_trajectory_following/config/pure_pursuit_config.yaml @@ -1,10 +1,11 @@ follower_action_server: ros__parameters: + trajectory_controller: "pure_pursuit" lookahead_distance: 0.75 desired_linear_velocity: 0.25 max_angular_velocity: 1.0 heading_alignment_tolerance: 0.2 - enable_holonomic_motion: true + enable_swerve_motion: true lethal_cost_threshold: 0.5 cmd_vel_stamped: false cmd_vel_topic: "/cmd_vel" diff --git a/urc_navigation/trajectory_following/include/follower_action_server.hpp b/urc_trajectory_following/include/urc_trajectory_following/follower_action_server.hpp similarity index 87% rename from urc_navigation/trajectory_following/include/follower_action_server.hpp rename to urc_trajectory_following/include/urc_trajectory_following/follower_action_server.hpp index d89c2497..75feb921 100644 --- a/urc_navigation/trajectory_following/include/follower_action_server.hpp +++ b/urc_trajectory_following/include/urc_trajectory_following/follower_action_server.hpp @@ -6,9 +6,8 @@ #include #include #include -#include #include -#include "grid_map_utils/grid_map_utils.hpp" +#include "urc_nav_common/grid_map_utils.hpp" #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" #include "urc_msgs/action/navigate_to_waypoint.hpp" @@ -29,10 +28,6 @@ class FollowerActionServer : public rclcpp::Node std::string target_frame, std::string source_frame); - visualization_msgs::msg::Marker create_lookahead_circle( - double x, double y, double radius, - std::string frame_id); - void publishZeroVelocity(); rclcpp_action::GoalResponse handle_navigate_goal( @@ -65,7 +60,6 @@ class FollowerActionServer : public rclcpp::Node grid_map_utils::GridMapUtils grid_map_utils_; rclcpp::Subscription::SharedPtr costmap_subscriber_; std::string costmap_layer_; - rclcpp::Publisher::SharedPtr carrot_pub_; rclcpp::Publisher::SharedPtr cmd_vel_pub_; rclcpp::Publisher::SharedPtr cmd_vel_stamped_pub_; rclcpp_action::Server::SharedPtr navigate_server_; @@ -75,8 +69,6 @@ class FollowerActionServer : public rclcpp::Node std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_; - rclcpp::Publisher::SharedPtr marker_pub_; - bool stamped_; }; } // namespace follower_action_server diff --git a/urc_navigation/trajectory_following/include/geometry_util.hpp b/urc_trajectory_following/include/urc_trajectory_following/geometry_util.hpp similarity index 100% rename from urc_navigation/trajectory_following/include/geometry_util.hpp rename to urc_trajectory_following/include/urc_trajectory_following/geometry_util.hpp diff --git a/urc_navigation/trajectory_following/include/pure_pursuit.hpp b/urc_trajectory_following/include/urc_trajectory_following/pure_pursuit/pure_pursuit.hpp similarity index 68% rename from urc_navigation/trajectory_following/include/pure_pursuit.hpp rename to urc_trajectory_following/include/urc_trajectory_following/pure_pursuit/pure_pursuit.hpp index d857f073..95f74df2 100644 --- a/urc_navigation/trajectory_following/include/pure_pursuit.hpp +++ b/urc_trajectory_following/include/urc_trajectory_following/pure_pursuit/pure_pursuit.hpp @@ -1,17 +1,18 @@ #ifndef PURE_PURSUIT_HPP_ #define PURE_PURSUIT_HPP_ -#include "geometry_msgs/msg/transform_stamped.hpp" -#include +#include "urc_trajectory_following/trajectory_controller.hpp" #include #include #include #include #include -namespace pure_pursuit { -template -Iterator find_min_by(Iterator first, Iterator last, UnaryPredicate pred) { +namespace pure_pursuit +{ +template +Iterator find_min_by(Iterator first, Iterator last, UnaryPredicate pred) +{ auto minValue = pred(*first); auto minIt = first; while (first != last) { @@ -26,7 +27,8 @@ Iterator find_min_by(Iterator first, Iterator last, UnaryPredicate pred) { return minIt; } -struct PurePursuitParams { +struct PurePursuitParams +{ double lookahead_distance; double desired_linear_velocity; double max_angular_velocity; @@ -34,12 +36,9 @@ struct PurePursuitParams { bool enable_swerve_motion; }; -struct PurePursuitOutput { - geometry_msgs::msg::TwistStamped cmd_vel; - geometry_msgs::msg::PointStamped lookahead_point; -}; -class PurePursuit { +class PurePursuit : public trajectory_following::TrajectoryController +{ public: explicit PurePursuit(PurePursuitParams params); @@ -48,16 +47,16 @@ class PurePursuit { * @param path The path to follow in the local frame (usually the base_link * frame) */ - void setPath(const nav_msgs::msg::Path &path); + void setPath(const nav_msgs::msg::Path & path) override; /** * @brief Get the desired velocity command and lookahead point given the * current pose * @param map_to_base_link Transform from the map frame to base link */ - PurePursuitOutput getCommandVelocity( - const rclcpp::Logger &logger, - const geometry_msgs::msg::TransformStamped &map_to_base_link); + trajectory_following::TrajectoryOutput getCommandVelocity( + const rclcpp::Logger & logger, + const geometry_msgs::msg::TransformStamped & map_to_base_link) override; private: /** @@ -67,8 +66,9 @@ class PurePursuit { * @param lookahead_distance The distance to look ahead (m) */ geometry_msgs::msg::PoseStamped - getLookaheadPose(const rclcpp::Logger &logger, - const nav_msgs::msg::Path &path, double lookahead_distance); + getLookaheadPose( + const rclcpp::Logger & logger, + const nav_msgs::msg::Path & path, double lookahead_distance); nav_msgs::msg::Path path_; diff --git a/urc_trajectory_following/include/urc_trajectory_following/trajectory_controller.hpp b/urc_trajectory_following/include/urc_trajectory_following/trajectory_controller.hpp new file mode 100644 index 00000000..8989bbf8 --- /dev/null +++ b/urc_trajectory_following/include/urc_trajectory_following/trajectory_controller.hpp @@ -0,0 +1,31 @@ +#ifndef TRAJECTORY_CONTROLLER_HPP_ +#define TRAJECTORY_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include + +namespace trajectory_following +{ +struct TrajectoryOutput +{ + geometry_msgs::msg::TwistStamped cmd_vel; + geometry_msgs::msg::PointStamped tracking_point; +}; + +class TrajectoryController +{ +public: + virtual ~TrajectoryController() = default; + + virtual void setPath(const nav_msgs::msg::Path & path) = 0; + + virtual TrajectoryOutput getCommandVelocity( + const rclcpp::Logger & logger, + const geometry_msgs::msg::TransformStamped & map_to_base_link) = 0; +}; +} // namespace trajectory_following + +#endif // TRAJECTORY_CONTROLLER_HPP_ diff --git a/urc_trajectory_following/include/urc_trajectory_following/trajectory_factory.hpp b/urc_trajectory_following/include/urc_trajectory_following/trajectory_factory.hpp new file mode 100644 index 00000000..cffb2b8f --- /dev/null +++ b/urc_trajectory_following/include/urc_trajectory_following/trajectory_factory.hpp @@ -0,0 +1,24 @@ +#ifndef TRAJECTORY_FACTORY_HPP_ +#define TRAJECTORY_FACTORY_HPP_ + +#include "urc_trajectory_following/trajectory_controller.hpp" + +#include +#include + +#include + +namespace trajectory_following +{ +class TrajectoryFactory +{ +public: + static bool supports(const std::string & controller_type); + + static std::unique_ptr create( + const std::string & controller_type, + const rclcpp::Node & node); +}; +} // namespace trajectory_following + +#endif // TRAJECTORY_FACTORY_HPP_ diff --git a/urc_navigation/trajectory_following/launch/trajectory_following.launch.py b/urc_trajectory_following/launch/trajectory_following.launch.py similarity index 75% rename from urc_navigation/trajectory_following/launch/trajectory_following.launch.py rename to urc_trajectory_following/launch/trajectory_following.launch.py index a5ef29a1..a5647b8f 100644 --- a/urc_navigation/trajectory_following/launch/trajectory_following.launch.py +++ b/urc_trajectory_following/launch/trajectory_following.launch.py @@ -6,13 +6,13 @@ def generate_launch_description(): trajectory_follower_action_server = Node( - package="trajectory_following", - executable="trajectory_following_FollowerActionServer", + package="urc_trajectory_following", + executable="urc_trajectory_following_FollowerActionServer", output="screen", parameters=[ PathJoinSubstitution( [ - FindPackageShare("trajectory_following"), + FindPackageShare("urc_trajectory_following"), "config", "pure_pursuit_config.yaml", ] diff --git a/urc_navigation/trajectory_following/package.xml b/urc_trajectory_following/package.xml similarity index 89% rename from urc_navigation/trajectory_following/package.xml rename to urc_trajectory_following/package.xml index 44461e9b..d4eabfcc 100644 --- a/urc_navigation/trajectory_following/package.xml +++ b/urc_trajectory_following/package.xml @@ -1,7 +1,7 @@ - trajectory_following + urc_trajectory_following 0.0.0 Package for trajectory following implementation Mrinal Jain @@ -13,7 +13,7 @@ ament_lint_common ament_cmake_gtest - grid_map_utils + urc_nav_common rclcpp rclcpp_components urc_msgs @@ -23,9 +23,9 @@ geometry_msgs tf2 tf2_geometry_msgs + tf2_ros sensor_msgs diagnostic_updater - visualization_msgs ament_cmake diff --git a/urc_navigation/trajectory_following/src/follower_action_server.cpp b/urc_trajectory_following/src/follower_action_server.cpp similarity index 54% rename from urc_navigation/trajectory_following/src/follower_action_server.cpp rename to urc_trajectory_following/src/follower_action_server.cpp index 648115ac..7bdd1630 100644 --- a/urc_navigation/trajectory_following/src/follower_action_server.cpp +++ b/urc_trajectory_following/src/follower_action_server.cpp @@ -1,15 +1,18 @@ -#include "follower_action_server.hpp" +#include "urc_trajectory_following/follower_action_server.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_util.hpp" -#include "pure_pursuit.hpp" +#include "urc_trajectory_following/geometry_util.hpp" +#include "urc_trajectory_following/trajectory_factory.hpp" #include "tf2/exceptions.h" #include #include #include +#include -namespace follower_action_server { -FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions &options) - : Node("follower_action_server", options) { +namespace follower_action_server +{ +FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions & options) +: Node("follower_action_server", options) +{ RCLCPP_INFO(this->get_logger(), "Follower node has been started."); declare_parameter("lookahead_distance", 2.0); @@ -26,6 +29,12 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions &options) declare_parameter("enforce_goal_heading", false); declare_parameter("goal_heading_tolerance", 0.1); declare_parameter("costmap_layer", "traversability_inflated"); + declare_parameter("trajectory_controller", "pure_pursuit"); + + const auto controller_type = get_parameter("trajectory_controller").as_string(); + if (!trajectory_following::TrajectoryFactory::supports(controller_type)) { + throw std::invalid_argument("Unsupported trajectory controller: " + controller_type); + } tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -36,70 +45,76 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions &options) if (stamped_) { cmd_vel_stamped_pub_ = create_publisher( - get_parameter("cmd_vel_topic").as_string(), 10); + get_parameter("cmd_vel_topic").as_string(), 10); } else { cmd_vel_pub_ = create_publisher( - get_parameter("cmd_vel_topic").as_string(), 10); + get_parameter("cmd_vel_topic").as_string(), 10); } - carrot_pub_ = - create_publisher("carrot", 10); - marker_pub_ = - create_publisher("lookahead_circle", 10); - // Setup the costmap costmap_subscriber_ = create_subscription( - "/costmap", rclcpp::SystemDefaultsQoS(), - std::bind(&FollowerActionServer::handleCostmap, this, - std::placeholders::_1)); + "/costmap", rclcpp::SystemDefaultsQoS(), + std::bind( + &FollowerActionServer::handleCostmap, this, + std::placeholders::_1)); // Create an action server for the navigate_to_waypoint action navigate_server_ = - rclcpp_action::create_server( - this, "navigate_to_waypoint", - std::bind(&FollowerActionServer::handle_navigate_goal, this, - std::placeholders::_1, std::placeholders::_2), - std::bind(&FollowerActionServer::handle_navigate_cancel, this, - std::placeholders::_1), - std::bind(&FollowerActionServer::handle_navigate_accepted, this, - std::placeholders::_1)); + rclcpp_action::create_server( + this, "navigate_to_waypoint", + std::bind( + &FollowerActionServer::handle_navigate_goal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind( + &FollowerActionServer::handle_navigate_cancel, this, + std::placeholders::_1), + std::bind( + &FollowerActionServer::handle_navigate_accepted, this, + std::placeholders::_1)); // Create a client for the path planning service planning_client_ = create_client("plan"); rover_position_pub_ = - create_publisher("rover_position", 10); + create_publisher("rover_position", 10); } void FollowerActionServer::handleCostmap( - const grid_map_msgs::msg::GridMap::SharedPtr msg) { + const grid_map_msgs::msg::GridMap::SharedPtr msg) +{ current_costmap_ = *msg; grid_map_utils_.setMap(*msg); } geometry_msgs::msg::TransformStamped -FollowerActionServer::lookup_transform(std::string target_frame, - std::string source_frame) { +FollowerActionServer::lookup_transform( + std::string target_frame, + std::string source_frame) +{ geometry_msgs::msg::TransformStamped transform; try { // RCLCPP_INFO(this->get_logger(), "Looking up transform from %s to %s", // source_frame.c_str(), target_frame.c_str()); - transform = tf_buffer_->lookupTransform(target_frame, source_frame, - tf2::TimePointZero); - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Could not lookup transform: %s", - ex.what()); + transform = tf_buffer_->lookupTransform( + target_frame, source_frame, + tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + this->get_logger(), "Could not lookup transform: %s", + ex.what()); } return transform; } -FollowerActionServer::~FollowerActionServer() { +FollowerActionServer::~FollowerActionServer() +{ RCLCPP_INFO(this->get_logger(), "Follower action server has been stopped."); } rclcpp_action::GoalResponse FollowerActionServer::handle_navigate_goal( - const rclcpp_action::GoalUUID &uuid, - std::shared_ptr goal) { + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) +{ RCLCPP_INFO(this->get_logger(), "Received navigate to waypoint goal request"); (void)uuid; @@ -118,9 +133,10 @@ rclcpp_action::GoalResponse FollowerActionServer::handle_navigate_goal( } rclcpp_action::CancelResponse FollowerActionServer::handle_navigate_cancel( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) +{ RCLCPP_INFO(this->get_logger(), "Received request to cancel navigate goal"); (void)goal_handle; @@ -129,49 +145,19 @@ rclcpp_action::CancelResponse FollowerActionServer::handle_navigate_cancel( } void FollowerActionServer::handle_navigate_accepted( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - std::thread{std::bind(&FollowerActionServer::execute_navigate, this, - std::placeholders::_1), - goal_handle} - .detach(); + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) +{ + std::thread{std::bind( + &FollowerActionServer::execute_navigate, this, + std::placeholders::_1), + goal_handle} + .detach(); } -visualization_msgs::msg::Marker -FollowerActionServer::create_lookahead_circle(double x, double y, double radius, - std::string frame_id) { - visualization_msgs::msg::Marker circle; - circle.header.frame_id = frame_id; - circle.header.stamp = get_clock()->now(); - uint32_t shape = visualization_msgs::msg::Marker::CYLINDER; - - circle.ns = "basic_shapes"; - circle.id = 0; - circle.type = shape; - circle.action = visualization_msgs::msg::Marker::ADD; - - circle.pose.position.x = x; - circle.pose.position.y = y; - circle.pose.position.z = 0.0; - circle.pose.orientation.x = 0.0; - circle.pose.orientation.y = 0.0; - circle.pose.orientation.z = 0.0; - circle.pose.orientation.w = 1.0; - - circle.scale.x = 2 * radius; - circle.scale.y = 2 * radius; - circle.scale.z = 0.01; - - circle.color.r = 0.0f; - circle.color.g = 0.0f; - circle.color.b = 1.0f; - circle.color.a = 0.3; - - return circle; -} - -void FollowerActionServer::publishZeroVelocity() { +void FollowerActionServer::publishZeroVelocity() +{ geometry_msgs::msg::TwistStamped cmd_vel; cmd_vel.header.stamp = get_clock()->now(); cmd_vel.twist.linear.x = 0.0; @@ -184,11 +170,13 @@ void FollowerActionServer::publishZeroVelocity() { } } -float FollowerActionServer::getCost(double x, double y) { +float FollowerActionServer::getCost(double x, double y) +{ float cost; if (!grid_map_utils_.tryGetCellCost(x, y, cost)) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, - "Costmap layer '%s' not found", costmap_layer_.c_str()); + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Costmap layer '%s' not found", costmap_layer_.c_str()); return 0.0f; } @@ -196,8 +184,9 @@ float FollowerActionServer::getCost(double x, double y) { } nav_msgs::msg::Path FollowerActionServer::callPlanningService( - const geometry_msgs::msg::PoseStamped &start, - const geometry_msgs::msg::PoseStamped &goal, bool &success) { + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal, bool & success) +{ auto request = std::make_shared(); request->start = start; request->goal = goal; @@ -209,14 +198,17 @@ nav_msgs::msg::Path FollowerActionServer::callPlanningService( if (result.wait_for(std::chrono::seconds(10)) == std::future_status::ready) { auto response = result.get(); if (response->error_code == - urc_msgs::srv::GeneratePlan::Response::SUCCESS) { - RCLCPP_INFO(this->get_logger(), "Planning successful, path has %ld poses", - response->path.poses.size()); + urc_msgs::srv::GeneratePlan::Response::SUCCESS) + { + RCLCPP_INFO( + this->get_logger(), "Planning successful, path has %ld poses", + response->path.poses.size()); success = true; return response->path; } else { - RCLCPP_ERROR(this->get_logger(), "Planning failed with error code %d", - response->error_code); + RCLCPP_ERROR( + this->get_logger(), "Planning failed with error code %d", + response->error_code); success = false; return nav_msgs::msg::Path(); } @@ -228,24 +220,25 @@ nav_msgs::msg::Path FollowerActionServer::callPlanningService( } void FollowerActionServer::execute_navigate( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) +{ RCLCPP_INFO(this->get_logger(), "Executing navigate to waypoint goal"); auto feedback = - std::make_shared(); + std::make_shared(); feedback->distance_to_goal = std::numeric_limits::max(); feedback->is_planning = false; feedback->replan_count = 0; auto result = - std::make_shared(); - const auto &goal_msg = goal_handle->get_goal(); + std::make_shared(); + const auto & goal_msg = goal_handle->get_goal(); // Determine if we should enforce goal heading (from request or config) bool enforce_heading = goal_msg->enforce_goal_heading || - get_parameter("enforce_goal_heading").as_bool(); + get_parameter("enforce_goal_heading").as_bool(); nav_msgs::msg::Path path; geometry_msgs::msg::PoseStamped goal_pose; @@ -262,18 +255,19 @@ void FollowerActionServer::execute_navigate( geometry_msgs::msg::PoseStamped start_pose; try { auto transform = tf_buffer_->lookupTransform( - get_parameter("map_frame").as_string(), - get_parameter("base_link_frame").as_string(), tf2::TimePointZero); + get_parameter("map_frame").as_string(), + get_parameter("base_link_frame").as_string(), tf2::TimePointZero); start_pose.header = transform.header; start_pose.pose.position.x = transform.transform.translation.x; start_pose.pose.position.y = transform.transform.translation.y; start_pose.pose.position.z = transform.transform.translation.z; start_pose.pose.orientation = transform.transform.rotation; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Could not get current pose: %s", - ex.what()); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + this->get_logger(), "Could not get current pose: %s", + ex.what()); result->error_code = - urc_msgs::action::NavigateToWaypoint::Result::PLANNING_FAILED; + urc_msgs::action::NavigateToWaypoint::Result::PLANNING_FAILED; goal_handle->abort(result); publishZeroVelocity(); return; @@ -286,7 +280,7 @@ void FollowerActionServer::execute_navigate( if (!planning_success || path.poses.empty()) { result->error_code = - urc_msgs::action::NavigateToWaypoint::Result::PLANNING_FAILED; + urc_msgs::action::NavigateToWaypoint::Result::PLANNING_FAILED; goal_handle->abort(result); RCLCPP_ERROR(this->get_logger(), "Initial planning failed"); publishZeroVelocity(); @@ -299,33 +293,14 @@ void FollowerActionServer::execute_navigate( goal_pose.pose = path.poses.back().pose; } - // Create a PurePursuit object - pure_pursuit::PurePursuitParams params; - params.lookahead_distance = get_parameter("lookahead_distance").as_double(); - params.desired_linear_velocity = - get_parameter("desired_linear_velocity").as_double(); - params.max_angular_velocity = - get_parameter("max_angular_velocity").as_double(); - params.heading_alignment_tolerance = - get_parameter("heading_alignment_tolerance").as_double(); - params.enable_swerve_motion = get_parameter("enable_swerve_motion").as_bool(); - pure_pursuit::PurePursuit pure_pursuit(params); - - pure_pursuit.setPath(path); - RCLCPP_INFO(this->get_logger(), "Lookahead distance: %f", - params.lookahead_distance); - RCLCPP_INFO(this->get_logger(), "Desired linear velocity: %f", - params.desired_linear_velocity); - RCLCPP_INFO(this->get_logger(), "Max angular velocity: %f", - params.max_angular_velocity); - RCLCPP_INFO(this->get_logger(), "Heading alignment tolerance: %f rad", - params.heading_alignment_tolerance); - RCLCPP_INFO(this->get_logger(), "swerve motion enabled: %s", - params.enable_swerve_motion ? "true" : "false"); - RCLCPP_INFO(this->get_logger(), "Following path with %ld poses", - path.poses.size()); - - pure_pursuit::PurePursuitOutput output; + auto trajectory_controller = trajectory_following::TrajectoryFactory::create( + get_parameter("trajectory_controller").as_string(), *this); + trajectory_controller->setPath(path); + RCLCPP_INFO( + this->get_logger(), "Following path with %ld poses", + path.poses.size()); + + trajectory_following::TrajectoryOutput output; rclcpp::Rate rate(10); while (rclcpp::ok()) { @@ -333,49 +308,52 @@ void FollowerActionServer::execute_navigate( geometry_msgs::msg::PoseStamped current_pose_map_frame_; try { auto transform = tf_buffer_->lookupTransform( - get_parameter("map_frame").as_string(), - get_parameter("base_link_frame").as_string(), tf2::TimePointZero); + get_parameter("map_frame").as_string(), + get_parameter("base_link_frame").as_string(), tf2::TimePointZero); current_pose_map_frame_.header = transform.header; current_pose_map_frame_.pose.position.x = - transform.transform.translation.x; + transform.transform.translation.x; current_pose_map_frame_.pose.position.y = - transform.transform.translation.y; + transform.transform.translation.y; current_pose_map_frame_.pose.position.z = - transform.transform.translation.z; + transform.transform.translation.z; current_pose_map_frame_.pose.orientation = transform.transform.rotation; - } catch (tf2::TransformException &ex) { - RCLCPP_WARN(this->get_logger(), "Could not get current pose: %s", - ex.what()); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN( + this->get_logger(), "Could not get current pose: %s", + ex.what()); rate.sleep(); continue; } // Update feedback distance feedback->distance_to_goal = geometry_util::dist2D( - current_pose_map_frame_.pose.position, goal_pose.pose.position); + current_pose_map_frame_.pose.position, goal_pose.pose.position); if (goal_handle->is_canceling()) { goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "Goal has been canceled"); break; } else if (feedback->distance_to_goal < - get_parameter("goal_tolerance").as_double()) { + get_parameter("goal_tolerance").as_double()) + { bool heading_satisfied = true; if (enforce_heading) { double heading_error = geometry_util::angularDistance( - current_pose_map_frame_.pose.orientation, - goal_pose.pose.orientation); + current_pose_map_frame_.pose.orientation, + goal_pose.pose.orientation); heading_satisfied = - heading_error < get_parameter("goal_heading_tolerance").as_double(); + heading_error < get_parameter("goal_heading_tolerance").as_double(); if (!heading_satisfied) { // For swerve drive, perform in-place turning to align heading if (get_parameter("enable_swerve_motion").as_bool()) { // Calculate signed heading error for proper turn direction tf2::Quaternion current_quat, goal_quat; - tf2::fromMsg(current_pose_map_frame_.pose.orientation, - current_quat); + tf2::fromMsg( + current_pose_map_frame_.pose.orientation, + current_quat); tf2::fromMsg(goal_pose.pose.orientation, goal_quat); double current_yaw = tf2::getYaw(current_quat); @@ -383,19 +361,22 @@ void FollowerActionServer::execute_navigate( double signed_error = goal_yaw - current_yaw; // Normalize to [-pi, pi] - while (signed_error > M_PI) + while (signed_error > M_PI) { signed_error -= 2.0 * M_PI; - while (signed_error < -M_PI) + } + while (signed_error < -M_PI) { signed_error += 2.0 * M_PI; + } geometry_msgs::msg::TwistStamped align_cmd; align_cmd.header.stamp = get_clock()->now(); align_cmd.twist.linear.x = 0.0; align_cmd.twist.linear.y = 0.0; align_cmd.twist.angular.z = - std::clamp(signed_error * 2.0, // Proportional control - -get_parameter("max_angular_velocity").as_double(), - get_parameter("max_angular_velocity").as_double()); + std::clamp( + signed_error * 2.0, // Proportional control + -get_parameter("max_angular_velocity").as_double(), + get_parameter("max_angular_velocity").as_double()); if (stamped_) { cmd_vel_stamped_pub_->publish(align_cmd); @@ -404,36 +385,40 @@ void FollowerActionServer::execute_navigate( } RCLCPP_DEBUG( - this->get_logger(), - "Aligning final heading: error %.3f rad, omega %.3f rad/s", - signed_error, align_cmd.twist.angular.z); + this->get_logger(), + "Aligning final heading: error %.3f rad, omega %.3f rad/s", + signed_error, align_cmd.twist.angular.z); goal_handle->publish_feedback(feedback); rate.sleep(); continue; } else { - RCLCPP_DEBUG(this->get_logger(), - "Position reached but heading error %.3f rad exceeds " - "tolerance %.3f rad", - heading_error, - get_parameter("goal_heading_tolerance").as_double()); + RCLCPP_DEBUG( + this->get_logger(), + "Position reached but heading error %.3f rad exceeds " + "tolerance %.3f rad", + heading_error, + get_parameter("goal_heading_tolerance").as_double()); } } } if (heading_satisfied) { result->error_code = - urc_msgs::action::NavigateToWaypoint::Result::SUCCESS; + urc_msgs::action::NavigateToWaypoint::Result::SUCCESS; goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "Goal has been reached!"); break; } - } else if (getCost(output.lookahead_point.point.x, - output.lookahead_point.point.y) > - get_parameter("lethal_cost_threshold").as_double()) { + } else if (getCost( + output.tracking_point.point.x, + output.tracking_point.point.y) > + get_parameter("lethal_cost_threshold").as_double()) + { // Obstacle detected - attempt to re-plan - RCLCPP_WARN(this->get_logger(), - "Obstacle detected! Attempting to re-plan..."); + RCLCPP_WARN( + this->get_logger(), + "Obstacle detected! Attempting to re-plan..."); feedback->is_planning = true; feedback->replan_count++; @@ -443,19 +428,20 @@ void FollowerActionServer::execute_navigate( geometry_msgs::msg::PoseStamped start_pose; try { auto transform = tf_buffer_->lookupTransform( - get_parameter("map_frame").as_string(), - get_parameter("base_link_frame").as_string(), tf2::TimePointZero); + get_parameter("map_frame").as_string(), + get_parameter("base_link_frame").as_string(), tf2::TimePointZero); start_pose.header = transform.header; start_pose.pose.position.x = transform.transform.translation.x; start_pose.pose.position.y = transform.transform.translation.y; start_pose.pose.position.z = transform.transform.translation.z; start_pose.pose.orientation = transform.transform.rotation; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), - "Could not get current pose for replanning: %s", - ex.what()); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + this->get_logger(), + "Could not get current pose for replanning: %s", + ex.what()); result->error_code = - urc_msgs::action::NavigateToWaypoint::Result::PLANNING_FAILED; + urc_msgs::action::NavigateToWaypoint::Result::PLANNING_FAILED; goal_handle->abort(result); publishZeroVelocity(); return; @@ -463,13 +449,13 @@ void FollowerActionServer::execute_navigate( bool planning_success = false; nav_msgs::msg::Path new_path = - callPlanningService(start_pose, goal_pose, planning_success); + callPlanningService(start_pose, goal_pose, planning_success); feedback->is_planning = false; if (!planning_success || new_path.poses.empty()) { result->error_code = - urc_msgs::action::NavigateToWaypoint::Result::PLANNING_FAILED; + urc_msgs::action::NavigateToWaypoint::Result::PLANNING_FAILED; goal_handle->abort(result); RCLCPP_ERROR(this->get_logger(), "Re-planning failed"); break; @@ -477,16 +463,18 @@ void FollowerActionServer::execute_navigate( // Update the path and continue path = new_path; - pure_pursuit.setPath(path); - RCLCPP_INFO(this->get_logger(), - "Re-planning successful, following new path with %ld poses", - path.poses.size()); + trajectory_controller->setPath(path); + RCLCPP_INFO( + this->get_logger(), + "Re-planning successful, following new path with %ld poses", + path.poses.size()); } - output = pure_pursuit.getCommandVelocity( - this->get_logger(), - lookup_transform(get_parameter("base_link_frame").as_string(), - get_parameter("map_frame").as_string())); + output = trajectory_controller->getCommandVelocity( + this->get_logger(), + lookup_transform( + get_parameter("base_link_frame").as_string(), + get_parameter("map_frame").as_string())); if (stamped_) { cmd_vel_stamped_pub_->publish(output.cmd_vel); @@ -500,15 +488,6 @@ void FollowerActionServer::execute_navigate( rover_point.point = current_pose_map_frame_.pose.position; rover_position_pub_->publish(rover_point); - auto circle = create_lookahead_circle( - current_pose_map_frame_.pose.position.x, - current_pose_map_frame_.pose.position.y, params.lookahead_distance, - get_parameter("map_frame").as_string()); - marker_pub_->publish(circle); - - // Publish the carrot point - carrot_pub_->publish(output.lookahead_point); - // Publish feedback goal_handle->publish_feedback(feedback); diff --git a/urc_navigation/trajectory_following/src/geometry_util.cpp b/urc_trajectory_following/src/geometry_util.cpp similarity index 89% rename from urc_navigation/trajectory_following/src/geometry_util.cpp rename to urc_trajectory_following/src/geometry_util.cpp index 425eaf65..15b3c123 100644 --- a/urc_navigation/trajectory_following/src/geometry_util.cpp +++ b/urc_trajectory_following/src/geometry_util.cpp @@ -1,4 +1,4 @@ -#include "geometry_util.hpp" +#include "urc_trajectory_following/geometry_util.hpp" #include #include @@ -50,8 +50,10 @@ geometry_msgs::msg::Point circleSegmentIntersection( discriminant = r * r * dr * dr - D * D; if (discriminant < 0) { - throw std::runtime_error("No intersection found, discriminant is zero or negative.\nPoint a: (" + std::to_string(a.x) + ", " + std::to_string(a.y) + "), Point b: (" + - std::to_string(b.x) + ", " + std::to_string(b.y) + "), radius: " + std::to_string(r)); + throw std::runtime_error( + "No intersection found, discriminant is zero or negative.\nPoint a: (" + + std::to_string(a.x) + ", " + std::to_string(a.y) + "), Point b: (" + + std::to_string(b.x) + ", " + std::to_string(b.y) + "), radius: " + std::to_string(r)); // throw std::runtime_error("No intersection found, discriminant is zero or negative."); } @@ -105,10 +107,10 @@ double angularDistance( tf2::Quaternion tf_q1, tf_q2; tf2::fromMsg(q1, tf_q1); tf2::fromMsg(q2, tf_q2); - + double dot = tf_q1.dot(tf_q2); dot = std::clamp(dot, -1.0, 1.0); - + return std::acos(std::abs(dot)) * 2.0; } diff --git a/urc_trajectory_following/src/pure_pursuit/README.md b/urc_trajectory_following/src/pure_pursuit/README.md new file mode 100644 index 00000000..fb01d179 --- /dev/null +++ b/urc_trajectory_following/src/pure_pursuit/README.md @@ -0,0 +1,30 @@ +# Pure Pursuit Controller + +`PurePursuit` implements the package-wide `TrajectoryController` interface. The +follower action server creates it through `TrajectoryFactory` when the +`trajectory_controller` parameter is set to `pure_pursuit`. + +## Controller Contract + +Every trajectory controller must: + +- accept a planned `nav_msgs/Path` through `setPath()` +- compute a `TrajectoryOutput` from the current map-to-base transform +- return a velocity command in `TrajectoryOutput::cmd_vel` +- return a map-frame collision-check position in + `TrajectoryOutput::tracking_point` + +Pure Pursuit uses its lookahead point as the collision-check position. The +follower action server compares the cost at that position against the lethal +cost threshold and requests a new path when necessary. + +## Adding LQR or MPC + +An additional controller must implement `TrajectoryController` and be +constructed by `TrajectoryFactory`. Its selection name and parameters must also +be added to the trajectory-following configuration. + +The current interface assumes every controller can provide one meaningful +collision-check position. LQR or MPC may instead need to expose a predicted +trajectory. If so, replace this single-point contract with a controller-neutral +collision-check representation before implementing those controllers. diff --git a/urc_navigation/trajectory_following/src/pure_pursuit.cpp b/urc_trajectory_following/src/pure_pursuit/pure_pursuit.cpp similarity index 59% rename from urc_navigation/trajectory_following/src/pure_pursuit.cpp rename to urc_trajectory_following/src/pure_pursuit/pure_pursuit.cpp index 248b07a5..2141f68d 100644 --- a/urc_navigation/trajectory_following/src/pure_pursuit.cpp +++ b/urc_trajectory_following/src/pure_pursuit/pure_pursuit.cpp @@ -1,33 +1,38 @@ -#include "pure_pursuit.hpp" -#include "geometry_util.hpp" +#include "urc_trajectory_following/pure_pursuit/pure_pursuit.hpp" +#include "urc_trajectory_following/geometry_util.hpp" #include #include -namespace pure_pursuit { +namespace pure_pursuit +{ -PurePursuit::PurePursuit(PurePursuitParams params) { params_ = params; } +PurePursuit::PurePursuit(PurePursuitParams params) {params_ = params;} -void PurePursuit::setPath(const nav_msgs::msg::Path &path) { path_ = path; } +void PurePursuit::setPath(const nav_msgs::msg::Path & path) {path_ = path;} geometry_msgs::msg::PoseStamped -PurePursuit::getLookaheadPose(const rclcpp::Logger &logger, - const nav_msgs::msg::Path &path, - double lookahead_distance) { +PurePursuit::getLookaheadPose( + const rclcpp::Logger & logger, + const nav_msgs::msg::Path & path, + double lookahead_distance) +{ // Find the closest pose in the path auto closestPoseIt = - find_min_by(path.poses.begin(), path.poses.end(), - [&](const geometry_msgs::msg::PoseStamped &pose) { - return geometry_util::magnitude(pose.pose.position); - }); + find_min_by( + path.poses.begin(), path.poses.end(), + [&](const geometry_msgs::msg::PoseStamped & pose) { + return geometry_util::magnitude(pose.pose.position); + }); // Find the first point outside the lookahead distance, starting at the pose // closest to the current pose auto pose = - std::find_if(closestPoseIt, path.poses.end(), - [&](const geometry_msgs::msg::PoseStamped &pose) { - return geometry_util::magnitude(pose.pose.position) > - lookahead_distance; - }); + std::find_if( + closestPoseIt, path.poses.end(), + [&](const geometry_msgs::msg::PoseStamped & pose) { + return geometry_util::magnitude(pose.pose.position) > + lookahead_distance; + }); // If no point is found, return the last pose in the path if (pose == path.poses.end()) { @@ -46,7 +51,7 @@ PurePursuit::getLookaheadPose(const rclcpp::Logger &logger, prev_pose->pose.position.y, pose->pose.position.x, pose->pose.position.y); */ auto point = geometry_util::circleSegmentIntersection( - prev_pose->pose.position, pose->pose.position, lookahead_distance); + prev_pose->pose.position, pose->pose.position, lookahead_distance); geometry_msgs::msg::PoseStamped lookahead_point; lookahead_point.header.frame_id = pose->header.frame_id; @@ -56,66 +61,69 @@ PurePursuit::getLookaheadPose(const rclcpp::Logger &logger, return lookahead_point; } -PurePursuitOutput PurePursuit::getCommandVelocity( - const rclcpp::Logger &logger, - const geometry_msgs::msg::TransformStamped &map_to_base_link) { +trajectory_following::TrajectoryOutput PurePursuit::getCommandVelocity( + const rclcpp::Logger & logger, + const geometry_msgs::msg::TransformStamped & map_to_base_link) +{ nav_msgs::msg::Path transformed_path_; // modify map_to_base_link to have zero z translation and zero x,y rotation geometry_msgs::msg::TransformStamped map_to_base_link_modified = - map_to_base_link; + map_to_base_link; map_to_base_link_modified.transform.translation.z = 0.0; map_to_base_link_modified.transform.rotation.x = 0.0; map_to_base_link_modified.transform.rotation.y = 0.0; - for (const auto &pose : path_.poses) { + for (const auto & pose : path_.poses) { geometry_msgs::msg::PoseStamped transformed_pose; tf2::doTransform(pose, transformed_pose, map_to_base_link_modified); transformed_path_.poses.push_back(transformed_pose); RCLCPP_INFO( - logger, - "Current Pose: (%f, %f, w: %f, z: %f), map_to_base_link: (%f, %f, w: " - "%f, z: %f), Transformed Path Pose: (%f, %f, w: %f, z: %f)", - pose.pose.position.x, pose.pose.position.y, pose.pose.orientation.w, - pose.pose.orientation.z, - map_to_base_link_modified.transform.translation.x, - map_to_base_link_modified.transform.translation.y, - map_to_base_link_modified.transform.rotation.w, - map_to_base_link_modified.transform.rotation.z, - transformed_pose.pose.position.x, transformed_pose.pose.position.y, - transformed_pose.pose.orientation.w, - transformed_pose.pose.orientation.z); + logger, + "Current Pose: (%f, %f, w: %f, z: %f), map_to_base_link: (%f, %f, w: " + "%f, z: %f), Transformed Path Pose: (%f, %f, w: %f, z: %f)", + pose.pose.position.x, pose.pose.position.y, pose.pose.orientation.w, + pose.pose.orientation.z, + map_to_base_link_modified.transform.translation.x, + map_to_base_link_modified.transform.translation.y, + map_to_base_link_modified.transform.rotation.w, + map_to_base_link_modified.transform.rotation.z, + transformed_pose.pose.position.x, transformed_pose.pose.position.y, + transformed_pose.pose.orientation.w, + transformed_pose.pose.orientation.z); } auto lookahead_pose = - getLookaheadPose(logger, transformed_path_, params_.lookahead_distance); + getLookaheadPose(logger, transformed_path_, params_.lookahead_distance); - PurePursuitOutput output; + trajectory_following::TrajectoryOutput output; geometry_msgs::msg::TwistStamped cmd_vel; cmd_vel.header = path_.header; if (params_.enable_swerve_motion) { // Swerve drive: Use swerve motion capabilities - double lookahead_angle = std::atan2(lookahead_pose.pose.position.y, - lookahead_pose.pose.position.x); + double lookahead_angle = std::atan2( + lookahead_pose.pose.position.y, + lookahead_pose.pose.position.x); double lookahead_distance = - geometry_util::magnitude(lookahead_pose.pose.position); + geometry_util::magnitude(lookahead_pose.pose.position); // Check if we need to align heading before moving if (lookahead_distance > 0.01 && - std::abs(lookahead_angle) > params_.heading_alignment_tolerance) { + std::abs(lookahead_angle) > params_.heading_alignment_tolerance) + { // Turn in place to face the lookahead point cmd_vel.twist.linear.x = 0.0; cmd_vel.twist.linear.y = 0.0; cmd_vel.twist.angular.z = std::clamp( - lookahead_angle * 2.0, // Proportional control - -params_.max_angular_velocity, params_.max_angular_velocity); + lookahead_angle * 2.0, // Proportional control + -params_.max_angular_velocity, params_.max_angular_velocity); RCLCPP_DEBUG( - logger, - "Aligning heading: angle error = %.3f rad, omega = %.3f rad/s", - lookahead_angle, cmd_vel.twist.angular.z); + logger, + "Aligning heading: angle error = %.3f rad, omega = %.3f rad/s", + lookahead_angle, cmd_vel.twist.angular.z); } else { // Move towards lookahead point with direct motion double linear_vel = params_.desired_linear_velocity; @@ -126,16 +134,17 @@ PurePursuitOutput PurePursuit::getCommandVelocity( // Add corrective angular velocity for path curvature double curvature = - geometry_util::calcCurvature(lookahead_pose.pose.position); + geometry_util::calcCurvature(lookahead_pose.pose.position); cmd_vel.twist.angular.z = - std::clamp(linear_vel * curvature, -params_.max_angular_velocity, - params_.max_angular_velocity); + std::clamp( + linear_vel * curvature, -params_.max_angular_velocity, + params_.max_angular_velocity); } } else { // Diff drive mode: Original pure pursuit behavior double linear_vel = params_.desired_linear_velocity; double curvature = - geometry_util::calcCurvature(lookahead_pose.pose.position); + geometry_util::calcCurvature(lookahead_pose.pose.position); double angular_vel = linear_vel * curvature; cmd_vel.twist.linear.x = linear_vel; @@ -160,7 +169,7 @@ PurePursuitOutput PurePursuit::getCommandVelocity( lookahead_point.point = lookahead_pose.pose.position; tf2::doTransform(lookahead_point, lookahead_point, base_link_to_map); - output.lookahead_point = lookahead_point; + output.tracking_point = lookahead_point; return output; } diff --git a/urc_trajectory_following/src/trajectory_factory.cpp b/urc_trajectory_following/src/trajectory_factory.cpp new file mode 100644 index 00000000..0d32b439 --- /dev/null +++ b/urc_trajectory_following/src/trajectory_factory.cpp @@ -0,0 +1,36 @@ +#include "urc_trajectory_following/trajectory_factory.hpp" + +#include "urc_trajectory_following/pure_pursuit/pure_pursuit.hpp" + +#include + +namespace trajectory_following +{ +namespace +{ +constexpr char kPurePursuit[] = "pure_pursuit"; +} + +bool TrajectoryFactory::supports(const std::string & controller_type) +{ + return controller_type == kPurePursuit; +} + +std::unique_ptr TrajectoryFactory::create( + const std::string & controller_type, + const rclcpp::Node & node) +{ + if (controller_type == kPurePursuit) { + pure_pursuit::PurePursuitParams params{ + node.get_parameter("lookahead_distance").as_double(), + node.get_parameter("desired_linear_velocity").as_double(), + node.get_parameter("max_angular_velocity").as_double(), + node.get_parameter("heading_alignment_tolerance").as_double(), + node.get_parameter("enable_swerve_motion").as_bool() + }; + return std::make_unique(params); + } + + throw std::invalid_argument("Unsupported trajectory controller: " + controller_type); +} +} // namespace trajectory_following