diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..9b3aa8b --- /dev/null +++ b/.clang-format @@ -0,0 +1 @@ +BasedOnStyle: LLVM diff --git a/.github/workflows/cpp-formatter.yml b/.github/workflows/cpp-formatter.yml new file mode 100644 index 0000000..34c019e --- /dev/null +++ b/.github/workflows/cpp-formatter.yml @@ -0,0 +1,16 @@ +name: C++ Formatting Check + +on: [push, pull_request] + +jobs: + cpp_formatter: + runs-on: ubuntu-latest + steps: + - name: Checkout code + uses: actions/checkout@v4 + + - name: Clang Formatter + uses: DoozyX/clang-format-lint-action@v0.18.1 + with: + clangFormatVersion: 14 + source: "src" diff --git a/.github/workflows/mkdocs.yml b/.github/workflows/mkdocs.yml new file mode 100644 index 0000000..ae9a5b4 --- /dev/null +++ b/.github/workflows/mkdocs.yml @@ -0,0 +1,26 @@ +name: ci +on: + push: + branches: + - main +permissions: + contents: write +jobs: + deploy: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: actions/setup-python@v4 + with: + python-version: 3.x + - run: echo "cache_id=$(date --utc '+%V')" >> $GITHUB_ENV + - uses: actions/cache@v3 + with: + key: mkdocs-material-${{ env.cache_id }} + path: .cache + restore-keys: | + mkdocs-material- + - run: sudo apt-get -y install doxygen + - run: pip install mkdocs-material + - run: pip install mkdoxy + - run: mkdocs gh-deploy --force diff --git a/.github/workflows/ros2_build_ros2_debian_package.yml b/.github/workflows/ros2_build_ros2_debian_package.yml new file mode 100644 index 0000000..c465955 --- /dev/null +++ b/.github/workflows/ros2_build_ros2_debian_package.yml @@ -0,0 +1,27 @@ +name: Build Debian Packages (ROS2 Jazzy) +on: + workflow_dispatch: + release: + types: [created] +jobs: + build-debian-packages: + runs-on: ubuntu-latest + env: + GH_TOKEN: ${{ secrets.GITHUB_TOKEN }} + steps: + - name: Install required packages + run: sudo apt-get update && sudo apt-get install libsdl2-dev -y + - name: Checkout this repository + uses: actions/checkout@v2.3.4 + - name: Build Debian packages + uses: ichiro-its/ros2-build-debian-action@main + with: + ros2-distro: jazzy + - name: Upload Release Asset + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + run: | + REPO_FULL=$(gh repo view --json nameWithOwner --jq .nameWithOwner) + TAG_NAME=$(gh release view --repo "$REPO_FULL" --json tagName --jq ".tagName") + + gh release upload "$TAG_NAME" package/*.deb --repo "$REPO_FULL" \ No newline at end of file diff --git a/.github/workflows/ros2_jazzy_code_compiles.yml b/.github/workflows/ros2_jazzy_code_compiles.yml new file mode 100644 index 0000000..7c8d0a4 --- /dev/null +++ b/.github/workflows/ros2_jazzy_code_compiles.yml @@ -0,0 +1,37 @@ +name: ros2_jazzy_code_compiles + +on: + push: + branches: ["main"] + pull_request: + branches: ["main"] + workflow_dispatch: {} + +jobs: + build: + # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. + # You can convert this to a matrix build if you need cross-platform coverage. + # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix + runs-on: ubuntu-24.04 + + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: jazzy + + - name: Install required packages + # To compile and run the code we require cmake, ninja and opencv + run: sudo apt-get update && sudo apt-get install build-essential cmake ninja-build ros-jazzy-diagnostic-updater libsdl2-dev + + - name: Install dependencies + working-directory: ${{ github.workspace }} + run: | + sudo rosdep init || true + rosdep update --rosdistro=jazzy + + - name: Colcon build + working-directory: ${{ github.workspace }} + run: | + source /opt/ros/jazzy/setup.bash + colcon build diff --git a/.github/workflows/ros2_jazzy_code_compiles_windows.yml b/.github/workflows/ros2_jazzy_code_compiles_windows.yml new file mode 100644 index 0000000..b6e9d84 --- /dev/null +++ b/.github/workflows/ros2_jazzy_code_compiles_windows.yml @@ -0,0 +1,22 @@ +name: ROS2 Jazzy with Pixi on Windows + +on: + push: + branches: + - main + pull_request: + +jobs: + build: + runs-on: windows-latest + + steps: + - name: Checkout Repository + uses: actions/checkout@v4 + - name: Setup Pixi + uses: prefix-dev/setup-pixi@v0.8.3 + + - name: Run colcon build (Release Mode) + shell: cmd + run: pixi run build + diff --git a/.github/workflows/ros2_jazzy_conda_package_linux.yml b/.github/workflows/ros2_jazzy_conda_package_linux.yml new file mode 100644 index 0000000..b0e31ea --- /dev/null +++ b/.github/workflows/ros2_jazzy_conda_package_linux.yml @@ -0,0 +1,32 @@ +name: Create ROS2 Jazzy package on Linux + +on: + workflow_dispatch: + release: + types: [created] +jobs: + build: + runs-on: ubuntu-latest + steps: + - name: Checkout Repository + uses: actions/checkout@v4 + + - name: Install Pixi + uses: prefix-dev/setup-pixi@v0.8.3 + + - name: Build package with Pixi + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + shell: pixi run bash -e {0} + run: | + source rattler/set_build_var.sh + rattler-build build --recipe rattler/recipe.yaml -c conda-forge -c https://prefix.dev/robostack-jazzy --package-format tar-bz2 + - name: Upload Release Asset + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + run: | + REPO_FULL=$(gh repo view --json nameWithOwner --jq .nameWithOwner) + TAG_NAME=$(gh release view --repo "$REPO_FULL" --json tagName --jq ".tagName") + + gh release upload "$TAG_NAME" output/linux-64/ros-jazzy-*.tar.bz2 --repo "$REPO_FULL" + diff --git a/.github/workflows/ros2_jazzy_conda_package_win.yml b/.github/workflows/ros2_jazzy_conda_package_win.yml new file mode 100644 index 0000000..8b91c05 --- /dev/null +++ b/.github/workflows/ros2_jazzy_conda_package_win.yml @@ -0,0 +1,56 @@ +name: Create ROS2 Jazzy package on Windows + +on: + workflow_dispatch: + release: + types: [created] + +jobs: + build: + runs-on: windows-latest + + steps: + - name: Checkout Repository + uses: actions/checkout@v4 + + - name: Install Pixi + uses: prefix-dev/setup-pixi@v0.8.3 + + + - name: Build package with Pixi (Windows) + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + shell: pwsh + run: | + rattler/set_build_var.ps1 + pixi run rattler-build build --recipe rattler/recipe.yaml -c conda-forge -c https://prefix.dev/robostack-jazzy --package-format tar-bz2 + + + - name: Upload Windows Conda Package to GitHub Release + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + shell: pwsh + run: | + $ErrorActionPreference = "Stop" + + $repo = (gh repo view --json nameWithOwner | ConvertFrom-Json).nameWithOwner + + $tag = gh release view --repo $repo --json tagName --jq ".tagName" + + $pkg = Get-ChildItem "output/win-64/ros-jazzy-*.tar.bz2" | + Where-Object { $_.Extension -in ".bz2", ".conda" } | + Sort-Object LastWriteTime -Descending | + Select-Object -First 1 + + if (-not $pkg) { + Write-Error "❌ No conda package found in win-64." + } + + if ($pkg.Name -like "*.tar.bz2") { + $renamed = $pkg.FullName -replace '\.tar.bz2$', '-win.tar.bz2' + Copy-Item $pkg.FullName $renamed + } else { + $renamed = $pkg.FullName + } + + gh release upload $tag $renamed --repo $repo --clobber diff --git a/.gitignore b/.gitignore index 259148f..a2f1744 100644 --- a/.gitignore +++ b/.gitignore @@ -30,3 +30,19 @@ *.exe *.out *.app + +# ROS2 build folders +build/ +install/ +log/ + + +# Rattler build folders +output/ + +# IDE folders +.vscode/ + +# Pixi artifacts +pixi.lock +.pixi/ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..22f8e77 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.1...3.14) +project(ti_iwr_pointcloud C CXX) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) + +include(FetchContent) + +FetchContent_Declare( + Serial + GIT_REPOSITORY https://github.com/ami-iit/serial_cpp.git + GIT_TAG v1.3.0 +) +FetchContent_MakeAvailable(Serial) + + +add_executable(publish_node src/publish_node.cpp + src/packet_parser/packet_parser.cpp + src/packet_parser/serial_data_parser.cpp + src/packet_parser/serial_cmd_parser.cpp + src/packet_parser/tlv_parser.cpp) + +ament_target_dependencies(publish_node + rclcpp + sensor_msgs +) +target_link_libraries(publish_node "${cpp_typesupport_target}" serial_cpp::serial_cpp) +target_include_directories(publish_node PUBLIC src/) + +if(UNIX AND NOT APPLE) # Linux only + target_compile_options(publish_node PRIVATE -Wall) +endif() + +install(TARGETS +publish_node +DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/colcon.pkg b/colcon.pkg new file mode 100644 index 0000000..a95e50a --- /dev/null +++ b/colcon.pkg @@ -0,0 +1,6 @@ +{ + "name": "ti_iwr_pointcloud", + "cmake-args":[ + "-G Ninja" + ] +} \ No newline at end of file diff --git a/docs/about/implementation.md b/docs/about/implementation.md new file mode 100644 index 0000000..57c3a41 --- /dev/null +++ b/docs/about/implementation.md @@ -0,0 +1,62 @@ +# 🌟 **`publish_node` - Radar PointCloud Publisher** + +Publishes 3D radar point cloud data as `sensor_msgs/PointCloud2` by interfacing with a custom radar parser (`PacketParser`). Ideal for integrating Texas Instruments radar sensors with ROS 2. + +--- + +### βœ… Topics + +| Topic | Type | Description | +|---------------------|----------------------------------|-----------------------------------------| +| `/radar_3d_points` | `sensor_msgs/msg/PointCloud2` | 3D radar point cloud with SNR as intensity | + +--- + +### βš™οΈ Parameters + +| Name | Type | Default Value | Description | +|------------------|----------|------------------------------------------------|--------------------------------------------| +| `cfg_path` | string | `"../radar/cfg/cfg_default_30fps.cfg"` | Path to the radar configuration file | +| `cli_port` | string | `"/dev/pts/2"` | Serial port for radar CLI control | +| `cli_baudrate` | int | `115200` | Baudrate for the CLI port | +| `data_port` | string | `"/dev/pts/6"` | Serial port for radar data stream | +| `data_baudrate` | int | `921600` | Baudrate for the data port | +| `publish_topic` | string | `"/radar_3d_points"` | Topic name to publish the point cloud data | + +--- + +### πŸ“¦ Message Notes + +- `sensor_msgs/msg/PointCloud2` fields: + - `x`, `y`, `z`: Cartesian coordinates (float32) + - `intensity`: SNR value (float32), mapped from radar point's `snr` + +--- + +### 🧩 Implementation Notes + +- Built using **`rclcpp`**. +- Uses a **separate thread** for processing and publishing radar frames. +- Interfaces with a custom radar parser (`PacketParser`). +- Uses a **condition variable and mutex** for frame queue synchronization. +- Each received radar frame is converted into a ROS 2 `PointCloud2` message. +- The frame is published to a topic specified by a **configurable parameter**. + +--- + +### 🏁 Launch Example + +```bash +ros2 run ti_iwr_pointcloud publish_node +``` + +--- + +### πŸ§ͺ Dependencies + +- `rclcpp` +- `sensor_msgs` +- `serial_cpp` +- Custom radar parser (PacketParser) +- Parameter server for dynamic config +- C++ standard threading and synchronization diff --git a/docs/assets/favicon.png b/docs/assets/favicon.png new file mode 100644 index 0000000..af0f517 Binary files /dev/null and b/docs/assets/favicon.png differ diff --git a/docs/assets/logo.png b/docs/assets/logo.png new file mode 100644 index 0000000..dcee91c Binary files /dev/null and b/docs/assets/logo.png differ diff --git a/docs/assets/stylesheets/logo.css b/docs/assets/stylesheets/logo.css new file mode 100644 index 0000000..cf530dc --- /dev/null +++ b/docs/assets/stylesheets/logo.css @@ -0,0 +1,12 @@ +.md-header__button.md-logo { + margin-top: 0; + margin-bottom: 0; + padding-top: 0; + padding-bottom: 0; + } + + .md-header__button.md-logo img, + .md-header__button.md-logo svg { + height: 10%; + width: 10%; + } \ No newline at end of file diff --git a/docs/contributing/license.md b/docs/contributing/license.md new file mode 100644 index 0000000..32719bb --- /dev/null +++ b/docs/contributing/license.md @@ -0,0 +1,4 @@ +# License + +This work is licensed under the apache-2.0 license. + diff --git a/docs/contributing/rules.md b/docs/contributing/rules.md new file mode 100644 index 0000000..8a8af07 --- /dev/null +++ b/docs/contributing/rules.md @@ -0,0 +1,16 @@ +# Contribution rules + +For contributing we recommend creating a fork and when ready a pull request to merge the changes with this repository. +In the pull request please state: + +- What has been added/changed? +- Some reasoning about implementation details + +Please don't do refactors without consent of administrators as we will not merge them. + +## New features + +If you have any cool feature/idea to add to the code, please start an issue in GitHub to introduce the idea to the maintainers. + + + diff --git a/docs/index.md b/docs/index.md new file mode 100644 index 0000000..06999b2 --- /dev/null +++ b/docs/index.md @@ -0,0 +1,3 @@ +# Home + +Welcome to the project wiki. \ No newline at end of file diff --git a/docs/usage/overview.md b/docs/usage/overview.md new file mode 100644 index 0000000..4f8fd56 --- /dev/null +++ b/docs/usage/overview.md @@ -0,0 +1,17 @@ +# Getting started + +# πŸš€ Quick Start Examples + +> See [node documentation](../about/implementation.md) above for parameters & message formats. + +--- + +### 1. Start Radar PointCloud Node + +Publishes 3D radar point cloud data on the `/radar_3d_points` topic: + +```bash +ros2 run ti_iwr_pointcloud publish_node +``` + +--- diff --git a/docs/usage/ros2_pixi_build_linux_windows.md b/docs/usage/ros2_pixi_build_linux_windows.md new file mode 100644 index 0000000..93ab02f --- /dev/null +++ b/docs/usage/ros2_pixi_build_linux_windows.md @@ -0,0 +1,102 @@ +# Getting started with Pixi + +Pixi makes cross-platform ROS 2 development easy. You can build and run radar point cloud publishing nodes on **Linux and Windows**β€”with no system-wide ROS install. + +--- + +## πŸ“¦ Install Pixi + +**Linux**: + +```bash +curl -fsSL https://pixi.sh/install.sh | bash +``` + +**Windows** (PowerShell): + +```powershell +powershell -ExecutionPolicy ByPass -c "irm -useb https://pixi.sh/install.ps1 | iex" +``` + +--- + +## πŸš€ Clone & Build Project + +```bash +git clone https://github.com/CLFML/TI_IWR_Pointcloud_ROS.git +cd TI_IWR_Pointcloud_ROS +pixi install +pixi run build +``` + +Or launch VSCode with the environment: + +```bash +pixi run vscode +``` + +> βœ… **Note (Windows):** Always build in **Release** or **RelWithDebInfo**, not Debug! +> *(Ctrl+Shift+P β†’ "CMake: Select Variant")* + +--- + +## ⚑ Using as a Pixi Dependency + +Want to use `ti_iwr_pointcloud` from another Pixi-based project? + +### 1. Init a new project + +```bash +mkdir my_project && cd my_project +pixi init +``` + +### 2. Edit `pixi.toml` + +Add these: + +```toml +[project] +channels = [ + "https://fast.prefix.dev/conda-forge", + "https://prefix.dev/robostack-jazzy", + "https://clfml.github.io/conda_ros2_jazzy_channel/" +] + +[dependencies] +ros-jazzy-ros-base = "*" +ros-jazzy-ti-iwr-pointcloud = "*" +colcon-common-extensions = "*" +rosdep = "*" +``` + +### 🧠 Optional: VSCode Support + +Add to your `pixi.toml`: + +```toml +[target.linux-64.dependencies] +python-devtools = "*" +pybind11 = "*" +numpy = "*" + +[target.win-64.dependencies] +python-devtools = "*" + +[target.linux-64.tasks] +vscode = 'env -u LD_LIBRARY_PATH code .' + +[target.win-64.tasks] +vscode = "code ." +``` + +--- + +### 3. Run the node + +```bash +pixi install +pixi run ros2 run ti_iwr_pointcloud publish_node +``` + +--- diff --git a/docs/usage/usage_with_native_linux.md b/docs/usage/usage_with_native_linux.md new file mode 100644 index 0000000..f3b6446 --- /dev/null +++ b/docs/usage/usage_with_native_linux.md @@ -0,0 +1,36 @@ +# 🐧 Getting Started with Native ROS + +Prefer a traditional system-wide install? Use the prebuilt `.deb` package for **Ubuntu Noble / ROS 2 Jazzy**. + +--- + +## πŸ“¦ Install package via `.deb` + +Install the latest `.deb` package directly from [Releases](https://github.com/CLFML/TI_IWR_Pointcloud_ROS/releases): + +```bash +curl -s https://api.github.com/repos/CLFML/TI_IWR_Pointcloud_ROS/releases/latest \ + | grep "browser_download_url.*deb" \ + | cut -d : -f 2,3 \ + | tr -d \" \ + | wget -qi - +sudo dpkg -i ./ros-jazzy-*.deb +``` + +--- + +## βœ… Run the Node + +Make sure ROS 2 is sourced: + +```bash +source /opt/ros/jazzy/setup.sh +``` + +Then launch the radar point cloud publisher: + +```bash +ros2 run ti_iwr_pointcloud publish_node +``` + +--- diff --git a/mkdocs.yml b/mkdocs.yml new file mode 100644 index 0000000..6d1d75f --- /dev/null +++ b/mkdocs.yml @@ -0,0 +1,65 @@ +site_name: TI_IWR_Pointcloud_ROS +extra_css: +- assets/stylesheets/logo.css +theme: + name: material + # custom_dir: docs/overrides + features: + - announce.dismiss + - content.action.edit + - content.action.view + - content.code.annotate + - content.code.copy + # - content.code.select + # - content.footnote.tooltips + # - content.tabs.link + - content.tooltips + # - header.autohide + # - navigation.expand + - navigation.footer + - navigation.indexes + # - navigation.instant + # - navigation.instant.prefetch + # - navigation.instant.progress + # - navigation.prune + - navigation.sections + - navigation.tabs + # - navigation.tabs.sticky + - navigation.top + - navigation.tracking + - search.highlight + - search.share + - search.suggest + - toc.follow + # - toc.integrate + palette: + scheme: default + primary: green + toggle: + icon: material/toggle-switch + name: Switch to dark mode + font: + text: Avenir Next Condensed + code: Avenir Next Condensed + favicon: assets/favicon.png + logo: assets/logo.png + +markdown_extensions: +- attr_list +- md_in_html +- admonition +- pymdownx.details +- pymdownx.superfences + +nav: +- index.md +- Getting Started: + - usage/overview.md + - Build Environment: + - usage/ros2_pixi_build_linux_windows.md + - usage/usage_with_native_linux.md +- Implementation: + - about/implementation.md +- Contributing: + - contributing/rules.md + - contributing/license.md diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..f8c576b --- /dev/null +++ b/package.xml @@ -0,0 +1,30 @@ + + + + ti_iwr_pointcloud + 3.11.5 + + Package for creating a ROS2 pointcloud demo with the TI IWR6843AOP radar. + + + Hoog-v + Apache-2.0 + + ament_cmake + rclcpp + sensor_msgs + + ament_lint_auto + ament_lint_common + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + + + diff --git a/pixi.toml b/pixi.toml new file mode 100644 index 0000000..e366291 --- /dev/null +++ b/pixi.toml @@ -0,0 +1,35 @@ +[project] +authors = ["example "] +name = "custom_pkg" +channels = ["https://fast.prefix.dev/conda-forge", "https://prefix.dev/robostack-jazzy"] +platforms = ["linux-64", "win-64"] + +[tasks] +build = "colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release" + +[target.linux-64.tasks] +vscode = 'env -u LD_LIBRARY_PATH code .' + +[tasks.start] +cmd = "ros2 run custom_pkg custom_node" +depends-on=["build"] + +[dependencies] +compilers = ">=1.9.0,<2" +pkg-config = ">=0.29.2,<0.30" +ninja = ">=1.12.1,<2" +ros-jazzy-desktop = "*" +colcon-common-extensions = "*" +rosdep = "*" +conda-build = ">=25.1.2,<26" +conda-verify = ">=3.4.2,<4" +rattler-build = "*" +pyserial = "*" +scikit-learn = "*" + +[target.linux-64.dependencies] +python-devtools = "*" # Optional but useful +python = "*" # This is critical for CMake to find Python +pybind11 = "*" # Optional, if you work with bindings +numpy = "*" +lttng-ust = "*" diff --git a/radar/bin/3D_people_track_6843_demo.bin b/radar/bin/3D_people_track_6843_demo.bin new file mode 100644 index 0000000..03dc312 Binary files /dev/null and b/radar/bin/3D_people_track_6843_demo.bin differ diff --git a/radar/cfg/cfg_default_30fps.cfg b/radar/cfg/cfg_default_30fps.cfg new file mode 100644 index 0000000..eba6fbe --- /dev/null +++ b/radar/cfg/cfg_default_30fps.cfg @@ -0,0 +1,53 @@ +% SDK Parameters +% See the SDK user's guide for more information +% "C:\ti\mmwave_sdk_[VER]\docs\mmwave_sdk_user_guide.pdf" +sensorStop +flushCfg +dfeDataOutputMode 1 +channelCfg 15 7 0 +adcCfg 2 1 +adcbufCfg -1 0 1 1 1 +lowPower 0 0 + +% Detection Layer Parameters +% See the Detection Layer Tuning Guide for more information +% "\source\ti\examples\People_Tracking\docs\3D_people_tracking_detection_layer_tuning_guide.pdf" + +profileCfg 0 60.75 10.00 10.00 59.10 0 0 54.71 1 94 3000.00 2 1 36 + +chirpCfg 0 0 0 0 0 0 0 1 +chirpCfg 1 1 0 0 0 0 0 2 +chirpCfg 2 2 0 0 0 0 0 4 +frameCfg 0 2 64 0 50 1 0 + +dynamicRACfarCfg -1 4 1 2 2 8 12 4 8 12.00 19.00 0.40 1 1 +staticRACfarCfg -1 20 20 2 2 8 8 6 4 23.00 31.00 0.60 0 0 + +dynamicRangeAngleCfg -1 0.75 0.0010 1 0 +dynamic2DAngleCfg -1 1.5 0.0300 1 0 1 0.30 0.85 8.00 +staticRangeAngleCfg -1 1 30 10 + +antGeometry0 -1 -1 0 0 -3 -3 -2 -2 -1 -1 0 0 +antGeometry1 -1 0 -1 0 -3 -2 -3 -2 -3 -2 -3 -2 +antPhaseRot 1 -1 1 -1 1 -1 1 -1 1 -1 1 -1 +fovCfg -1 70.0 20.0 +compRangeBiasAndRxChanPhase 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 + +% Tracker Layer Parameters +% See the Tracking Layer Tuning Guide for more information +% "C:\ti\mmwave_industrial_toolbox_[VER]\labs\people_counting\docs\3D_people_counting_tracker_layer_tuning_guide.pdf" +staticBoundaryBox -1.8 1.8 0.2 1.5 0.0 2.0 +boundaryBox -1.8 1.8 0.2 1.5 0.0 2.0 +sensorPosition 0.5 0 0 +gatingParam 4 2 2 2 10 +stateParam 12 12 6 13 5 250 +allocationParam 15 15 0.05 15 0.5 3 +maxAcceleration 0.1 0.1 0.1 +trackingCfg 1 2 400 10 20 260 94 +presenceBoundaryBox -1.8 1.8 0.2 1.5 0.0 2.0 + +% numZones, pointsEntryThreshold, snrEntryThreshold, frameEntryThreshold, pointsMaintainThreshold, snrMaintainThreshold, pointsExitThreshold, frameExitThreshold +occStateMach 1 6 8 3 1 5 0 5 +% ZoneNumber minx maxx miny maxy minz maxz +zoneDef 0 -1.8 1.8 0.2 1.5 -0.5 1.5 +sensorStart \ No newline at end of file diff --git a/rattler/activate.bat b/rattler/activate.bat new file mode 100644 index 0000000..48ff1d7 --- /dev/null +++ b/rattler/activate.bat @@ -0,0 +1,15 @@ +:: Generated by vinca http://github.com/RoboStack/vinca. +:: DO NOT EDIT! +@if not defined CONDA_PREFIX goto:eof + +@REM Don't do anything when we are in conda build. +@if defined SYS_PREFIX exit /b 0 + +@set "QT_PLUGIN_PATH=%CONDA_PREFIX%\Library\plugins" + +@call "%CONDA_PREFIX%\Library\local_setup.bat" +@set PYTHONHOME= +@set "ROS_OS_OVERRIDE=conda:win64" +@set "ROS_ETC_DIR=%CONDA_PREFIX%\Library\etc\ros" +@set "AMENT_PREFIX_PATH=%CONDA_PREFIX%\Library" +@set "AMENT_PYTHON_EXECUTABLE=%CONDA_PREFIX%\python.exe" diff --git a/rattler/activate.sh b/rattler/activate.sh new file mode 100644 index 0000000..4a35f58 --- /dev/null +++ b/rattler/activate.sh @@ -0,0 +1,26 @@ +# Generated by vinca http://github.com/RoboStack/vinca. +# DO NOT EDIT! +# if [ -z "${CONDA_PREFIX}" ]; then +# exit 0; +# fi + +# Not sure if this is necessary on UNIX? +# export QT_PLUGIN_PATH=$CONDA_PREFIX\plugins + +if [ "$CONDA_BUILD" = "1" -a "$target_platform" != "$build_platform" ]; then + # ignore sourcing + echo "Not activating ROS when cross-compiling"; +else + source $CONDA_PREFIX/setup.sh +fi + +case "$OSTYPE" in + darwin*) export ROS_OS_OVERRIDE="conda:osx";; + linux*) export ROS_OS_OVERRIDE="conda:linux";; +esac + +export ROS_ETC_DIR=$CONDA_PREFIX/etc/ros +export AMENT_PREFIX_PATH=$CONDA_PREFIX + +# Looks unnecessary for UNIX +# unset PYTHONHOME= diff --git a/rattler/bld_ament_cmake.bat b/rattler/bld_ament_cmake.bat new file mode 100644 index 0000000..7fad009 --- /dev/null +++ b/rattler/bld_ament_cmake.bat @@ -0,0 +1,39 @@ +:: Generated by vinca http://github.com/RoboStack/vinca. +:: DO NOT EDIT! +setlocal EnableDelayedExpansion + +set "PYTHONPATH=%LIBRARY_PREFIX%\lib\site-packages;%SP_DIR%" + +:: MSVC is preferred. +set CC=cl.exe +set CXX=cl.exe + +rd /s /q build +mkdir build +pushd build + +:: set "CMAKE_GENERATOR=Ninja" +:: We use the Visual Studio generator as a workaround for +:: problems in Ninja when using long paths, see https://github.com/RoboStack/ros-humble/pull/229#issuecomment-2564856467 +:: Once those are solved, we can switch back to use Ninja +set "CMAKE_GENERATOR=Ninja" + +cmake ^ + -G "%CMAKE_GENERATOR%" ^ + -DCMAKE_INSTALL_PREFIX=%LIBRARY_PREFIX% ^ + -DCMAKE_BUILD_TYPE=Release ^ + -DBUILD_SHARED_LIBS=ON ^ + %SRC_DIR% +if errorlevel 1 exit 1 + +:: We explicitly pass %CPU_COUNT% to cmake --build as we are not using Ninja, +:: see the comment before setting the CMAKE_GENERATOR env variable +cmake --build . --config Release --parallel %CPU_COUNT% --target install +if errorlevel 1 exit 1 + +:: Copy the [de]activate scripts to %PREFIX%\etc\conda\[de]activate.d. +:: This will allow them to be run on environment activation. +for %%F in (activate deactivate) DO ( + if not exist %PREFIX%\etc\conda\%%F.d mkdir %PREFIX%\etc\conda\%%F.d + copy %RECIPE_DIR%\%%F.bat %PREFIX%\etc\conda\%%F.d\%PKG_NAME%_%%F.bat +) diff --git a/rattler/build_ament_cmake.sh b/rattler/build_ament_cmake.sh new file mode 100755 index 0000000..3cfa4e8 --- /dev/null +++ b/rattler/build_ament_cmake.sh @@ -0,0 +1,153 @@ +# Generated by vinca http://github.com/RoboStack/vinca. +# DO NOT EDIT! + +set -eo pipefail + +rm -rf build +mkdir build +cd build + +echo "building package $package_name" +# necessary for correctly linking SIP files (from python_qt_bindings) +export LINK=$CXX + +if [[ "$CONDA_BUILD_CROSS_COMPILATION" != "1" ]]; then + PYTHON_EXECUTABLE=$PREFIX/bin/python + PKG_CONFIG_EXECUTABLE=$PREFIX/bin/pkg-config + OSX_DEPLOYMENT_TARGET="10.15" +else + PYTHON_EXECUTABLE=$BUILD_PREFIX/bin/python + PKG_CONFIG_EXECUTABLE=$BUILD_PREFIX/bin/pkg-config + OSX_DEPLOYMENT_TARGET="11.0" +fi + +if [[ "${CONDA_BUILD_CROSS_COMPILATION:-}" == "1" ]]; then + export QT_HOST_PATH="$BUILD_PREFIX" +else + export QT_HOST_PATH="$PREFIX" +fi + +echo "USING PYTHON_EXECUTABLE=${PYTHON_EXECUTABLE}" +echo "USING PKG_CONFIG_EXECUTABLE=${PKG_CONFIG_EXECUTABLE}" + +export ROS_PYTHON_VERSION=`$PYTHON_EXECUTABLE -c "import sys; print('%i.%i' % (sys.version_info[0:2]))"` +echo "Using Python ${ROS_PYTHON_VERSION}" + +# see https://github.com/conda-forge/cross-python-feedstock/issues/24 +if [[ "$CONDA_BUILD_CROSS_COMPILATION" == "1" ]]; then + find $PREFIX/lib/cmake -type f -exec sed -i "s~\${_IMPORT_PREFIX}/lib/python${ROS_PYTHON_VERSION}/site-packages~${BUILD_PREFIX}/lib/python${ROS_PYTHON_VERSION}/site-packages~g" {} + || true + find $PREFIX/share/rosidl* -type f -exec sed -i "s~${PREFIX}/lib/python${ROS_PYTHON_VERSION}/site-packages~${BUILD_PREFIX}/lib/python${ROS_PYTHON_VERSION}/site-packages~g" {} + || true + find $PREFIX/share/rosidl* -type f -exec sed -i "s~\${_IMPORT_PREFIX}/lib/python${ROS_PYTHON_VERSION}/site-packages~${BUILD_PREFIX}/lib/python${ROS_PYTHON_VERSION}/site-packages~g" {} + || true + find $PREFIX/lib/cmake -type f -exec sed -i "s~message(FATAL_ERROR \"The imported target~message(WARNING \"The imported target~g" {} + || true +fi + +if [[ $target_platform =~ linux.* ]]; then + export CFLAGS="${CFLAGS} -D__STDC_FORMAT_MACROS=1" + export CXXFLAGS="${CXXFLAGS} -D__STDC_FORMAT_MACROS=1" +fi; + +# Needed for qt-gui-cpp .. +if [[ $target_platform =~ linux.* ]]; then + ln -s $GCC ${BUILD_PREFIX}/bin/gcc + ln -s $GXX ${BUILD_PREFIX}/bin/g++ +fi; + +# Set SP_DIR manually if not set +if [[ -z "${SP_DIR:-}" ]]; then + SP_DIR="$($PREFIX/bin/python -c 'import site; print(site.getsitepackages()[0])')" + export SP_DIR +fi +# Ensure both SP_DIR and PREFIX are set +if [[ -z "${SP_DIR:-}" ]]; then + echo "Error: SP_DIR is not set" >&2 + exit 1 +fi + +if [[ -z "${PREFIX:-}" ]]; then + echo "Error: PREFIX is not set" >&2 + exit 1 +fi + +# Compute relative path from SP_DIR to PREFIX +export PYTHON_INSTALL_DIR=$(python -c "import os; print(os.path.relpath(os.environ['SP_DIR'], os.environ['PREFIX']))") + +echo "Using PYTHON_INSTALL_DIR: $PYTHON_INSTALL_DIR" +if [[ $target_platform =~ emscripten.* ]]; then + export CONDA_BUILD_CROSS_COMPILATION="1" + PYTHON_EXECUTABLE=$BUILD_PREFIX/bin/python$PY_VER + echo "set_property(GLOBAL PROPERTY TARGET_SUPPORTS_SHARED_LIBS TRUE)"> $SRC_DIR/__vinca_shared_lib_patch.cmake + echo "set(CMAKE_STRIP FALSE) # used by default in pybind11 on .so modules">> $SRC_DIR/__vinca_shared_lib_patch.cmake + echo "set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE BOTH) # fixes an error where numpy header files are not found correctly">> $SRC_DIR/__vinca_shared_lib_patch.cmake + + # if [ "${PKG_NAME}" == "ros-humble-examples-rclcpp-minimal-publisher" ] || [ "${PKG_NAME}" == "ros-humble-examples-rclcpp-minimal-subscriber" ] || [ "${PKG_NAME}" == "ros-humble-rclcpp-components" ]; then + # echo "set(CMAKE_SHARED_LIBRARY_CREATE_C_FLAGS \"-s ASSERTIONS=1 -s SIDE_MODULE=1 -sWASM_BIGINT -s USE_PTHREADS=0 -s DEMANGLE_SUPPORT=1 -s ALLOW_MEMORY_GROWTH=1 \")">> $SRC_DIR/__vinca_shared_lib_patch.cmake + # echo "set(CMAKE_SHARED_LIBRARY_CREATE_CXX_FLAGS \"-s ASSERTIONS=1 -s SIDE_MODULE=1 -sWASM_BIGINT -s USE_PTHREADS=0 -s DEMANGLE_SUPPORT=1 -s ALLOW_MEMORY_GROWTH=1 -sASYNCIFY -O3 -s ASYNCIFY_STACK_SIZE=24576 \")">> $SRC_DIR/__vinca_shared_lib_patch.cmake + # echo "set(CMAKE_EXE_LINKER_FLAGS \"-sMAIN_MODULE=1 -sASSERTIONS=1 -fexceptions -lembind -sWASM_BIGINT -s USE_PTHREADS=0 -s DEMANGLE_SUPPORT=1 -sALLOW_MEMORY_GROWTH=1 -sASYNCIFY -O3 -s ASYNCIFY_STACK_SIZE=24576 -L$SRC_DIR/build -L$PREFIX/lib\") # remove SIDE_MODULE from exe linker flags">> $SRC_DIR/__vinca_shared_lib_patch.cmake + # else + echo "set(CMAKE_SHARED_LIBRARY_CREATE_C_FLAGS \"-s ASSERTIONS=1 -s SIDE_MODULE=1 -sWASM_BIGINT -s USE_PTHREADS=0 -s ALLOW_MEMORY_GROWTH=1 -s DEMANGLE_SUPPORT=1 \")">> $SRC_DIR/__vinca_shared_lib_patch.cmake + echo "set(CMAKE_SHARED_LIBRARY_CREATE_CXX_FLAGS \"-s ASSERTIONS=1 -s SIDE_MODULE=1 -sWASM_BIGINT -s USE_PTHREADS=0 -s ALLOW_MEMORY_GROWTH=1 -s DEMANGLE_SUPPORT=1 \")">> $SRC_DIR/__vinca_shared_lib_patch.cmake + echo "set(CMAKE_EXE_LINKER_FLAGS \"-sMAIN_MODULE=1 -sASSERTIONS=1 -fexceptions -lembind -sWASM_BIGINT -s USE_PTHREADS=0 -sALLOW_MEMORY_GROWTH=1 -s DEMANGLE_SUPPORT=1 -L$SRC_DIR/build -L$PREFIX/lib\") # remove SIDE_MODULE from exe linker flags">> $SRC_DIR/__vinca_shared_lib_patch.cmake + # fi + + export BUILD_TYPE="Debug" + export EXTRA_CMAKE_ARGS=" \ + -DPYTHON_SOABI="cpython-${ROS_PYTHON_VERSION//./}-wasm32-emscripten" \ + -DRMW_IMPLEMENTATION=rmw_wasm_cpp \ + -DCMAKE_FIND_ROOT_PATH=$PREFIX \ + -DCMAKE_POSITION_INDEPENDENT_CODE=TRUE \ + -DCMAKE_PROJECT_INCLUDE=$SRC_DIR/__vinca_shared_lib_patch.cmake \ + -DTRACETOOLS_DISABLED=ON + " + + unset -f cmake + export CMAKE_GEN="emcmake cmake" + export CMAKE_BLD="cmake" + + export STATIC_ROSIDL_TYPESUPPORT_C=rosidl_typesupport_introspection_c + export STATIC_ROSIDL_TYPESUPPORT_CPP=rosidl_typesupport_introspection_cpp +else + export BUILD_TYPE="Release" + export CMAKE_GEN="cmake" + export CMAKE_BLD="cmake" +fi; + +if [ "${PKG_NAME}" == "ros-humble-rmw-wasm-cpp" ]; then + WORK_DIR=$SRC_DIR/$PKG_NAME/src/work/rmw_wasm_cpp +elif [ "${PKG_NAME}" == "ros-humble-wasm-cpp" ]; then + WORK_DIR=$SRC_DIR/$PKG_NAME/src/work/wasm_cpp +elif [ "${PKG_NAME}" == "dynmsg" ]; then + WORK_DIR=$SRC_DIR/$PKG_NAME/src/work/dynmsg +else + WORK_DIR=$SRC_DIR +fi; +export RATTLER_BUILD_SKIP_RELINKING=1 + +export LDFLAGS="-L$PREFIX/lib" +export CFLAGS="-I$PREFIX/include" + +$CMAKE_GEN \ + -G "Ninja" \ + -DCMAKE_BUILD_TYPE=Release\ + -DCMAKE_INSTALL_PREFIX=$PREFIX \ + -DCMAKE_PREFIX_PATH=$PREFIX \ + -DAMENT_PREFIX_PATH=$PREFIX \ + -DCMAKE_INSTALL_LIBDIR=lib \ + -DPYTHON_EXECUTABLE=$PYTHON_EXECUTABLE \ + -DPython_EXECUTABLE=$PYTHON_EXECUTABLE \ + -DPython3_EXECUTABLE=$PYTHON_EXECUTABLE \ + -DPython3_FIND_STRATEGY=LOCATION \ + -DPKG_CONFIG_EXECUTABLE=$PKG_CONFIG_EXECUTABLE \ + -DPYTHON_INSTALL_DIR=$PYTHON_INSTALL_DIR \ + -DSETUPTOOLS_DEB_LAYOUT=OFF \ + -DCATKIN_SKIP_TESTING=$SKIP_TESTING \ + -DCMAKE_INSTALL_SYSTEM_RUNTIME_LIBS_SKIP=True \ + -DBUILD_SHARED_LIBS=ON \ + -DBUILD_TESTING=OFF \ + -DCMAKE_IGNORE_PREFIX_PATH="/opt/homebrew;/usr/local/homebrew" \ + -DCMAKE_OSX_DEPLOYMENT_TARGET=$OSX_DEPLOYMENT_TARGET \ + -DTRACETOOLS_DISABLED=ON \ + --compile-no-warning-as-error \ + $EXTRA_CMAKE_ARGS \ + $WORK_DIR + +$CMAKE_BLD --build . --config $BUILD_TYPE --target install diff --git a/rattler/deactivate.bat b/rattler/deactivate.bat new file mode 100644 index 0000000..6de79cc --- /dev/null +++ b/rattler/deactivate.bat @@ -0,0 +1,18 @@ +:: Generated by vinca http://github.com/RoboStack/vinca. +:: DO NOT EDIT! +@if not defined CONDA_PREFIX goto:eof + +@set ROS_OS_OVERRIDE= +@set ROS_DISTRO= +@set ROS_ETC_DIR= +@set ROS_PACKAGE_PATH= +@set ROS_PYTHON_VERSION= +@set ROS_VERSION= +@set PYTHONHOME= +@set PYTHONPATH= +@set CMAKE_PREFIX_PATH= +@set AMENT_PREFIX_PATH= +@set COLCON_PREFIX_PATH= +@set QT_PLUGIN_PATH= +@set ROS_LOCALHOST_ONLY= +@set ament_python_executable= diff --git a/rattler/deactivate.sh b/rattler/deactivate.sh new file mode 100644 index 0000000..87d4d83 --- /dev/null +++ b/rattler/deactivate.sh @@ -0,0 +1,21 @@ +# Generated by vinca http://github.com/RoboStack/vinca. +# DO NOT EDIT! +if [ -z "${CONDA_PREFIX}" ]; then + exit 0 +fi + +unset ROS_DISTRO +unset ROS_ETC_DIR +unset ROS_PACKAGE_PATH +unset ROS_PYTHON_VERSION +unset CMAKE_PREFIX_PATH +unset AMENT_PREFIX_PATH +unset COLCON_PREFIX_PATH +unset ROS_VERSION +unset ROS_OS_OVERRIDE +# unset PYTHONPATH +# unset PYTHONHOME +# unset QT_PLUGIN_PATH +unset ROS_LOCALHOST_ONLY +unset ament_python_executable +unset RMW_IMPLEMENTATION diff --git a/rattler/recipe.yaml b/rattler/recipe.yaml new file mode 100644 index 0000000..8fda3df --- /dev/null +++ b/rattler/recipe.yaml @@ -0,0 +1,95 @@ +# yaml-language-server: $schema=https://raw.githubusercontent.com/prefix-dev/recipe-format/main/schema.json +context: + name: ros-jazzy-${{ env.get("package_name", default="custom_pkg") }} + version: ${{ env.get("package_ver", default="0.0.0") }} + +package: + name: ${{ name|lower }} + version: ${{ version }} + +source: + path: ../ + use_gitignore: true # note: defaults to true +build: + # settings for shared libraries and executables + dynamic_linking: + # linux only, list of rpaths relative to the installation prefix + rpaths: list of paths (defaults to ['lib/']) + + # Allow runpath / rpath to point to these locations + # outside of the environment + rpath_allowlist: list of globs + + # whether to relocate binaries or not. If this is a list of paths, then + # only the listed paths are relocated + binary_relocation: false + + # Allow linking against libraries that are not in the run requirements + missing_dso_allowlist: list of globs + + # what to do when detecting overdepending + overdepending_behavior: "ignore" + + # what to do when detecting overlinking + overlinking_behavior: "ignore" + number: 0 + script: + - if: win + then: 'bld_ament_cmake.bat' + else: '$RECIPE_DIR/build_ament_cmake.sh' +requirements: + build: + - ninja + - python + - make + - cmake + - ros-jazzy-ros-core + - ros-jazzy-sensor-msgs + - colcon-core + - colcon-ros + - if: unix + then: + - glib + - libcxx + - ros-jazzy-desktop + - gcc + - gxx + - patch + - coreutils + - lttng-ust + - if: win + then: + - msbuild + - make + - setuptools + - m2-patch + - numpy + + host: + - ros-jazzy-ament-cmake + - ros-jazzy-rosidl-default-generators + - ros-jazzy-rclcpp + - ros-jazzy-sensor-msgs + - ros-jazzy-rclcpp + - ros-jazzy-std-msgs + - ros-jazzy-ament-lint-auto + - ros-jazzy-ament-lint-common + run: + - ros-jazzy-rclcpp + - ros-jazzy-sensor-msgs + - ros-jazzy-rosidl-default-runtime + - ros-jazzy-rclcpp + - ros-jazzy-std-msgs + +about: + homepage: ${{ env.get("package_gh", default="https://github.com/CLFML") }} + license: Apache-2.0 + license_file: LICENSE + summary: ${{ env.get("package_summary", default="Some library") }} + description: ${{ env.get("package_desc", default="doing something") }} + documentation: ${{ env.get("package_documentation_link", default="https://github.com/CLFML") }} + repository: ${{ env.get("package_gh", default="https://github.com/CLFML") }} + +extra: + recipe-maintainers: + - ${{ env.get("package_maintainer", default="someone") }} diff --git a/rattler/set_build_var.ps1 b/rattler/set_build_var.ps1 new file mode 100644 index 0000000..879922f --- /dev/null +++ b/rattler/set_build_var.ps1 @@ -0,0 +1,53 @@ +# Get version from latest GitHub release (strip 'v') +$env:package_ver = (gh release view --json tagName | ConvertFrom-Json).tagName -replace '^v', '' + +# Get GitHub repo URL +$env:package_gh = (gh repo view --json url | ConvertFrom-Json).url + +# Convert GitHub repo URL to GitHub Pages URL +$repo_info = gh repo view --json owner,name | ConvertFrom-Json +$repo_owner = $repo_info.owner.login.ToLower() +$repo_name = $repo_info.name +$env:package_documentation_link = "https://${repo_owner}.github.io/${repo_name}" + +# Parse package.xml if available +if (Test-Path "package.xml") { + try { + [xml]$xml = Get-Content package.xml + $ros_package_name = $xml.package.name.Trim() + $env:package_name = $ros_package_name.ToLower() + + $env:package_desc = $xml.package.description.Trim() + $env:package_maintainer_email = $xml.package.maintainer.email + + $maintainer_raw = $xml.package.maintainer.InnerXml + $env:package_maintainer = $xml.package.maintainer.InnerText -replace '\s*<[^>]+>', '' + } catch { + $env:package_name = "unknown" + $env:package_desc = "Custom ROS2 package template" + $env:package_maintainer = "Unknown" + $env:package_maintainer_email = "unknown@example.com" + } +} else { + $env:package_name = "unknown" + $env:package_desc = "Custom ROS2 package template" + $env:package_maintainer = "Unknown" + $env:package_maintainer_email = "unknown@example.com" +} + +# Fallback version if release is not tagged +if (-not $env:package_ver) { + $env:package_ver = "1.0.0" +} + +# Ensure additional expected vars for rattler +$env:package_summary = $env:package_desc +$env:repository = $env:package_gh + +# Print for visibility +Write-Host "package_name = $env:package_name" +Write-Host "version = $env:package_ver" +Write-Host "repo = $env:package_gh" +Write-Host "description = $env:package_desc" +Write-Host "maintainer = $env:package_maintainer" +Write-Host "documentation_link = $env:package_documentation_link" diff --git a/rattler/set_build_var.sh b/rattler/set_build_var.sh new file mode 100644 index 0000000..f43bfe7 --- /dev/null +++ b/rattler/set_build_var.sh @@ -0,0 +1,45 @@ +# Get version from latest GitHub release (strip 'v') +export package_ver="$(gh release view --json tagName --jq .tagName | sed 's/^v//')" + +# Get GitHub repo URL +export package_gh="$(gh repo view --json url --jq .url)" + +# Convert GitHub repo URL to GitHub Pages URL +repo_owner="$(gh repo view --json owner --jq .owner.login | tr '[:upper:]' '[:lower:]')" +repo_name="$(gh repo view --json name --jq .name)" +export package_documentation_link="https://${repo_owner}.github.io/${repo_name}" + +# Parse package.xml if available +if [ -f package.xml ]; then + ros_package_name="$(xmllint --xpath 'normalize-space(/package/name)' package.xml 2>/dev/null || echo 'unknown')" + export package_name="$(echo "$ros_package_name" | tr '[:upper:]' '[:lower:]')" + + export package_desc="$(xmllint --xpath 'normalize-space(/package/description)' package.xml 2>/dev/null || echo 'Custom ROS2 package template')" + export package_maintainer_email="$(xmllint --xpath 'string(/package/maintainer/@email)' package.xml 2>/dev/null || echo 'unknown@example.com')" + + maintainer_raw="$(xmllint --xpath 'normalize-space(/package/maintainer)' package.xml 2>/dev/null || echo 'Unknown')" + export package_maintainer="$(echo "$maintainer_raw" | sed -E 's/ *<[^>]+>//g')" +else + export package_name="unknown" + export package_desc="Custom ROS2 package template" + export package_maintainer="Unknown" + export package_maintainer_email="unknown@example.com" +fi + +# Fallback version if release is not tagged +if [ -z "$package_ver" ]; then + export package_ver="1.0.0" +fi + +# Ensure expected aliases +export package_summary="$package_desc" +export repository="$package_gh" + +# Print for visibility +echo "package_name = $package_name" +echo "version = $package_ver" +echo "repo = $package_gh" +echo "description = $package_desc" +echo "summary = $package_summary" +echo "maintainer = $package_maintainer" +echo "documentation_link = $package_documentation_link" diff --git a/src/packet_parser/packet_parser.cpp b/src/packet_parser/packet_parser.cpp new file mode 100644 index 0000000..3bda916 --- /dev/null +++ b/src/packet_parser/packet_parser.cpp @@ -0,0 +1,36 @@ +#include "packet_parser.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +PacketParser::PacketParser(packet_parser_cfg_t &cfg) + : _running(true), _serial_ports_opened(true), + _serial_cmd(cfg.cli_port, cfg.cli_baudrate), + _serial_data(cfg.data_port, cfg.data_baudrate), _cfg_file(cfg.cfg_path), + _cfg(cfg) + +{ + + if (!std::filesystem::exists(cfg.cfg_path) || !cfg.cfg_path.has_extension() || + cfg.cfg_path.extension() != ".cfg") { + std::cerr << "Invalid config file: " << cfg.cfg_path << '\n'; + return; + } + + configure_radar_with_cfg_file(); + serial_thread_ = std::thread(&PacketParser::serial_loop, this); +} + +PacketParser::~PacketParser() { + _running = false; + if (serial_thread_.joinable()) { + serial_thread_.join(); + } + _serial_cmd.close(); + _serial_data.close(); +} diff --git a/src/packet_parser/packet_parser.hpp b/src/packet_parser/packet_parser.hpp new file mode 100644 index 0000000..10e3e40 --- /dev/null +++ b/src/packet_parser/packet_parser.hpp @@ -0,0 +1,210 @@ +#ifndef PACKET_PARSER_HPP +#define PACKET_PARSER_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +/** + * @brief Represents a single radar detection point. + */ +struct radar_point_t { + float x; /**< X coordinate (meters) */ + float y; /**< Y coordinate (meters) */ + float z; /**< Z coordinate (meters) */ + float snr; /**< Signal-to-noise ratio */ + float noise; /**< Noise level */ + float velocity; /**< Velocity (m/s) */ +}; + +/** + * @brief Represents a complete radar frame with various matrices and point + * clouds. + */ +struct radar_frame_t { + std::vector points; /**< List of detected points */ + std::vector RA_Mat_F; /**< Range-Angle Matrix (Forward) */ + std::vector RA_Mat_R; /**< Range-Angle Matrix (Rear) */ + std::vector RD_Mat; /**< Range-Doppler Matrix */ + std::vector> + target_list; /**< List of tracked targets [target][9 values] */ + std::vector target_cov_matrix; /**< Covariance matrix (4 floats) */ +}; + +/** + * @brief Configuration parameters for initializing the PacketParser. + */ +struct packet_parser_cfg_t { + std::string cli_port; /**< Serial port for CLI communication */ + uint32_t cli_baudrate; /**< CLI port baudrate */ + std::string data_port; /**< Serial port for radar data */ + uint32_t data_baudrate; /**< Data port baudrate */ + std::filesystem::path cfg_path; /**< Path to radar configuration file */ + std::function cbfunc = + nullptr; /**< Callback for parsed radar frames */ +}; + +/** + * @brief Class responsible for reading and parsing radar data packets. + */ +class PacketParser { +public: + /** + * @brief Constructs a PacketParser with the provided configuration. + * @param cfg Configuration struct containing serial port info and callback. + */ + PacketParser(packet_parser_cfg_t &cfg); + + ~PacketParser(); + +private: + std::thread serial_thread_; /**< Thread for reading from serial port */ + std::atomic _running; /**< Flag to control reading loop */ + bool _serial_ports_opened = + false; /**< Indicates if serial ports were successfully opened */ + + serial_cpp::Serial _serial_cmd; + serial_cpp::Serial _serial_data; + + std::filesystem::path _cfg_file; + packet_parser_cfg_t _cfg; + bool _radar_cfg_valid = false; + bool _cfg_mode = false; + + /** + * @brief Radar profile settings as extracted from configuration. + */ + struct radar_profile_settings_t { + float start_freq; /**< Start frequency (GHz) */ + float idle_time; /**< Idle time (us) */ + float adc_start_time; /**< ADC start time (us) */ + float ramp_end_time; /**< Ramp end time (us) */ + float freq_slope_const; /**< Frequency slope constant (MHz/us) */ + int num_adc_samples; /**< Number of ADC samples */ + float dig_out_sample_rate; /**< Digital output sample rate (ksps) */ + } _profile_settings; + + /** + * @brief Frame configuration settings for the radar. + */ + struct radar_frame_settings_t { + int ntx; /**< Number of transmit antennas */ + int num_chirp_loop; /**< Number of chirps per loop */ + int ms_per_frame; /**< Milliseconds per radar frame */ + } _frame_settings; + + /** + * @brief Derived radar session parameters. + */ + struct radar_session_params_t { + double adc_duration; /**< Duration of ADC window */ + double bandwidth; /**< Bandwidth (Hz) */ + double pulse_repetition_interval; /**< Time between chirps (s) */ + int range_fft_size; /**< Size of FFT for range */ + int range_doppler_size; /**< Size of FFT for Doppler */ + double range_resolution; /**< Range resolution (m) */ + double range_max; /**< Maximum detectable range (m) */ + double vel_max; /**< Maximum relative velocity (m/s) */ + double vel_abs_max; /**< Absolute maximum velocity (m/s) */ + double vel_resolution; /**< Velocity resolution (m/s) */ + } _session_params; + + std::vector _buffer; /**< Buffer for incoming serial data */ + size_t _read_offset = 0; /**< Offset in buffer for parsing */ + + static constexpr uint32_t _speed_of_light = + 299792458; /**< Speed of light in m/s */ + static constexpr std::array _magic_word = { + 0x02, 0x01, 0x04, 0x03, + 0x06, 0x05, 0x08, 0x07}; /**< Magic word for packet start */ + + /** + * @brief Applies radar configuration using the provided config file. + * @return 0 on success, non-zero on failure. + */ + int configure_radar_with_cfg_file(); + + /** + * @brief Main loop for reading serial data packets. + */ + void serial_loop(); + + /** + * @brief Processes the current buffer for complete radar packets. + */ + void process_buffer(); + + /** + * @brief Parses a complete radar frame from a data buffer. + * @param buffer Raw data buffer containing a radar frame. + * @return Parsed radar_frame_t object. + */ + radar_frame_t parse_radar_frame(const std::vector &buffer); + + /** + * @brief Finds the magic word in a buffer starting from an offset. + * @param buf The input buffer. + * @param offset Start searching from this offset. + * @return Index of the magic word or std::string::npos if not found. + */ + size_t find_magic_word(const std::vector &buf, size_t offset); + + /** + * @brief Tokenizes a line into a vector of strings. + * @param line Input string line. + * @param minimum_tokens Minimum number of tokens required. + * @return Vector of string tokens. + */ + const std::vector tokenize_line(const std::string &line, + size_t minimum_tokens); + + /** + * @brief Checks if a line contains a given keyword. + * @param line The line to inspect. + * @param keyword The keyword to search for. + * @return True if keyword is found, false otherwise. + */ + bool line_contains_keyword(const std::string &line, + const std::string keyword); + + /** + * @brief Parses and sets profile configuration from a line. + * @param line Configuration line string. + * @return True if successfully parsed, false otherwise. + */ + bool set_profileconfig_from_cfg_line(const std::string &line); + + /** + * @brief Parses and sets frame configuration from a line. + * @param line Configuration line string. + * @return True if successfully parsed, false otherwise. + */ + bool set_frameconfig_from_cfg_line(const std::string &line); + + /** + * @brief Parses detected points TLV from raw data. + * @param data Input binary data of TLV. + * @param frame Output radar frame structure to populate. + */ + void parse_detected_points_tlv(const std::vector &data, + radar_frame_t &frame); + + /** + * @brief Parses side information TLV from raw data. + * @param data Input binary data of TLV. + * @param frame Output radar frame structure to populate. + */ + void parse_side_info_tlv(const std::vector &data, + radar_frame_t &frame); +}; + +#endif // PACKET_PARSER_HPP diff --git a/src/packet_parser/serial_cmd_parser.cpp b/src/packet_parser/serial_cmd_parser.cpp new file mode 100644 index 0000000..18c5c68 --- /dev/null +++ b/src/packet_parser/serial_cmd_parser.cpp @@ -0,0 +1,150 @@ +#include "packet_parser.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +const std::vector +PacketParser::tokenize_line(const std::string &line, + const size_t minimum_tokens) { + std::istringstream iss(line); + std::vector tokens; + std::string token; + + // Tokenize by whitespace + while (iss >> token) { + tokens.push_back(token); + } + // Ensure all required fields are present + if (tokens.size() <= minimum_tokens) { + throw std::runtime_error( + "Incomplete 'profileCfg' line: expected at least " + + std::to_string(minimum_tokens) + " tokens"); + } + + return tokens; +} + +bool PacketParser::line_contains_keyword(const std::string &line, + const std::string keyword) { + size_t pos = line.find(keyword); + if (pos != std::string::npos) { + size_t end_of_word = pos + std::string(keyword).length(); + + // Check for space after the word + if (end_of_word < line.length() && line[end_of_word] == ' ') { + return true; + } + } + return false; +} + +bool PacketParser::set_profileconfig_from_cfg_line(const std::string &line) { + try { + std::vector tokens = tokenize_line(line, 12); + _profile_settings.start_freq = std::stof(tokens[2]); + _profile_settings.idle_time = std::stof(tokens[3]); + _profile_settings.adc_start_time = std::stof(tokens[4]); + _profile_settings.ramp_end_time = std::stof(tokens[5]); + _profile_settings.freq_slope_const = std::stof(tokens[8]); + _profile_settings.num_adc_samples = std::stoi(tokens[10]); + _profile_settings.dig_out_sample_rate = std::stof(tokens[11]); + } catch (const std::exception &e) { + throw std::runtime_error("Failed to parse values from 'profileCfg' line: " + + std::string(e.what())); + return false; + } + return true; +} + +bool PacketParser::set_frameconfig_from_cfg_line(const std::string &line) { + try { + std::vector tokens = tokenize_line(line, 6); + _frame_settings.ntx = std::stof(tokens[2]) - std::stof(tokens[1]) + 1; + _frame_settings.num_chirp_loop = std::stof(tokens[3]); + _frame_settings.ms_per_frame = std::stof(tokens[5]); + std::cout << "ntx: " << _frame_settings.ntx + << " num_chirp_loop: " << _frame_settings.num_chirp_loop + << " ms_per_frame: " << _frame_settings.ms_per_frame << '\n'; + } catch (const std::exception &e) { + throw std::runtime_error("Failed to parse values from 'profileCfg' line: " + + std::string(e.what())); + return false; + } + return true; +} + +int PacketParser::configure_radar_with_cfg_file() { + std::fstream cfg; + cfg.open(_cfg_file, std::ios_base::in); + if (!cfg.is_open()) { + std::cerr << "Failed to open file: " << _cfg_file << std::endl; + return 1; + } + + std::vector ti_config; + std::string line; + + bool valid_frame_config = false; + bool valid_profile_config = false; + _radar_cfg_valid = false; + while (std::getline(cfg, line)) { + // Remove trailing \r and \n + line.erase(std::remove(line.begin(), line.end(), '\r'), line.end()); + line.erase(std::remove(line.begin(), line.end(), '\n'), line.end()); + + if (!line.empty() && line[0] != '%') { + ti_config.push_back(line); + } + + if (line_contains_keyword(line, "profileCfg")) { + valid_profile_config = set_profileconfig_from_cfg_line(line); + } else if (line_contains_keyword(line, "frameCfg")) { + valid_frame_config = set_frameconfig_from_cfg_line(line); + } + } + + // Check if filename (without extension) contains "tracking" + _cfg_mode = _cfg_file.stem().string().find("tracking") != std::string::npos; + if (valid_frame_config && valid_profile_config) { + _radar_cfg_valid = true; + for (const auto &cmd : ti_config) { + _serial_cmd.write(cmd + "\n"); + _serial_cmd.flush(); + std::cout << _serial_cmd.readline() << '\n'; + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + } + } + _session_params.adc_duration = _profile_settings.num_adc_samples / + (_profile_settings.dig_out_sample_rate * 1e3); + _session_params.bandwidth = + _session_params.adc_duration * _profile_settings.freq_slope_const * 1e12; + _session_params.pulse_repetition_interval = + (_profile_settings.idle_time + _profile_settings.ramp_end_time) * 1e-6; + _session_params.range_fft_size = static_cast( + std::pow(2, std::ceil(std::log2(_profile_settings.num_adc_samples)))); + _session_params.range_doppler_size = static_cast( + std::pow(2, std::ceil(std::log2(_frame_settings.num_chirp_loop)))); + _session_params.range_resolution = + _speed_of_light / (2 * _session_params.bandwidth); + _session_params.range_max = + _session_params.range_resolution * _session_params.range_fft_size; + double center_freq_hz = _profile_settings.start_freq * 1e9 + + (_profile_settings.freq_slope_const * 1e12) * + (_profile_settings.adc_start_time * 1e-6 + + _session_params.adc_duration / 2); + + _session_params.vel_max = + _speed_of_light / + (2 * center_freq_hz * _session_params.pulse_repetition_interval) / + _frame_settings.ntx; + + _session_params.vel_abs_max = _session_params.vel_max / 2; + _session_params.vel_resolution = + _session_params.vel_max / _session_params.range_doppler_size; + return 0; +} diff --git a/src/packet_parser/serial_data_parser.cpp b/src/packet_parser/serial_data_parser.cpp new file mode 100644 index 0000000..b82a503 --- /dev/null +++ b/src/packet_parser/serial_data_parser.cpp @@ -0,0 +1,86 @@ +#include "packet_parser.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +void PacketParser::serial_loop() { + constexpr size_t kReadSize = 2048; + + while (_running) { + std::vector chunk; + size_t bytes_read = _serial_data.read(chunk, kReadSize); + if (bytes_read == 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; + } + _buffer.insert(_buffer.end(), chunk.begin(), chunk.begin() + bytes_read); + + process_buffer(); + } +} + +size_t PacketParser::find_magic_word(const std::vector &buf, + size_t offset) { + if (buf.size() < offset + _magic_word.size()) + return std::string::npos; + + auto it = std::search(buf.begin() + offset, buf.end(), _magic_word.begin(), + _magic_word.end()); + + return (it != buf.end()) ? std::distance(buf.begin(), it) : std::string::npos; +} + +void PacketParser::process_buffer() { + while (true) { + size_t start = find_magic_word(_buffer, _read_offset); + if (start == std::string::npos) { + // Too much stale data? Clean up + if (_buffer.size() > 4096) { + _buffer.erase(_buffer.begin(), _buffer.end() - _magic_word.size()); + } + _read_offset = 0; + return; + } + + // Minimum valid header size + if (_buffer.size() < start + 36) + return; + + uint32_t packet_length = 0; + std::memcpy(&packet_length, &_buffer[start + 12], sizeof(uint32_t)); + + if (packet_length < 36 || packet_length > 65536) { + std::cerr << "Suspicious packet length: " << packet_length + << ", skipping magic.\n"; + _read_offset = start + _magic_word.size(); + continue; + } + + if (_buffer.size() < start + packet_length) + return; + + std::vector frame(_buffer.begin() + start, + _buffer.begin() + start + packet_length); + _read_offset = start + packet_length; + + try { + radar_frame_t parsed = parse_radar_frame(frame); + _cfg.cbfunc(parsed); + } catch (const std::exception &e) { + std::cerr << "Frame parse error: " << e.what() << '\n'; + } + + // Compact buffer + if (_read_offset > 1024 || _read_offset > _buffer.size() / 2) { + size_t remaining = _buffer.size() - _read_offset; + std::memmove(_buffer.data(), _buffer.data() + _read_offset, remaining); + _buffer.resize(remaining); + _read_offset = 0; + } + } +} diff --git a/src/packet_parser/tlv_parser.cpp b/src/packet_parser/tlv_parser.cpp new file mode 100644 index 0000000..0a97568 --- /dev/null +++ b/src/packet_parser/tlv_parser.cpp @@ -0,0 +1,114 @@ +#include "packet_parser.hpp" +#include + +template +T unpack(const std::vector &buffer, size_t &offset) { + if (offset + sizeof(T) > buffer.size()) + throw std::runtime_error("Buffer overrun"); + T value; + std::memcpy(&value, &buffer[offset], sizeof(T)); + offset += sizeof(T); + return value; +} + +void PacketParser::parse_detected_points_tlv(const std::vector &data, + radar_frame_t &frame) { + size_t offset = 0; + size_t point_struct_size = 4 * sizeof(float); // x, y, z, velocity + size_t num_points = data.size() / point_struct_size; + + for (size_t i = 0; i < num_points; ++i) { + radar_point_t pt; + pt.x = unpack(data, offset); + pt.y = unpack(data, offset); + pt.z = unpack(data, offset); + pt.velocity = unpack(data, offset); + frame.points.push_back(pt); + } +} + +void PacketParser::parse_side_info_tlv(const std::vector &data, + radar_frame_t &frame) { + size_t offset = 0; + size_t num_points = frame.points.size(); + + for (size_t i = 0; + i < num_points && offset + sizeof(uint16_t) * 2 <= data.size(); ++i) { + uint16_t snr = unpack(data, offset); + uint16_t noise = unpack(data, offset); + frame.points[i].snr = static_cast(snr) / 10.0f; // SNR in dB + frame.points[i].noise = static_cast(noise) / 10.0f; // SNR in dB + } +} + +radar_frame_t +PacketParser::parse_radar_frame(const std::vector &buffer) { + radar_frame_t frame; + + if (buffer.size() < 44) { + std::cerr << "Frame too small to be valid\n"; + return frame; + } + + size_t offset = 8; // Skip magic word + +#if DEBUG + uint32_t version = unpack(buffer, offset); + uint32_t packet_length = unpack(buffer, offset); + uint32_t platform = unpack(buffer, offset); + uint32_t frame_number = unpack(buffer, offset); + uint32_t cpu_cycles = unpack(buffer, offset); + uint32_t num_detected_objs = unpack(buffer, offset); + uint32_t num_tlv = unpack(buffer, offset); + uint32_t subframe_idx = unpack(buffer, offset); + + std::cout << "Header Info -- " + << "Version: " << version << ", Packet Length: " << packet_length + << ", Platform: " << platform << ", Frame #: " << frame_number + << ", CPU Cycles: " << cpu_cycles + << ", Detected objs: " << num_detected_objs + << ", Num tlv: " << num_tlv << ", Subframe idx: " << subframe_idx + << '\n'; +#else + offset += 24; + uint32_t num_tlv = unpack(buffer, offset); + offset += 4; +#endif + for (uint32_t i = 0; i < num_tlv; ++i) { + if (offset + 8 > buffer.size()) { + std::cerr << "Incomplete TLV header at TLV " << i << '\n'; + break; + } + // Read TLV type and length + uint32_t tlv_type = unpack(buffer, offset); + uint32_t tlv_length = unpack(buffer, offset); + +#if DEBUG + std::cout << "TLV #" << i << " | Type: " << tlv_type + << ", Length: " << tlv_length << '\n'; +#endif + // Check if there's enough data for the value + if (offset + (tlv_length) > buffer.size()) { + std::cerr << "TLV value extends beyond buffer size!\n"; + break; + } + + std::vector tlv_data(buffer.begin() + offset, + buffer.begin() + offset + (tlv_length)); + + switch (tlv_type) { + case 1020: + parse_detected_points_tlv(tlv_data, frame); + break; + case 1021: + parse_side_info_tlv(tlv_data, frame); + break; + default: + std::cerr << "Unknown TLV type: " << tlv_type << '\n'; + break; + } + offset += tlv_length; + } + + return frame; +} \ No newline at end of file diff --git a/src/publish_node.cpp b/src/publish_node.cpp new file mode 100644 index 0000000..d462615 --- /dev/null +++ b/src/publish_node.cpp @@ -0,0 +1,127 @@ +#include "publish_node.hpp" + +PublishNode::PublishNode() : Node("publish_node") { + RCLCPP_INFO(this->get_logger(), "Publish node has been started."); + + _running = true; + + // Declare parameters with default values + this->declare_parameter("cfg_path", + "../radar/cfg/cfg_default_30fps.cfg"); + this->declare_parameter("cli_port", "/dev/pts/2"); + this->declare_parameter("cli_baudrate", 115200); + this->declare_parameter("data_port", "/dev/pts/6"); + this->declare_parameter("data_baudrate", 921600); + this->declare_parameter("publish_topic", "/radar_3d_points"); + + // Retrieve parameter values + packet_parser_cfg_t cfg; + this->get_parameter("cfg_path", cfg.cfg_path); + this->get_parameter("cli_port", cfg.cli_port); + this->get_parameter("cli_baudrate", cfg.cli_baudrate); + this->get_parameter("data_port", cfg.data_port); + this->get_parameter("data_baudrate", cfg.data_baudrate); + cfg.cbfunc = + std::bind(&PublishNode::radar_frame_cb, this, std::placeholders::_1); + + std::string publish_topic; + this->get_parameter("publish_topic", publish_topic); + + try { + _packet_parser = std::make_unique(cfg); + } catch (const std::exception &e) { + std::cerr << "[PublishNode]: ERROR! Cannot open serial ports: " << e.what() + << '\n'; + exit(1); + } + + radar_publisher_ = + this->create_publisher(publish_topic, 10); + + _processing_thread = std::thread([this]() { + while (_running) { + std::unique_lock lock(_mutex); + _cv.wait(lock, [this] { + std::lock_guard qlock(queue_mutex); + return !_running || !frame_queue.empty(); + }); + + if (!_running) + break; + + radar_frame_t frame; + { + std::lock_guard qlock(queue_mutex); + if (!frame_queue.empty()) { + frame = frame_queue.front(); + frame_queue.pop(); + } else { + continue; + } + } + + lock.unlock(); + publish_pointcloud(frame); + } + }); +} + +PublishNode::~PublishNode() { + _running = false; + _cv.notify_one(); + if (_processing_thread.joinable()) { + _processing_thread.join(); + } +} + +void PublishNode::radar_frame_cb(const radar_frame_t &radar_points) { + { + std::lock_guard lock(queue_mutex); + frame_queue.push(radar_points); + } + _cv.notify_one(); +} + +void PublishNode::publish_pointcloud(const radar_frame_t &radar_points) { + sensor_msgs::msg::PointCloud2 cloud_msg; + cloud_msg.header.stamp = this->get_clock()->now(); + cloud_msg.header.frame_id = "radar_frame"; + + cloud_msg.height = 1; + cloud_msg.width = radar_points.points.size(); + cloud_msg.is_dense = false; + cloud_msg.is_bigendian = false; + + sensor_msgs::PointCloud2Modifier modifier(cloud_msg); + modifier.setPointCloud2Fields(4, // number of fields + "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "intensity", 1, + sensor_msgs::msg::PointField::FLOAT32); + + sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_intensity(cloud_msg, + "intensity"); + + for (const auto &point : radar_points.points) { + *iter_x = point.x; + *iter_y = point.y; + *iter_z = point.z; + *iter_intensity = point.snr; // or use point.intensity if you rename + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_intensity; + } + radar_publisher_->publish(cloud_msg); +} + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/publish_node.hpp b/src/publish_node.hpp new file mode 100644 index 0000000..d751837 --- /dev/null +++ b/src/publish_node.hpp @@ -0,0 +1,30 @@ +#ifndef PUBLISH_NODE_HPP +#define PUBLISH_NODE_HPP + +#include +#include +#include +#include + +class PublishNode : public rclcpp::Node { +public: + PublishNode(); + ~PublishNode(); + +private: + void radar_frame_cb(const radar_frame_t &radar_points); + void publish_pointcloud(const radar_frame_t &radar_points); + + std::unique_ptr _packet_parser; + std::thread _processing_thread; + bool _running = true; + + std::queue frame_queue; + std::mutex queue_mutex; + std::mutex _mutex; + std::condition_variable _cv; + + rclcpp::Publisher::SharedPtr radar_publisher_; +}; + +#endif /* PUBLISH_NODE_HPP */ \ No newline at end of file diff --git a/tests/manual_tests/README.md b/tests/manual_tests/README.md new file mode 100644 index 0000000..765bff2 --- /dev/null +++ b/tests/manual_tests/README.md @@ -0,0 +1,49 @@ +# Manual testing + +To manually test the node and it's parsing library while developing. There are some small convenient scripts which make a virtual serial port, to which the library can be hooked up. This provides an easy way to validate the TLV parsing, without having to hookup the Radar EVM to your pc. + +**This guide is Linux only!** + +## What to do first? +Create a virtual serial port by running: +```bash +socat -d -d pty,raw,echo=0 pty,raw,echo=0 +``` +The output looks something like this: +```bash +$ socat -d -d pty,raw,echo=0 pty,raw,echo=0 +2025/04/06 21:40:15 socat[130785] N PTY is /dev/pts/6 +2025/04/06 21:40:15 socat[130785] N PTY is /dev/pts/13 +2025/04/06 21:40:15 socat[130785] N starting data transfer loop with FDs [5,5] and [7,7] +``` +Keep an eye out for the pts/6 and pts/13, you will need them later. As these are two virtual ports which are linked to each other, meaning one-end will be connected to the app. And one to your shell script. + +**The process is blocking, thus you need to keep socat running on the background.** + +**The radar has two serial ports, instantiate this socat thing in two seperate terminals.** +```bash +$ socat -d -d pty,raw,echo=0 pty,raw,echo=0 +2025/04/06 21:45:11 socat[132988] N PTY is /dev/pts/2 +2025/04/06 21:45:11 socat[132988] N PTY is /dev/pts/14 +2025/04/06 21:45:11 socat[132988] N starting data transfer loop with FDs [5,5] and [7,7] +``` + +Okay you're almost set; Now adjust the ports in the script: + +``` +CMD_WRITE="/dev/pts/13" +DATA_WRITE="/dev/pts/14" +``` + +## How to use the mock_radar.sh script + +Well simple after you set above section up for your machine. Just run: +```bash +sh ./mock_radar.sh +``` +Which will say something like this: +```bash +$ sh ./mock_radar.sh +[Listening on /dev/pts/13 for 'sensorStart'] +``` +Great! Now run the program node with the parameters: diff --git a/tests/manual_tests/mock_radar.sh b/tests/manual_tests/mock_radar.sh new file mode 100644 index 0000000..927fbe9 --- /dev/null +++ b/tests/manual_tests/mock_radar.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +CMD_WRITE="/dev/pts/5" +DATA_WRITE="/dev/pts/8" +BIN_FILE="serial_dump.bin" + +echo "[Listening on $CMD_WRITE for 'sensorStart']" + +# Open ports +exec 3<> "$CMD_WRITE" +exec 4> "$DATA_WRITE" + +sensor_started=0 + +while true; do + if read -r -t 1 line <&3; then + echo "[RX] $line" + + if [[ "$line" == *"sensorStart"* ]]; then + echo "[!] Got sensorStart. Replaying binary to $DATA_WRITE" + pv -L 92160 "$BIN_FILE" >&4 & + sensor_started=1 + fi + + echo -ne "Done\n" >&3 + fi +done diff --git a/tests/manual_tests/serial_dump.bin b/tests/manual_tests/serial_dump.bin new file mode 100644 index 0000000..d27387a Binary files /dev/null and b/tests/manual_tests/serial_dump.bin differ diff --git a/tests/some_test_code.cpp b/tests/some_test_code.cpp new file mode 100644 index 0000000..f6df654 --- /dev/null +++ b/tests/some_test_code.cpp @@ -0,0 +1,7 @@ +#include + +int main(int argc, char *argv[]) { + while (1) { + + } +} \ No newline at end of file